packages feed

Box2D (empty) → 0.1.0.0

raw patch · 126 files changed

+63559/−0 lines, 126 filesdep +Box2Ddep +basedep +tasty

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

Files

+ Box2D.cabal view
@@ -0,0 +1,237 @@+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:           Box2D+version:        0.1.0.0+synopsis:       Low-level bindings to the Box2D physics engine+description:    Low-level FFI bindings to Box2D, Erin Catto's 2D rigid-body physics engine.+                The engine's C sources (v3.2.0, 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 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/box2d_gen_shim.h+    cbits/box2d_shim.h+    upstream/include/box2d/base.h+    upstream/include/box2d/box2d.h+    upstream/include/box2d/collision.h+    upstream/include/box2d/config.h+    upstream/include/box2d/constants.h+    upstream/include/box2d/id.h+    upstream/include/box2d/math_functions.h+    upstream/include/box2d/math_types.h+    upstream/include/box2d/types.h+    upstream/src/aabb.h+    upstream/src/arena_allocator.h+    upstream/src/bitset.h+    upstream/src/body.h+    upstream/src/broad_phase.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/dynamic_tree.h+    upstream/src/id_pool.h+    upstream/src/island.h+    upstream/src/joint.h+    upstream/src/parallel_for.h+    upstream/src/physics_world.h+    upstream/src/platform.h+    upstream/src/recording.h+    upstream/src/replay.h+    upstream/src/scheduler.h+    upstream/src/sensor.h+    upstream/src/shape.h+    upstream/src/simd.h+    upstream/src/snapshot.h+    upstream/src/solver.h+    upstream/src/solver_set.h+    upstream/src/table.h+    upstream/src/contact_solver_wide.inl+    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:+      Box2D.Base+      Box2D.Body+      Box2D.Callbacks+      Box2D.Chain+      Box2D.Collision+      Box2D.Contact+      Box2D.DebugDraw+      Box2D.Distance+      Box2D.DistanceJoint+      Box2D.DynamicTree+      Box2D.Events+      Box2D.Extra+      Box2D.FilterJoint+      Box2D.Geometry+      Box2D.Id+      Box2D.Internal+      Box2D.Internal.DrawClosure+      Box2D.Joint+      Box2D.MathFunctions+      Box2D.MathTypes+      Box2D.MotorJoint+      Box2D.Mover+      Box2D.MoverJoint+      Box2D.PogoJoint+      Box2D.PrismaticJoint+      Box2D.Recording+      Box2D.Replay+      Box2D.RevoluteJoint+      Box2D.Shape+      Box2D.Tags+      Box2D.UserData+      Box2D.WeldJoint+      Box2D.WheelJoint+      Box2D.World+  other-modules:+      Paths_Box2D+  autogen-modules:+      Paths_Box2D+  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/box2d_shim.c+      cbits/box2d_gen_shim.c+      upstream/src/aabb.c+      upstream/src/arena_allocator.c+      upstream/src/bitset.c+      upstream/src/body.c+      upstream/src/broad_phase.c+      upstream/src/constraint_graph.c+      upstream/src/contact.c+      upstream/src/contact_solver.c+      upstream/src/contact_solver_w4.c+      upstream/src/contact_solver_w8.c+      upstream/src/core.c+      upstream/src/distance.c+      upstream/src/distance_joint.c+      upstream/src/dynamic_tree.c+      upstream/src/geometry.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/motor_joint.c+      upstream/src/mover.c+      upstream/src/mover_joint.c+      upstream/src/parallel_for.c+      upstream/src/physics_world.c+      upstream/src/pogo_joint.c+      upstream/src/prismatic_joint.c+      upstream/src/recording.c+      upstream/src/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/snapshot.c+      upstream/src/solver.c+      upstream/src/solver_set.c+      upstream/src/table.c+      upstream/src/timer.c+      upstream/src/types.c+      upstream/src/weld_joint.c+      upstream/src/wheel_joint.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: -DBOX2D_DOUBLE_PRECISION++test-suite Box2D-test+  type: exitcode-stdio-1.0+  main-is: Spec.hs+  other-modules:+      Paths_Box2D+  autogen-modules:+      Paths_Box2D+  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:+      Box2D+    , base >=4.16 && <5+    , tasty+    , tasty-hunit+    , vector ==0.13.*+  default-language: GHC2021++benchmark Box2D-bench+  type: exitcode-stdio-1.0+  main-is: Bench.hs+  other-modules:+      Paths_Box2D+  autogen-modules:+      Paths_Box2D+  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:+      Box2D+    , base >=4.16 && <5+    , tasty-bench+    , vector ==0.13.*+  default-language: GHC2021
+ CHANGELOG.md view
@@ -0,0 +1,11 @@+# Changelog for `Box2D`++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 Box2D `v3.2.0` (upstream commit `178495b`).
+ 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,110 @@+# Box2D++Low-level Haskell bindings to [Box2D](https://box2d.org/), Erin Catto's 2D+rigid-body physics engine.++The engine's C sources (v3.2.0, MIT licensed) are bundled and compiled with the+package. You need a C compiler, but no system library.++The bindings stay close to the C API: `b2World_Step` is `Box2D.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 Box2D.Body (BodyDef (..), defaultBodyDef, pattern DynamicBody)+import Box2D.Body qualified as Body+import Box2D.Geometry (Circle (..))+import Box2D.MathTypes (Pos (..), Vec2 (..), vec2Zero)+import Box2D.Shape (defaultShapeDef)+import Box2D.Shape qualified as Shape+import Box2D.World (WorldDef (..), defaultWorldDef)+import Box2D.World qualified as World++main :: IO ()+main = do+  worldDef <- defaultWorldDef+  world <- World.create worldDef{gravity = Vec2 0 (-10)}++  bodyDef <- defaultBodyDef+  ball <- Body.create world bodyDef{type_ = DynamicBody, position = Pos 0 10}+  shapeDef <- defaultShapeDef+  _ <- Shape.createCircle ball shapeDef (Circle vec2Zero 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`, `Chain`, `Contact`, `Events` |+| Joints | `Joint`, `DistanceJoint`, `FilterJoint`, `MotorJoint`, `MoverJoint`, `PogoJoint`, `PrismaticJoint`, `RevoluteJoint`, `WeldJoint`, `WheelJoint` |+| Geometry and queries | `Geometry`, `Collision`, `Distance`, `DynamicTree`, `Mover` |+| Callbacks and drawing | `Callbacks`, `DebugDraw` |+| Recording | `Recording`, `Replay` |+| Foundation | `Id`, `MathTypes`, `MathFunctions`, `Base`, `UserData`, `Extra`, `Tags` |++`Box2D.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 `Vec2`. `MathTypes.toPos`, `toVec2`, `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 `Box2D.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`. An exception escaping any other callback stops the process,+  because it cannot unwind through C frames.+- **Events are copied.** `Box2D.Events` copies each event buffer into a vector+  that is safe to keep. The raw buffers from `Box2D.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.setAssertCallback` ever fire. Turn+it on while developing, for example with `stack build --flag Box2D: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 Box2D` 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 Box2D.Body (BodyDef (..), defaultBodyDef, pattern DynamicBody)+import Box2D.Body qualified as Body+import Box2D.Geometry (Circle (..), Segment (..))+import Box2D.Id (WorldId)+import Box2D.MathTypes+import Box2D.Shape (defaultShapeDef)+import Box2D.Shape qualified as Shape+import Box2D.World (WorldDef (..), defaultWorldDef)+import Box2D.World qualified as World++{- | Build a world holding @n@ dynamic circles dropped onto a static ground+segment, 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 = Vec2 0 (-10), enableSleep = 0}++  gd <- defaultBodyDef+  ground <- Body.create w gd+  gsd <- defaultShapeDef+  _ <- Shape.createSegment ground gsd (Segment (Vec2 (-40) 0) (Vec2 40 0))++  bd <- defaultBodyDef+  sd <- defaultShapeDef+  forM_ [0 .. n - 1] $ \i -> do+    let+      col = fromIntegral (i `mod` 16)+      row = fromIntegral (i `div` 16)+      position = Pos (col - 8) (2 + row)+    b <- Body.create w bd{type_ = DynamicBody, position = position}+    _ <- Shape.createCircle b sd (Circle vec2Zero 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 circles" $+        whnfIO (World.step world64 (1 / 60) 4)+    , bench "step 256 circles" $+        whnfIO (World.step world256 (1 / 60) 4)+    ]
+ cbits/box2d_gen_shim.c view
@@ -0,0 +1,650 @@+// SPDX-License-Identifier: BSD-3-Clause+//+// Generated by boxnd-gen. See box2d_gen_shim.h.++#include "box2d_gen_shim.h"++void hsg_b2World_GetBounds(b2WorldId a0, b2AABB* out)+{+	*out = b2World_GetBounds(a0);+}++void hsg_b2World_GetBodyEvents(b2WorldId a0, b2BodyEvents* out)+{+	*out = b2World_GetBodyEvents(a0);+}++void hsg_b2World_GetSensorEvents(b2WorldId a0, b2SensorEvents* out)+{+	*out = b2World_GetSensorEvents(a0);+}++void hsg_b2World_GetContactEvents(b2WorldId a0, b2ContactEvents* out)+{+	*out = b2World_GetContactEvents(a0);+}++void hsg_b2World_GetJointEvents(b2WorldId a0, b2JointEvents* out)+{+	*out = b2World_GetJointEvents(a0);+}++void hsg_b2World_OverlapAABB(b2WorldId a0, const b2Pos* a1, const b2AABB* a2, const b2QueryFilter* a3, b2OverlapResultCallbackFcn* a4, void* a5, b2TreeStats* out)+{+	*out = b2World_OverlapAABB(a0, *a1, *a2, *a3, a4, a5);+}++void hsg_b2World_OverlapShape(b2WorldId a0, const b2Pos* a1, const b2ShapeProxy* a2, const b2QueryFilter* a3, b2OverlapResultCallbackFcn* a4, void* a5, b2TreeStats* out)+{+	*out = b2World_OverlapShape(a0, *a1, a2, *a3, a4, a5);+}++void hsg_b2World_CastRay(b2WorldId a0, const b2Pos* a1, const b2Vec2* a2, const b2QueryFilter* a3, b2CastResultCallbackFcn* a4, void* a5, b2TreeStats* out)+{+	*out = b2World_CastRay(a0, *a1, *a2, *a3, a4, a5);+}++void hsg_b2World_CastRayClosest(b2WorldId a0, const b2Pos* a1, const b2Vec2* a2, const b2QueryFilter* a3, b2RayResult* out)+{+	*out = b2World_CastRayClosest(a0, *a1, *a2, *a3);+}++void hsg_b2World_CastShape(b2WorldId a0, const b2Pos* a1, const b2ShapeProxy* a2, const b2Vec2* a3, const b2QueryFilter* a4, b2CastResultCallbackFcn* a5, void* a6, b2TreeStats* out)+{+	*out = b2World_CastShape(a0, *a1, a2, *a3, *a4, a5, a6);+}++float hsg_b2World_CastMover(b2WorldId a0, const b2Pos* a1, const b2Capsule* a2, const b2Vec2* a3, const b2QueryFilter* a4)+{+	return b2World_CastMover(a0, *a1, a2, *a3, *a4);+}++void hsg_b2World_CollideMover(b2WorldId a0, const b2Pos* a1, const b2Capsule* a2, const b2QueryFilter* a3, b2PlaneResultCallbackFcn* a4, void* a5)+{+	b2World_CollideMover(a0, *a1, a2, *a3, a4, a5);+}++void hsg_b2World_SetGravity(b2WorldId a0, const b2Vec2* a1)+{+	b2World_SetGravity(a0, *a1);+}++void hsg_b2World_GetGravity(b2WorldId a0, b2Vec2* out)+{+	*out = b2World_GetGravity(a0);+}++void hsg_b2World_GetProfile(b2WorldId a0, b2Profile* out)+{+	*out = b2World_GetProfile(a0);+}++void hsg_b2World_GetCounters(b2WorldId a0, b2Counters* out)+{+	*out = b2World_GetCounters(a0);+}++void hsg_b2World_GetMaxCapacity(b2WorldId a0, b2Capacity* out)+{+	*out = b2World_GetMaxCapacity(a0);+}++void hsg_b2Body_GetPosition(b2BodyId a0, b2Pos* out)+{+	*out = b2Body_GetPosition(a0);+}++void hsg_b2Body_GetRotation(b2BodyId a0, b2Rot* out)+{+	*out = b2Body_GetRotation(a0);+}++void hsg_b2Body_GetTransform(b2BodyId a0, b2WorldTransform* out)+{+	*out = b2Body_GetTransform(a0);+}++void hsg_b2Body_SetTransform(b2BodyId a0, const b2Pos* a1, const b2Rot* a2)+{+	b2Body_SetTransform(a0, *a1, *a2);+}++void hsg_b2Body_GetLocalPoint(b2BodyId a0, const b2Pos* a1, b2Vec2* out)+{+	*out = b2Body_GetLocalPoint(a0, *a1);+}++void hsg_b2Body_GetWorldPoint(b2BodyId a0, const b2Vec2* a1, b2Pos* out)+{+	*out = b2Body_GetWorldPoint(a0, *a1);+}++void hsg_b2Body_GetLocalVector(b2BodyId a0, const b2Vec2* a1, b2Vec2* out)+{+	*out = b2Body_GetLocalVector(a0, *a1);+}++void hsg_b2Body_GetWorldVector(b2BodyId a0, const b2Vec2* a1, b2Vec2* out)+{+	*out = b2Body_GetWorldVector(a0, *a1);+}++void hsg_b2Body_GetLinearVelocity(b2BodyId a0, b2Vec2* out)+{+	*out = b2Body_GetLinearVelocity(a0);+}++void hsg_b2Body_SetLinearVelocity(b2BodyId a0, const b2Vec2* a1)+{+	b2Body_SetLinearVelocity(a0, *a1);+}++void hsg_b2Body_SetTargetTransform(b2BodyId a0, const b2WorldTransform* a1, float a2, bool a3)+{+	b2Body_SetTargetTransform(a0, *a1, a2, a3);+}++void hsg_b2Body_GetLocalPointVelocity(b2BodyId a0, const b2Vec2* a1, b2Vec2* out)+{+	*out = b2Body_GetLocalPointVelocity(a0, *a1);+}++void hsg_b2Body_GetWorldPointVelocity(b2BodyId a0, const b2Pos* a1, b2Vec2* out)+{+	*out = b2Body_GetWorldPointVelocity(a0, *a1);+}++void hsg_b2Body_ApplyForce(b2BodyId a0, const b2Vec2* a1, const b2Pos* a2, bool a3)+{+	b2Body_ApplyForce(a0, *a1, *a2, a3);+}++void hsg_b2Body_ApplyForceToCenter(b2BodyId a0, const b2Vec2* a1, bool a2)+{+	b2Body_ApplyForceToCenter(a0, *a1, a2);+}++void hsg_b2Body_ApplyLinearImpulse(b2BodyId a0, const b2Vec2* a1, const b2Pos* a2, bool a3)+{+	b2Body_ApplyLinearImpulse(a0, *a1, *a2, a3);+}++void hsg_b2Body_ApplyLinearImpulseToCenter(b2BodyId a0, const b2Vec2* a1, bool a2)+{+	b2Body_ApplyLinearImpulseToCenter(a0, *a1, a2);+}++void hsg_b2Body_GetLocalCenter(b2BodyId a0, b2Vec2* out)+{+	*out = b2Body_GetLocalCenter(a0);+}++void hsg_b2Body_GetWorldCenter(b2BodyId a0, b2Pos* out)+{+	*out = b2Body_GetWorldCenter(a0);+}++void hsg_b2Body_SetMassData(b2BodyId a0, const b2MassData* a1)+{+	b2Body_SetMassData(a0, *a1);+}++void hsg_b2Body_GetMassData(b2BodyId a0, b2MassData* out)+{+	*out = b2Body_GetMassData(a0);+}++void hsg_b2Body_SetMotionLocks(b2BodyId a0, const b2MotionLocks* a1)+{+	b2Body_SetMotionLocks(a0, *a1);+}++void hsg_b2Body_GetMotionLocks(b2BodyId a0, b2MotionLocks* out)+{+	*out = b2Body_GetMotionLocks(a0);+}++void hsg_b2Body_ComputeBounds(b2BodyId a0, b2AABB* out)+{+	*out = b2Body_ComputeBounds(a0);+}++void hsg_b2Shape_GetSurfaceMaterial(b2ShapeId a0, b2SurfaceMaterial* out)+{+	*out = b2Shape_GetSurfaceMaterial(a0);+}++void hsg_b2Shape_GetFilter(b2ShapeId a0, b2Filter* out)+{+	*out = b2Shape_GetFilter(a0);+}++void hsg_b2Shape_SetFilter(b2ShapeId a0, const b2Filter* a1)+{+	b2Shape_SetFilter(a0, *a1);+}++bool hsg_b2Shape_TestPoint(b2ShapeId a0, const b2Pos* a1)+{+	return b2Shape_TestPoint(a0, *a1);+}++void hsg_b2Shape_CastRay(b2ShapeId a0, const b2Pos* a1, const b2Vec2* a2, b2WorldCastOutput* out)+{+	*out = b2Shape_CastRay(a0, *a1, *a2);+}++void hsg_b2Shape_GetCircle(b2ShapeId a0, b2Circle* out)+{+	*out = b2Shape_GetCircle(a0);+}++void hsg_b2Shape_GetSegment(b2ShapeId a0, b2Segment* out)+{+	*out = b2Shape_GetSegment(a0);+}++void hsg_b2Shape_GetChainSegment(b2ShapeId a0, b2ChainSegment* out)+{+	*out = b2Shape_GetChainSegment(a0);+}++void hsg_b2Shape_GetCapsule(b2ShapeId a0, b2Capsule* out)+{+	*out = b2Shape_GetCapsule(a0);+}++void hsg_b2Shape_GetPolygon(b2ShapeId a0, b2Polygon* out)+{+	*out = b2Shape_GetPolygon(a0);+}++void hsg_b2Shape_GetBounds(b2ShapeId a0, b2AABB* out)+{+	*out = b2Shape_GetBounds(a0);+}++void hsg_b2Shape_ComputeMassData(b2ShapeId a0, b2MassData* out)+{+	*out = b2Shape_ComputeMassData(a0);+}++void hsg_b2Shape_GetClosestPoint(b2ShapeId a0, const b2Pos* a1, b2Pos* out)+{+	*out = b2Shape_GetClosestPoint(a0, *a1);+}++void hsg_b2Shape_ApplyWind(b2ShapeId a0, const b2Vec2* a1, float a2, float a3, bool a4)+{+	b2Shape_ApplyWind(a0, *a1, a2, a3, a4);+}++void hsg_b2Chain_GetSurfaceMaterial(b2ChainId a0, int a1, b2SurfaceMaterial* out)+{+	*out = b2Chain_GetSurfaceMaterial(a0, a1);+}++void hsg_b2Joint_SetLocalFrameA(b2JointId a0, const b2Transform* a1)+{+	b2Joint_SetLocalFrameA(a0, *a1);+}++void hsg_b2Joint_GetLocalFrameA(b2JointId a0, b2Transform* out)+{+	*out = b2Joint_GetLocalFrameA(a0);+}++void hsg_b2Joint_SetLocalFrameB(b2JointId a0, const b2Transform* a1)+{+	b2Joint_SetLocalFrameB(a0, *a1);+}++void hsg_b2Joint_GetLocalFrameB(b2JointId a0, b2Transform* out)+{+	*out = b2Joint_GetLocalFrameB(a0);+}++void hsg_b2Joint_GetConstraintForce(b2JointId a0, b2Vec2* out)+{+	*out = b2Joint_GetConstraintForce(a0);+}++void hsg_b2MotorJoint_SetLinearVelocity(b2JointId a0, const b2Vec2* a1)+{+	b2MotorJoint_SetLinearVelocity(a0, *a1);+}++void hsg_b2MotorJoint_GetLinearVelocity(b2JointId a0, b2Vec2* out)+{+	*out = b2MotorJoint_GetLinearVelocity(a0);+}++void hsg_b2MoverJoint_SetLinearVelocity(b2JointId a0, const b2Vec2* a1)+{+	b2MoverJoint_SetLinearVelocity(a0, *a1);+}++void hsg_b2MoverJoint_GetLinearVelocity(b2JointId a0, b2Vec2* out)+{+	*out = b2MoverJoint_GetLinearVelocity(a0);+}++void hsg_b2MoverJoint_SetMaxVelocityForce(b2JointId a0, const b2Vec2* a1)+{+	b2MoverJoint_SetMaxVelocityForce(a0, *a1);+}++void hsg_b2MoverJoint_GetMaxVelocityForce(b2JointId a0, b2Vec2* out)+{+	*out = b2MoverJoint_GetMaxVelocityForce(a0);+}++bool hsg_b2Contact_IsValid(const b2ContactId* a0)+{+	return b2Contact_IsValid(*a0);+}++void hsg_b2Contact_GetData(const b2ContactId* a0, b2ContactData* out)+{+	*out = b2Contact_GetData(*a0);+}++void hsg_b2Replay_GetInfo(const b2Replay* a0, b2ReplayInfo* out)+{+	*out = b2Replay_GetInfo(a0);+}++void hsg_b2Replay_GetFrameQuery(const b2Replay* a0, int a1, b2ReplayQueryInfo* out)+{+	*out = b2Replay_GetFrameQuery(a0, a1);+}++void hsg_b2Replay_GetFrameQueryHit(const b2Replay* a0, int a1, int a2, b2ReplayQueryHit* out)+{+	*out = b2Replay_GetFrameQueryHit(a0, a1, a2);+}++void hsg_b2MakePolygon(const b2Hull* a0, float a1, b2Polygon* out)+{+	*out = b2MakePolygon(a0, a1);+}++void hsg_b2MakeOffsetPolygon(const b2Hull* a0, const b2Vec2* a1, const b2Rot* a2, b2Polygon* out)+{+	*out = b2MakeOffsetPolygon(a0, *a1, *a2);+}++void hsg_b2MakeOffsetRoundedPolygon(const b2Hull* a0, const b2Vec2* a1, const b2Rot* a2, float a3, b2Polygon* out)+{+	*out = b2MakeOffsetRoundedPolygon(a0, *a1, *a2, a3);+}++void hsg_b2MakeSquare(float a0, b2Polygon* out)+{+	*out = b2MakeSquare(a0);+}++void hsg_b2MakeBox(float a0, float a1, b2Polygon* out)+{+	*out = b2MakeBox(a0, a1);+}++void hsg_b2MakeRoundedBox(float a0, float a1, float a2, b2Polygon* out)+{+	*out = b2MakeRoundedBox(a0, a1, a2);+}++void hsg_b2MakeOffsetBox(float a0, float a1, const b2Vec2* a2, const b2Rot* a3, b2Polygon* out)+{+	*out = b2MakeOffsetBox(a0, a1, *a2, *a3);+}++void hsg_b2MakeOffsetRoundedBox(float a0, float a1, const b2Vec2* a2, const b2Rot* a3, float a4, b2Polygon* out)+{+	*out = b2MakeOffsetRoundedBox(a0, a1, *a2, *a3, a4);+}++void hsg_b2TransformPolygon(const b2Transform* a0, const b2Polygon* a1, b2Polygon* out)+{+	*out = b2TransformPolygon(*a0, a1);+}++void hsg_b2ComputeCircleMass(const b2Circle* a0, float a1, b2MassData* out)+{+	*out = b2ComputeCircleMass(a0, a1);+}++void hsg_b2ComputeCapsuleMass(const b2Capsule* a0, float a1, b2MassData* out)+{+	*out = b2ComputeCapsuleMass(a0, a1);+}++void hsg_b2ComputePolygonMass(const b2Polygon* a0, float a1, b2MassData* out)+{+	*out = b2ComputePolygonMass(a0, a1);+}++void hsg_b2ComputeCircleBounds(const b2Circle* a0, const b2WorldTransform* a1, b2AABB* out)+{+	*out = b2ComputeCircleBounds(a0, *a1);+}++void hsg_b2ComputeCapsuleBounds(const b2Capsule* a0, const b2WorldTransform* a1, b2AABB* out)+{+	*out = b2ComputeCapsuleBounds(a0, *a1);+}++void hsg_b2ComputePolygonBounds(const b2Polygon* a0, const b2WorldTransform* a1, b2AABB* out)+{+	*out = b2ComputePolygonBounds(a0, *a1);+}++void hsg_b2ComputeSegmentBounds(const b2Segment* a0, const b2WorldTransform* a1, b2AABB* out)+{+	*out = b2ComputeSegmentBounds(a0, *a1);+}++bool hsg_b2PointInCircle(const b2Circle* a0, const b2Vec2* a1)+{+	return b2PointInCircle(a0, *a1);+}++bool hsg_b2PointInCapsule(const b2Capsule* a0, const b2Vec2* a1)+{+	return b2PointInCapsule(a0, *a1);+}++bool hsg_b2PointInPolygon(const b2Polygon* a0, const b2Vec2* a1)+{+	return b2PointInPolygon(a0, *a1);+}++void hsg_b2CastRayCircle(const b2Circle* a0, const b2RayCastInput* a1, b2CastOutput* out)+{+	*out = b2CastRayCircle(a0, a1);+}++void hsg_b2CastRayCapsule(const b2Capsule* a0, const b2RayCastInput* a1, b2CastOutput* out)+{+	*out = b2CastRayCapsule(a0, a1);+}++void hsg_b2CastRaySegment(const b2Segment* a0, const b2RayCastInput* a1, bool a2, b2CastOutput* out)+{+	*out = b2CastRaySegment(a0, a1, a2);+}++void hsg_b2CastRayPolygon(const b2Polygon* a0, const b2RayCastInput* a1, b2CastOutput* out)+{+	*out = b2CastRayPolygon(a0, a1);+}++void hsg_b2CastShapeCircle(const b2Circle* a0, const b2ShapeCastInput* a1, b2CastOutput* out)+{+	*out = b2CastShapeCircle(a0, a1);+}++void hsg_b2CastShapeCapsule(const b2Capsule* a0, const b2ShapeCastInput* a1, b2CastOutput* out)+{+	*out = b2CastShapeCapsule(a0, a1);+}++void hsg_b2CastShapeSegment(const b2Segment* a0, const b2ShapeCastInput* a1, b2CastOutput* out)+{+	*out = b2CastShapeSegment(a0, a1);+}++void hsg_b2CastShapePolygon(const b2Polygon* a0, const b2ShapeCastInput* a1, b2CastOutput* out)+{+	*out = b2CastShapePolygon(a0, a1);+}++void hsg_b2ComputeHull(const b2Vec2* a0, int a1, b2Hull* out)+{+	*out = b2ComputeHull(a0, a1);+}++void hsg_b2ComputeSegmentDistance(const b2Vec2* a0, const b2Vec2* a1, const b2Vec2* a2, const b2Vec2* a3, b2SegmentDistanceResult* out)+{+	*out = b2ComputeSegmentDistance(*a0, *a1, *a2, *a3);+}++void hsg_b2ComputeShapeDistance(const b2DistanceInput* a0, b2SimplexCache* a1, b2Simplex* a2, int a3, b2DistanceOutput* out)+{+	*out = b2ComputeShapeDistance(a0, a1, a2, a3);+}++void hsg_b2CastShape(const b2ShapeCastPairInput* a0, b2CastOutput* out)+{+	*out = b2CastShape(a0);+}++void hsg_b2MakeProxy(const b2Vec2* a0, int a1, float a2, b2ShapeProxy* out)+{+	*out = b2MakeProxy(a0, a1, a2);+}++void hsg_b2MakeOffsetProxy(const b2Vec2* a0, int a1, float a2, const b2Vec2* a3, const b2Rot* a4, b2ShapeProxy* out)+{+	*out = b2MakeOffsetProxy(a0, a1, a2, *a3, *a4);+}++void hsg_b2GetSweepTransform(const b2Sweep* a0, float a1, b2Transform* out)+{+	*out = b2GetSweepTransform(a0, a1);+}++void hsg_b2ComputeTimeOfImpact(const b2TOIInput* a0, b2TOIOutput* out)+{+	*out = b2ComputeTimeOfImpact(a0);+}++void hsg_b2CollideCircles(const b2Circle* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollideCircles(a0, a1, *a2);+}++void hsg_b2CollideCapsuleAndCircle(const b2Capsule* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollideCapsuleAndCircle(a0, a1, *a2);+}++void hsg_b2CollideSegmentAndCircle(const b2Segment* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollideSegmentAndCircle(a0, a1, *a2);+}++void hsg_b2CollidePolygonAndCircle(const b2Polygon* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollidePolygonAndCircle(a0, a1, *a2);+}++void hsg_b2CollideCapsules(const b2Capsule* a0, const b2Capsule* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollideCapsules(a0, a1, *a2);+}++void hsg_b2CollideSegmentAndCapsule(const b2Segment* a0, const b2Capsule* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollideSegmentAndCapsule(a0, a1, *a2);+}++void hsg_b2CollidePolygonAndCapsule(const b2Polygon* a0, const b2Capsule* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollidePolygonAndCapsule(a0, a1, *a2);+}++void hsg_b2CollidePolygons(const b2Polygon* a0, const b2Polygon* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollidePolygons(a0, a1, *a2);+}++void hsg_b2CollideSegmentAndPolygon(const b2Segment* a0, const b2Polygon* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollideSegmentAndPolygon(a0, a1, *a2);+}++void hsg_b2CollideChainSegmentAndCircle(const b2ChainSegment* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out)+{+	*out = b2CollideChainSegmentAndCircle(a0, a1, *a2);+}++void hsg_b2CollideChainSegmentAndCapsule(const b2ChainSegment* a0, const b2Capsule* a1, const b2Transform* a2, b2SimplexCache* a3, b2LocalManifold* out)+{+	*out = b2CollideChainSegmentAndCapsule(a0, a1, *a2, a3);+}++void hsg_b2CollideChainSegmentAndPolygon(const b2ChainSegment* a0, const b2Polygon* a1, const b2Transform* a2, b2SimplexCache* a3, b2LocalManifold* out)+{+	*out = b2CollideChainSegmentAndPolygon(a0, a1, *a2, a3);+}++int hsg_b2CreateTreeProxy(b2DynamicTree* a0, const b2AABB* a1, uint64_t a2, uint64_t a3)+{+	return b2CreateTreeProxy(a0, *a1, a2, a3);+}++void hsg_b2DynamicTree_MoveProxy(b2DynamicTree* a0, int a1, const b2AABB* a2)+{+	b2DynamicTree_MoveProxy(a0, a1, *a2);+}++void hsg_b2DynamicTree_OverlapAABB(const b2DynamicTree* a0, const b2AABB* a1, uint64_t a2, b2TreeQueryCallbackFcn* a3, void* a4, b2TreeStats* out)+{+	*out = b2DynamicTree_OverlapAABB(a0, *a1, a2, a3, a4);+}++void hsg_b2DynamicTree_OverlapAABBAll(const b2DynamicTree* a0, const b2AABB* a1, b2TreeQueryCallbackFcn* a2, void* a3, b2TreeStats* out)+{+	*out = b2DynamicTree_OverlapAABBAll(a0, *a1, a2, a3);+}++void hsg_b2DynamicTree_CastRay(const b2DynamicTree* a0, const b2RayCastInput* a1, uint64_t a2, b2TreeRayCastCallbackFcn* a3, void* a4, b2TreeStats* out)+{+	*out = b2DynamicTree_CastRay(a0, a1, a2, a3, a4);+}++void hsg_b2DynamicTree_CastBox(const b2DynamicTree* a0, const b2BoxCastInput* a1, uint64_t a2, b2TreeBoxCastCallbackFcn* a3, void* a4, b2TreeStats* out)+{+	*out = b2DynamicTree_CastBox(a0, a1, a2, a3, a4);+}++void hsg_b2DynamicTree_GetRootBounds(const b2DynamicTree* a0, b2AABB* out)+{+	*out = b2DynamicTree_GetRootBounds(a0);+}++void hsg_b2DynamicTree_GetProxyBounds(const b2DynamicTree* a0, int a1, b2AABB* out)+{+	*out = b2DynamicTree_GetProxyBounds(a0, a1);+}++void hsg_b2SolvePlanes(const b2Vec2* a0, b2CollisionPlane* a1, int a2, b2PlaneSolverResult* out)+{+	*out = b2SolvePlanes(*a0, a1, a2);+}++void hsg_b2ClipVector(const b2Vec2* a0, const b2CollisionPlane* a1, int a2, b2Vec2* out)+{+	*out = b2ClipVector(*a0, a1, a2);+}
+ cbits/box2d_gen_shim.h view
@@ -0,0 +1,141 @@+// SPDX-License-Identifier: BSD-3-Clause+//+// Generated by boxnd-gen. By-value struct arguments/returns of the+// Box2D 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 "box2d/box2d.h"++void hsg_b2World_GetBounds(b2WorldId a0, b2AABB* out);+void hsg_b2World_GetBodyEvents(b2WorldId a0, b2BodyEvents* out);+void hsg_b2World_GetSensorEvents(b2WorldId a0, b2SensorEvents* out);+void hsg_b2World_GetContactEvents(b2WorldId a0, b2ContactEvents* out);+void hsg_b2World_GetJointEvents(b2WorldId a0, b2JointEvents* out);+void hsg_b2World_OverlapAABB(b2WorldId a0, const b2Pos* a1, const b2AABB* a2, const b2QueryFilter* a3, b2OverlapResultCallbackFcn* a4, void* a5, b2TreeStats* out);+void hsg_b2World_OverlapShape(b2WorldId a0, const b2Pos* a1, const b2ShapeProxy* a2, const b2QueryFilter* a3, b2OverlapResultCallbackFcn* a4, void* a5, b2TreeStats* out);+void hsg_b2World_CastRay(b2WorldId a0, const b2Pos* a1, const b2Vec2* a2, const b2QueryFilter* a3, b2CastResultCallbackFcn* a4, void* a5, b2TreeStats* out);+void hsg_b2World_CastRayClosest(b2WorldId a0, const b2Pos* a1, const b2Vec2* a2, const b2QueryFilter* a3, b2RayResult* out);+void hsg_b2World_CastShape(b2WorldId a0, const b2Pos* a1, const b2ShapeProxy* a2, const b2Vec2* a3, const b2QueryFilter* a4, b2CastResultCallbackFcn* a5, void* a6, b2TreeStats* out);+float hsg_b2World_CastMover(b2WorldId a0, const b2Pos* a1, const b2Capsule* a2, const b2Vec2* a3, const b2QueryFilter* a4);+void hsg_b2World_CollideMover(b2WorldId a0, const b2Pos* a1, const b2Capsule* a2, const b2QueryFilter* a3, b2PlaneResultCallbackFcn* a4, void* a5);+void hsg_b2World_SetGravity(b2WorldId a0, const b2Vec2* a1);+void hsg_b2World_GetGravity(b2WorldId a0, b2Vec2* out);+void hsg_b2World_GetProfile(b2WorldId a0, b2Profile* out);+void hsg_b2World_GetCounters(b2WorldId a0, b2Counters* out);+void hsg_b2World_GetMaxCapacity(b2WorldId a0, b2Capacity* out);+void hsg_b2Body_GetPosition(b2BodyId a0, b2Pos* out);+void hsg_b2Body_GetRotation(b2BodyId a0, b2Rot* out);+void hsg_b2Body_GetTransform(b2BodyId a0, b2WorldTransform* out);+void hsg_b2Body_SetTransform(b2BodyId a0, const b2Pos* a1, const b2Rot* a2);+void hsg_b2Body_GetLocalPoint(b2BodyId a0, const b2Pos* a1, b2Vec2* out);+void hsg_b2Body_GetWorldPoint(b2BodyId a0, const b2Vec2* a1, b2Pos* out);+void hsg_b2Body_GetLocalVector(b2BodyId a0, const b2Vec2* a1, b2Vec2* out);+void hsg_b2Body_GetWorldVector(b2BodyId a0, const b2Vec2* a1, b2Vec2* out);+void hsg_b2Body_GetLinearVelocity(b2BodyId a0, b2Vec2* out);+void hsg_b2Body_SetLinearVelocity(b2BodyId a0, const b2Vec2* a1);+void hsg_b2Body_SetTargetTransform(b2BodyId a0, const b2WorldTransform* a1, float a2, bool a3);+void hsg_b2Body_GetLocalPointVelocity(b2BodyId a0, const b2Vec2* a1, b2Vec2* out);+void hsg_b2Body_GetWorldPointVelocity(b2BodyId a0, const b2Pos* a1, b2Vec2* out);+void hsg_b2Body_ApplyForce(b2BodyId a0, const b2Vec2* a1, const b2Pos* a2, bool a3);+void hsg_b2Body_ApplyForceToCenter(b2BodyId a0, const b2Vec2* a1, bool a2);+void hsg_b2Body_ApplyLinearImpulse(b2BodyId a0, const b2Vec2* a1, const b2Pos* a2, bool a3);+void hsg_b2Body_ApplyLinearImpulseToCenter(b2BodyId a0, const b2Vec2* a1, bool a2);+void hsg_b2Body_GetLocalCenter(b2BodyId a0, b2Vec2* out);+void hsg_b2Body_GetWorldCenter(b2BodyId a0, b2Pos* out);+void hsg_b2Body_SetMassData(b2BodyId a0, const b2MassData* a1);+void hsg_b2Body_GetMassData(b2BodyId a0, b2MassData* out);+void hsg_b2Body_SetMotionLocks(b2BodyId a0, const b2MotionLocks* a1);+void hsg_b2Body_GetMotionLocks(b2BodyId a0, b2MotionLocks* out);+void hsg_b2Body_ComputeBounds(b2BodyId a0, b2AABB* out);+void hsg_b2Shape_GetSurfaceMaterial(b2ShapeId a0, b2SurfaceMaterial* out);+void hsg_b2Shape_GetFilter(b2ShapeId a0, b2Filter* out);+void hsg_b2Shape_SetFilter(b2ShapeId a0, const b2Filter* a1);+bool hsg_b2Shape_TestPoint(b2ShapeId a0, const b2Pos* a1);+void hsg_b2Shape_CastRay(b2ShapeId a0, const b2Pos* a1, const b2Vec2* a2, b2WorldCastOutput* out);+void hsg_b2Shape_GetCircle(b2ShapeId a0, b2Circle* out);+void hsg_b2Shape_GetSegment(b2ShapeId a0, b2Segment* out);+void hsg_b2Shape_GetChainSegment(b2ShapeId a0, b2ChainSegment* out);+void hsg_b2Shape_GetCapsule(b2ShapeId a0, b2Capsule* out);+void hsg_b2Shape_GetPolygon(b2ShapeId a0, b2Polygon* out);+void hsg_b2Shape_GetBounds(b2ShapeId a0, b2AABB* out);+void hsg_b2Shape_ComputeMassData(b2ShapeId a0, b2MassData* out);+void hsg_b2Shape_GetClosestPoint(b2ShapeId a0, const b2Pos* a1, b2Pos* out);+void hsg_b2Shape_ApplyWind(b2ShapeId a0, const b2Vec2* a1, float a2, float a3, bool a4);+void hsg_b2Chain_GetSurfaceMaterial(b2ChainId a0, int a1, b2SurfaceMaterial* out);+void hsg_b2Joint_SetLocalFrameA(b2JointId a0, const b2Transform* a1);+void hsg_b2Joint_GetLocalFrameA(b2JointId a0, b2Transform* out);+void hsg_b2Joint_SetLocalFrameB(b2JointId a0, const b2Transform* a1);+void hsg_b2Joint_GetLocalFrameB(b2JointId a0, b2Transform* out);+void hsg_b2Joint_GetConstraintForce(b2JointId a0, b2Vec2* out);+void hsg_b2MotorJoint_SetLinearVelocity(b2JointId a0, const b2Vec2* a1);+void hsg_b2MotorJoint_GetLinearVelocity(b2JointId a0, b2Vec2* out);+void hsg_b2MoverJoint_SetLinearVelocity(b2JointId a0, const b2Vec2* a1);+void hsg_b2MoverJoint_GetLinearVelocity(b2JointId a0, b2Vec2* out);+void hsg_b2MoverJoint_SetMaxVelocityForce(b2JointId a0, const b2Vec2* a1);+void hsg_b2MoverJoint_GetMaxVelocityForce(b2JointId a0, b2Vec2* out);+bool hsg_b2Contact_IsValid(const b2ContactId* a0);+void hsg_b2Contact_GetData(const b2ContactId* a0, b2ContactData* out);+void hsg_b2Replay_GetInfo(const b2Replay* a0, b2ReplayInfo* out);+void hsg_b2Replay_GetFrameQuery(const b2Replay* a0, int a1, b2ReplayQueryInfo* out);+void hsg_b2Replay_GetFrameQueryHit(const b2Replay* a0, int a1, int a2, b2ReplayQueryHit* out);+void hsg_b2MakePolygon(const b2Hull* a0, float a1, b2Polygon* out);+void hsg_b2MakeOffsetPolygon(const b2Hull* a0, const b2Vec2* a1, const b2Rot* a2, b2Polygon* out);+void hsg_b2MakeOffsetRoundedPolygon(const b2Hull* a0, const b2Vec2* a1, const b2Rot* a2, float a3, b2Polygon* out);+void hsg_b2MakeSquare(float a0, b2Polygon* out);+void hsg_b2MakeBox(float a0, float a1, b2Polygon* out);+void hsg_b2MakeRoundedBox(float a0, float a1, float a2, b2Polygon* out);+void hsg_b2MakeOffsetBox(float a0, float a1, const b2Vec2* a2, const b2Rot* a3, b2Polygon* out);+void hsg_b2MakeOffsetRoundedBox(float a0, float a1, const b2Vec2* a2, const b2Rot* a3, float a4, b2Polygon* out);+void hsg_b2TransformPolygon(const b2Transform* a0, const b2Polygon* a1, b2Polygon* out);+void hsg_b2ComputeCircleMass(const b2Circle* a0, float a1, b2MassData* out);+void hsg_b2ComputeCapsuleMass(const b2Capsule* a0, float a1, b2MassData* out);+void hsg_b2ComputePolygonMass(const b2Polygon* a0, float a1, b2MassData* out);+void hsg_b2ComputeCircleBounds(const b2Circle* a0, const b2WorldTransform* a1, b2AABB* out);+void hsg_b2ComputeCapsuleBounds(const b2Capsule* a0, const b2WorldTransform* a1, b2AABB* out);+void hsg_b2ComputePolygonBounds(const b2Polygon* a0, const b2WorldTransform* a1, b2AABB* out);+void hsg_b2ComputeSegmentBounds(const b2Segment* a0, const b2WorldTransform* a1, b2AABB* out);+bool hsg_b2PointInCircle(const b2Circle* a0, const b2Vec2* a1);+bool hsg_b2PointInCapsule(const b2Capsule* a0, const b2Vec2* a1);+bool hsg_b2PointInPolygon(const b2Polygon* a0, const b2Vec2* a1);+void hsg_b2CastRayCircle(const b2Circle* a0, const b2RayCastInput* a1, b2CastOutput* out);+void hsg_b2CastRayCapsule(const b2Capsule* a0, const b2RayCastInput* a1, b2CastOutput* out);+void hsg_b2CastRaySegment(const b2Segment* a0, const b2RayCastInput* a1, bool a2, b2CastOutput* out);+void hsg_b2CastRayPolygon(const b2Polygon* a0, const b2RayCastInput* a1, b2CastOutput* out);+void hsg_b2CastShapeCircle(const b2Circle* a0, const b2ShapeCastInput* a1, b2CastOutput* out);+void hsg_b2CastShapeCapsule(const b2Capsule* a0, const b2ShapeCastInput* a1, b2CastOutput* out);+void hsg_b2CastShapeSegment(const b2Segment* a0, const b2ShapeCastInput* a1, b2CastOutput* out);+void hsg_b2CastShapePolygon(const b2Polygon* a0, const b2ShapeCastInput* a1, b2CastOutput* out);+void hsg_b2ComputeHull(const b2Vec2* a0, int a1, b2Hull* out);+void hsg_b2ComputeSegmentDistance(const b2Vec2* a0, const b2Vec2* a1, const b2Vec2* a2, const b2Vec2* a3, b2SegmentDistanceResult* out);+void hsg_b2ComputeShapeDistance(const b2DistanceInput* a0, b2SimplexCache* a1, b2Simplex* a2, int a3, b2DistanceOutput* out);+void hsg_b2CastShape(const b2ShapeCastPairInput* a0, b2CastOutput* out);+void hsg_b2MakeProxy(const b2Vec2* a0, int a1, float a2, b2ShapeProxy* out);+void hsg_b2MakeOffsetProxy(const b2Vec2* a0, int a1, float a2, const b2Vec2* a3, const b2Rot* a4, b2ShapeProxy* out);+void hsg_b2GetSweepTransform(const b2Sweep* a0, float a1, b2Transform* out);+void hsg_b2ComputeTimeOfImpact(const b2TOIInput* a0, b2TOIOutput* out);+void hsg_b2CollideCircles(const b2Circle* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollideCapsuleAndCircle(const b2Capsule* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollideSegmentAndCircle(const b2Segment* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollidePolygonAndCircle(const b2Polygon* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollideCapsules(const b2Capsule* a0, const b2Capsule* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollideSegmentAndCapsule(const b2Segment* a0, const b2Capsule* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollidePolygonAndCapsule(const b2Polygon* a0, const b2Capsule* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollidePolygons(const b2Polygon* a0, const b2Polygon* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollideSegmentAndPolygon(const b2Segment* a0, const b2Polygon* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollideChainSegmentAndCircle(const b2ChainSegment* a0, const b2Circle* a1, const b2Transform* a2, b2LocalManifold* out);+void hsg_b2CollideChainSegmentAndCapsule(const b2ChainSegment* a0, const b2Capsule* a1, const b2Transform* a2, b2SimplexCache* a3, b2LocalManifold* out);+void hsg_b2CollideChainSegmentAndPolygon(const b2ChainSegment* a0, const b2Polygon* a1, const b2Transform* a2, b2SimplexCache* a3, b2LocalManifold* out);+int hsg_b2CreateTreeProxy(b2DynamicTree* a0, const b2AABB* a1, uint64_t a2, uint64_t a3);+void hsg_b2DynamicTree_MoveProxy(b2DynamicTree* a0, int a1, const b2AABB* a2);+void hsg_b2DynamicTree_OverlapAABB(const b2DynamicTree* a0, const b2AABB* a1, uint64_t a2, b2TreeQueryCallbackFcn* a3, void* a4, b2TreeStats* out);+void hsg_b2DynamicTree_OverlapAABBAll(const b2DynamicTree* a0, const b2AABB* a1, b2TreeQueryCallbackFcn* a2, void* a3, b2TreeStats* out);+void hsg_b2DynamicTree_CastRay(const b2DynamicTree* a0, const b2RayCastInput* a1, uint64_t a2, b2TreeRayCastCallbackFcn* a3, void* a4, b2TreeStats* out);+void hsg_b2DynamicTree_CastBox(const b2DynamicTree* a0, const b2BoxCastInput* a1, uint64_t a2, b2TreeBoxCastCallbackFcn* a3, void* a4, b2TreeStats* out);+void hsg_b2DynamicTree_GetRootBounds(const b2DynamicTree* a0, b2AABB* out);+void hsg_b2DynamicTree_GetProxyBounds(const b2DynamicTree* a0, int a1, b2AABB* out);+void hsg_b2SolvePlanes(const b2Vec2* a0, b2CollisionPlane* a1, int a2, b2PlaneSolverResult* out);+void hsg_b2ClipVector(const b2Vec2* a0, const b2CollisionPlane* a1, int a2, b2Vec2* out);
+ cbits/box2d_shim.c view
@@ -0,0 +1,322 @@+// SPDX-License-Identifier: BSD-3-Clause+//+// See box2d_shim.h for the rationale behind this file.++#include "box2d_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(b2WorldId) == 4, "b2WorldId is passed as Word32");+_Static_assert(sizeof(b2BodyId) == 8, "b2BodyId is passed as Word64");+_Static_assert(sizeof(b2ShapeId) == 8, "b2ShapeId is passed as Word64");+_Static_assert(sizeof(b2ChainId) == 8, "b2ChainId is passed as Word64");+_Static_assert(sizeof(b2JointId) == 8, "b2JointId is passed as Word64");++// base.h -----------------------------------------------------------------++void hs_b2GetVersion(b2Version* out)+{+	*out = b2GetVersion();+}++// math_functions.h -------------------------------------------------------++bool hs_b2IsValidVec2(const b2Vec2* a)+{+	return b2IsValidVec2(*a);+}++bool hs_b2IsValidRot(const b2Rot* q)+{+	return b2IsValidRot(*q);+}++bool hs_b2IsValidTransform(const b2Transform* t)+{+	return b2IsValidTransform(*t);+}++bool hs_b2IsValidAABB(const b2AABB* a)+{+	return b2IsValidAABB(*a);+}++bool hs_b2IsValidPlane(const b2Plane* a)+{+	return b2IsValidPlane(*a);+}++void hs_b2ComputeCosSin(float radians, b2CosSin* out)+{+	*out = b2ComputeCosSin(radians);+}++void hs_b2MakeRot(float radians, b2Rot* out)+{+	*out = b2MakeRot(radians);+}++// types.h: default definition constructors --------------------------------++void hs_b2DefaultWorldDef(b2WorldDef* out)+{+	*out = b2DefaultWorldDef();+}++void hs_b2DefaultBodyDef(b2BodyDef* out)+{+	*out = b2DefaultBodyDef();+}++void hs_b2DefaultShapeDef(b2ShapeDef* out)+{+	*out = b2DefaultShapeDef();+}++void hs_b2DefaultFilter(b2Filter* out)+{+	*out = b2DefaultFilter();+}++void hs_b2DefaultSurfaceMaterial(b2SurfaceMaterial* out)+{+	*out = b2DefaultSurfaceMaterial();+}++void hs_b2DefaultQueryFilter(b2QueryFilter* out)+{+	*out = b2DefaultQueryFilter();+}++void hs_b2DefaultChainDef(b2ChainDef* out)+{+	*out = b2DefaultChainDef();+}++void hs_b2DefaultExplosionDef(b2ExplosionDef* out)+{+	*out = b2DefaultExplosionDef();+}++void hs_b2DefaultDebugDraw(b2DebugDraw* out)+{+	*out = b2DefaultDebugDraw();+}++// types.h: joint definition defaults ---------------------------------------++void hs_b2DefaultDistanceJointDef(b2DistanceJointDef* out)+{+	*out = b2DefaultDistanceJointDef();+}++void hs_b2DefaultMotorJointDef(b2MotorJointDef* out)+{+	*out = b2DefaultMotorJointDef();+}++void hs_b2DefaultPrismaticJointDef(b2PrismaticJointDef* out)+{+	*out = b2DefaultPrismaticJointDef();+}++void hs_b2DefaultRevoluteJointDef(b2RevoluteJointDef* out)+{+	*out = b2DefaultRevoluteJointDef();+}++void hs_b2DefaultWeldJointDef(b2WeldJointDef* out)+{+	*out = b2DefaultWeldJointDef();+}++void hs_b2DefaultWheelJointDef(b2WheelJointDef* out)+{+	*out = b2DefaultWheelJointDef();+}++void hs_b2DefaultFilterJointDef(b2FilterJointDef* out)+{+	*out = b2DefaultFilterJointDef();+}+++// Callback trampolines ------------------------------------------------------+// GHC callbacks cannot receive structs by value, so these fixed C functions+// take the by-value b2Pos/b2Vec2 arguments and forward pointers to a+// Haskell-wrapped function carried in the context slot.++typedef float (*hs_b2CastResultHsFcn)(b2ShapeId shapeId, const b2Pos* point, const b2Vec2* normal, float fraction,+									  void* context);++typedef bool (*hs_b2PreContinuousHsFcn)(b2ShapeId shapeIdA, b2ShapeId shapeIdB, const b2Pos* point, const b2Vec2* normal,+								   void* context);++float hs_b2CastResultTrampoline(b2ShapeId shapeId, b2Pos point, b2Vec2 normal, float fraction, void* context)+{+	hs_b2Closure* c = context;+	return ((hs_b2CastResultHsFcn)c->fn)(shapeId, &point, &normal, fraction, c->context);+}++bool hs_b2PreContinuousTrampoline(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point, b2Vec2 normal, void* context)+{+	hs_b2Closure* c = context;+	return ((hs_b2PreContinuousHsFcn)c->fn)(shapeIdA, shapeIdB, &point, &normal, c->context);+}++// b2DebugDraw trampolines ---------------------------------------------------+// The Haskell side fills hs_b2DrawClosure 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_b2DrawPolygonHsFcn)(const b2WorldTransform* transform, const b2Vec2* vertices, int vertexCount,+									  b2HexColor color, void* context);+typedef void (*hs_b2DrawSolidPolygonHsFcn)(const b2WorldTransform* transform, const b2Vec2* vertices, int vertexCount,+										   float radius, b2HexColor color, void* context);+typedef void (*hs_b2DrawCircleHsFcn)(const b2Pos* center, float radius, b2HexColor color, void* context);+typedef void (*hs_b2DrawSolidCircleHsFcn)(const b2WorldTransform* transform, const b2Vec2* center, float radius,+										  b2HexColor color, void* context);+typedef void (*hs_b2DrawSolidCapsuleHsFcn)(const b2Pos* p1, const b2Pos* p2, float radius, b2HexColor color,+										   void* context);+typedef void (*hs_b2DrawLineHsFcn)(const b2Pos* p1, const b2Pos* p2, b2HexColor color, void* context);+typedef void (*hs_b2DrawTransformHsFcn)(const b2WorldTransform* transform, void* context);+typedef void (*hs_b2DrawPointHsFcn)(const b2Pos* p, float size, b2HexColor color, void* context);+typedef void (*hs_b2DrawStringHsFcn)(const b2Pos* p, const char* s, b2HexColor color, void* context);+typedef void (*hs_b2DrawBoundsHsFcn)(const b2AABB* aabb, b2HexColor color, void* context);++void hs_b2DrawPolygonTrampoline(b2WorldTransform transform, const b2Vec2* vertices, int vertexCount, b2HexColor color,+								void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->polygon != NULL)+	{+		((hs_b2DrawPolygonHsFcn)c->polygon)(&transform, vertices, vertexCount, color, c->context);+	}+}++void hs_b2DrawSolidPolygonTrampoline(b2WorldTransform transform, const b2Vec2* vertices, int vertexCount, float radius,+									 b2HexColor color, void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->solidPolygon != NULL)+	{+		((hs_b2DrawSolidPolygonHsFcn)c->solidPolygon)(&transform, vertices, vertexCount, radius, color, c->context);+	}+}++void hs_b2DrawCircleTrampoline(b2Pos center, float radius, b2HexColor color, void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->circle != NULL)+	{+		((hs_b2DrawCircleHsFcn)c->circle)(&center, radius, color, c->context);+	}+}++void hs_b2DrawSolidCircleTrampoline(b2WorldTransform transform, b2Vec2 center, float radius, b2HexColor color,+									void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->solidCircle != NULL)+	{+		((hs_b2DrawSolidCircleHsFcn)c->solidCircle)(&transform, &center, radius, color, c->context);+	}+}++void hs_b2DrawSolidCapsuleTrampoline(b2Pos p1, b2Pos p2, float radius, b2HexColor color, void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->solidCapsule != NULL)+	{+		((hs_b2DrawSolidCapsuleHsFcn)c->solidCapsule)(&p1, &p2, radius, color, c->context);+	}+}++void hs_b2DrawLineTrampoline(b2Pos p1, b2Pos p2, b2HexColor color, void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->line != NULL)+	{+		((hs_b2DrawLineHsFcn)c->line)(&p1, &p2, color, c->context);+	}+}++void hs_b2DrawTransformTrampoline(b2WorldTransform transform, void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->transform != NULL)+	{+		((hs_b2DrawTransformHsFcn)c->transform)(&transform, c->context);+	}+}++void hs_b2DrawPointTrampoline(b2Pos p, float size, b2HexColor color, void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->point != NULL)+	{+		((hs_b2DrawPointHsFcn)c->point)(&p, size, color, c->context);+	}+}++void hs_b2DrawStringTrampoline(b2Pos p, const char* s, b2HexColor color, void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->string != NULL)+	{+		((hs_b2DrawStringHsFcn)c->string)(&p, s, color, c->context);+	}+}++void hs_b2DrawBoundsTrampoline(b2AABB aabb, b2HexColor color, void* context)+{+	hs_b2DrawClosure* c = context;+	if (c->bounds != NULL)+	{+		((hs_b2DrawBoundsHsFcn)c->bounds)(&aabb, color, c->context);+	}+}++// chains ---------------------------------------------------------------------++// The toggles need each segment's b2ShapeId, but b2Chain_GetSegments always+// copies from segment zero, so fetching them through the public API would+// force a heap allocation (with a silent no-op as its only failure mode).+// These internal headers give the loop direct access to the chain's shape+// indices instead; b2GetChainShape is static in shape.c, so its two-line+// lookup is inlined here.+#include "physics_world.h"+#include "shape.h"++typedef void (*hs_b2SegmentToggle)(b2ShapeId shapeId, bool flag);++static void hs_b2Chain_ForEachSegment(b2ChainId chainId, hs_b2SegmentToggle toggle, bool flag)+{+	b2World* world = b2GetWorldLocked(chainId.world0);+	if (world == NULL)+	{+		return;+	}++	int id = chainId.index1 - 1;+	b2ChainShape* chain = b2Array_Get(world->chainShapes, id);+	B2_ASSERT(chain->id == id && chain->generation == chainId.generation);++	for (int i = 0; i < chain->segmentCount; ++i)+	{+		int shapeId = chain->shapeIndices[i];+		b2Shape* shape = b2Array_Get(world->shapes, shapeId);+		toggle((b2ShapeId){ shapeId + 1, chainId.world0, shape->generation }, flag);+	}+}++void hs_b2Chain_EnableContactEvents(b2ChainId chainId, bool flag)+{+	hs_b2Chain_ForEachSegment(chainId, b2Shape_EnableContactEvents, flag);+}++void hs_b2Chain_EnableHitEvents(b2ChainId chainId, bool flag)+{+	hs_b2Chain_ForEachSegment(chainId, b2Shape_EnableHitEvents, flag);+}
+ cbits/box2d_shim.h view
@@ -0,0 +1,121 @@+// SPDX-License-Identifier: BSD-3-Clause+//+// Thin C shim for the Box2D Haskell bindings.+//+// The Box2D 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+// box2d_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 b2 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 "box2d/box2d.h"++// base.h -----------------------------------------------------------------++// b2Version b2GetVersion(void)+void hs_b2GetVersion(b2Version* out);++// math_functions.h -------------------------------------------------------++bool hs_b2IsValidVec2(const b2Vec2* a);+bool hs_b2IsValidRot(const b2Rot* q);+bool hs_b2IsValidTransform(const b2Transform* t);+bool hs_b2IsValidAABB(const b2AABB* a);+bool hs_b2IsValidPlane(const b2Plane* a);++// b2CosSin b2ComputeCosSin(float radians)+void hs_b2ComputeCosSin(float radians, b2CosSin* out);++// b2Rot b2MakeRot(float radians)+void hs_b2MakeRot(float radians, b2Rot* out);++// types.h: default definition constructors --------------------------------++void hs_b2DefaultWorldDef(b2WorldDef* out);+void hs_b2DefaultBodyDef(b2BodyDef* out);+void hs_b2DefaultShapeDef(b2ShapeDef* out);+void hs_b2DefaultFilter(b2Filter* out);+void hs_b2DefaultSurfaceMaterial(b2SurfaceMaterial* out);+void hs_b2DefaultQueryFilter(b2QueryFilter* out);+void hs_b2DefaultChainDef(b2ChainDef* out);+void hs_b2DefaultExplosionDef(b2ExplosionDef* out);+void hs_b2DefaultDebugDraw(b2DebugDraw* out);++// The world / body by-value accessors now live in the generated+// box2d_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_b2DefaultDistanceJointDef(b2DistanceJointDef* out);+void hs_b2DefaultMotorJointDef(b2MotorJointDef* out);+void hs_b2DefaultPrismaticJointDef(b2PrismaticJointDef* out);+void hs_b2DefaultRevoluteJointDef(b2RevoluteJointDef* out);+void hs_b2DefaultWeldJointDef(b2WeldJointDef* out);+void hs_b2DefaultWheelJointDef(b2WheelJointDef* out);+void hs_b2DefaultFilterJointDef(b2FilterJointDef* out);++// chains ---------------------------------------------------------------------++// b2CreateChain leaves contact/hit events disabled on its segment shapes;+// these toggle every segment of a chain in one call instead of one FFI+// round-trip per segment.+void hs_b2Chain_EnableContactEvents(b2ChainId chainId, bool flag);+void hs_b2Chain_EnableHitEvents(b2ChainId chainId, bool flag);++// Callback trampolines: fixed C functions with the engine's callback+// signatures. The context must point at an hs_b2Closure 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_b2Closure+{+	void* fn;+	void* context;+} hs_b2Closure;++float hs_b2CastResultTrampoline(b2ShapeId shapeId, b2Pos point, b2Vec2 normal, float fraction, void* context);+bool hs_b2PreContinuousTrampoline(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point, b2Vec2 normal, void* context);++// b2DebugDraw trampolines. The struct's ten callback slots share one context+// field, so they take a wider closure than hs_b2Closure: one Haskell-wrapped+// function per slot, in the declaration order of b2DebugDraw, followed by the+// user context. A NULL slot draws nothing.++typedef struct hs_b2DrawClosure+{+	void* polygon;+	void* solidPolygon;+	void* circle;+	void* solidCircle;+	void* solidCapsule;+	void* line;+	void* transform;+	void* point;+	void* string;+	void* bounds;+	void* context;+} hs_b2DrawClosure;++void hs_b2DrawPolygonTrampoline(b2WorldTransform transform, const b2Vec2* vertices, int vertexCount, b2HexColor color,+								void* context);+void hs_b2DrawSolidPolygonTrampoline(b2WorldTransform transform, const b2Vec2* vertices, int vertexCount, float radius,+									 b2HexColor color, void* context);+void hs_b2DrawCircleTrampoline(b2Pos center, float radius, b2HexColor color, void* context);+void hs_b2DrawSolidCircleTrampoline(b2WorldTransform transform, b2Vec2 center, float radius, b2HexColor color,+									void* context);+void hs_b2DrawSolidCapsuleTrampoline(b2Pos p1, b2Pos p2, float radius, b2HexColor color, void* context);+void hs_b2DrawLineTrampoline(b2Pos p1, b2Pos p2, b2HexColor color, void* context);+void hs_b2DrawTransformTrampoline(b2WorldTransform transform, void* context);+void hs_b2DrawPointTrampoline(b2Pos p, float size, b2HexColor color, void* context);+void hs_b2DrawStringTrampoline(b2Pos p, const char* s, b2HexColor color, void* context);+void hs_b2DrawBoundsTrampoline(b2AABB aabb, b2HexColor color, void* context);
+ src/Box2D/Base.hsc view
@@ -0,0 +1,141 @@+{-# LANGUAGE ForeignFunctionInterface #-}++-- | Low-level bindings to @box2d/base.h@: version info, global allocator /+-- assert / log hooks, and timing helpers.+module Box2D.Base+  ( -- * Version+    Version (..)+  , getVersion+  , isDoublePrecision+  , isAVX2Available++    -- * Allocator+  , AllocCallbackFcn+  , FreeCallbackFcn+  , setAllocator+  , getByteCount++    -- * Assert / log hooks+  , AssertCallbackFcn+  , LogCallbackFcn+  , setAssertCallback+  , setLogCallback++    -- * Timing+  , getTicks+  , getMilliseconds+  , getMillisecondsAndReset+  , yield+  , hash+  ) where++import Foreign+import Foreign.C.Types+import Foreign.C.String (CString)++import Box2D.Internal (allocaOut)++#include "box2d/box2d.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 b2Version}+  alignment _ = #{alignment b2Version}+  peek p =+    Version+      <$> #{peek b2Version, major} p+      <*> #{peek b2Version, minor} p+      <*> #{peek b2Version, revision} p+  poke p (Version ma mi re) = do+    #{poke b2Version, major} p ma+    #{poke b2Version, minor} p mi+    #{poke b2Version, revision} p re++foreign import ccall unsafe "hs_b2GetVersion"+  c_b2GetVersion :: Ptr Version -> IO ()++-- | Get the current version of Box2D.+getVersion :: IO Version+getVersion = allocaOut c_b2GetVersion++-- | 'True' if the library was built with @BOX2D_DOUBLE_PRECISION@.+foreign import ccall unsafe "b2IsDoublePrecision"+  c_b2IsDoublePrecision :: IO CBool++isDoublePrecision :: IO Bool+isDoublePrecision = (/= 0) <$> c_b2IsDoublePrecision++-- | 'True' if the current CPU has the AVX2 instruction set.+foreign import ccall unsafe "b2IsAVX2Available"+  c_b2IsAVX2Available :: IO CBool++isAVX2Available :: IO Bool+isAVX2Available = (/= 0) <$> c_b2IsAVX2Available++-- Allocator ---------------------------------------------------------------++-- | User allocation function: @void* (size_t size, int alignment)@.+type AllocCallbackFcn = CSize -> CInt -> IO (Ptr ())++-- | User free function: @void (void* mem, size_t size)@.+type FreeCallbackFcn = Ptr () -> CSize -> 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 "b2SetAllocator"+  setAllocator :: FunPtr AllocCallbackFcn -> FunPtr FreeCallbackFcn -> IO ()++-- | Total bytes allocated by Box2D.+foreign import ccall unsafe "b2GetByteCount"+  getByteCount :: IO CSize++-- Assert / log ------------------------------------------------------------++-- | Assert callback: @int (const char* condition, const char* file, int line)@.+-- Return 0 to skip the debugger break.+type AssertCallbackFcn = CString -> CString -> CInt -> IO CInt++-- | Log callback: @void (const char* message)@.+type LogCallbackFcn = CString -> IO ()++-- | Override the default assert callback. Must point to a C function, see+-- 'setAllocator'.+foreign import ccall unsafe "b2SetAssertCallback"+  setAssertCallback :: FunPtr AssertCallbackFcn -> IO ()++-- | Override the default logging callback. Must point to a C function, see+-- 'setAllocator'.+foreign import ccall unsafe "b2SetLogCallback"+  setLogCallback :: FunPtr LogCallbackFcn -> IO ()++-- Timing ------------------------------------------------------------------++-- | Absolute number of system ticks. The value is platform specific.+foreign import ccall unsafe "b2GetTicks"+  getTicks :: IO Word64++-- | Milliseconds passed from an initial tick value.+foreign import ccall unsafe "b2GetMilliseconds"+  getMilliseconds :: Word64 -> IO CFloat++-- | Milliseconds passed from an initial tick value, resetting it to now.+foreign import ccall unsafe "b2GetMillisecondsAndReset"+  getMillisecondsAndReset :: Ptr Word64 -> IO CFloat++-- | Yield to be used in a busy loop.+foreign import ccall unsafe "b2Yield"+  yield :: IO ()++-- | Simple djb2 hash function for determinism testing.+foreign import ccall unsafe "b2Hash"+  hash :: Word32 -> Ptr Word8 -> CInt -> IO Word32
+ src/Box2D/Body.hsc view
@@ -0,0 +1,1520 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.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+  , setLinearVelocity+  , setAngularVelocity+  , setTargetTransform+  , getLocalPointVelocity+  , getWorldPointVelocity+  , applyForce+  , applyForceToCenter+  , applyTorque+  , clearForces+  , applyLinearImpulse+  , applyLinearImpulseToCenter+  , applyAngularImpulse+  , getMass+  , getRotationalInertia+  , getLocalCenter+  , getLocalCenterInto+  , getWorldCenter+  , getWorldCenterInto+  , setMassData+  , getMassData+  , getMassDataInto+  , updateMassFromShapes+  , setLinearDamping+  , getLinearDamping+  , setAngularDamping+  , getAngularDamping+  , setGravityScale+  , getGravityScale+  , isAwake+  , setAwake+  , wakeTouching+  , enableSleeping+  , isSleepingEnabled+  , setSleepThreshold+  , getSleepThreshold+  , setSafetyFactor+  , getSafetyFactor+  , isEnabled+  , disable+  , enable+  , setMotionLocks+  , getMotionLocks+  , getMotionLocksInto+  , setBullet+  , isBullet+  , enableContactRecycling+  , isContactRecyclingEnabled+  , enableContactEvents+  , enableHitEvents+  , getWorld+  , getShapeCount+  , getShapes+  , getJointCount+  , getJoints+  , getContactCapacity+  , getContactData+  , computeBounds+  , computeBoundsInto+  , getMinExtent+  , getMaxExtent+  , getMaxExtentFromOrigin+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Foreign.C.String (CString)+import Data.Coerce (coerce)+import Box2D.Internal (allocaOut)+import Box2D.Contact (ContactData)+import Box2D.Geometry (MassData)+import Box2D.Id (BodyId (..), JointId (..), ShapeId (..), WorldId (..))+import Box2D.MathTypes (AABB, Pos, Rot, Vec2, WorldTransform)++#include "box2d/box2d.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 @b2DefaultBodyDef()@.+-}+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 :: Rot+  -- ^ The initial world rotation of the body. Use @b2MakeRot()@ if you have an angle.+  , linearVelocity :: Vec2+  -- ^ The initial linear velocity of the body\'s origin. Usually in meters per second.+  , angularVelocity :: Float+  -- ^ 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+  , safetyFactor :: Float+  -- ^ Continuous collision safety factor. The solver only uses continuous collision if there is a+  -- risk of tunneling. If the body is moving fast enough to risk tunneling then it is considered+  -- a \"fast body\". This improves performance and prevents movement hitches. If a body moving N+  -- meters risks tunneling, then the body will be considered fast if it moves more than a+  -- safetyFactor times N meters over one full time step. Non-dimensional. Recommended range+  -- \[0.01, 0.5\]. Default is 0.5 for high performance with low tunneling risk.+  , name :: CString+  -- ^ Optional body name for debugging. Up to @B2_NAME_LENGTH@ characters+  , userData :: Ptr ()+  -- ^ Use this to store application specific body data.+  , motionLocks :: MotionLocks+  -- ^ Motion locks to restrict linear and angular movement. Caution: may lead to softer+  -- constraints along the locked direction+  , 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 dynamic 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 a 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 Box2D. 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 collisions 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 b2BodyDef}+  alignment _ = #{alignment b2BodyDef}+  peek p =+    BodyDef+      <$> #{peek b2BodyDef, type} p+      <*> #{peek b2BodyDef, position} p+      <*> #{peek b2BodyDef, rotation} p+      <*> #{peek b2BodyDef, linearVelocity} p+      <*> #{peek b2BodyDef, angularVelocity} p+      <*> #{peek b2BodyDef, linearDamping} p+      <*> #{peek b2BodyDef, angularDamping} p+      <*> #{peek b2BodyDef, gravityScale} p+      <*> #{peek b2BodyDef, sleepThreshold} p+      <*> #{peek b2BodyDef, safetyFactor} p+      <*> #{peek b2BodyDef, name} p+      <*> #{peek b2BodyDef, userData} p+      <*> #{peek b2BodyDef, motionLocks} p+      <*> #{peek b2BodyDef, enableSleep} p+      <*> #{peek b2BodyDef, isAwake} p+      <*> #{peek b2BodyDef, isBullet} p+      <*> #{peek b2BodyDef, isEnabled} p+      <*> #{peek b2BodyDef, allowFastRotation} p+      <*> #{peek b2BodyDef, enableContactRecycling} p+      <*> #{peek b2BodyDef, internalValue} p+  poke p (BodyDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14 f15 f16 f17 f18 f19 f20) = do+    #{poke b2BodyDef, type} p f1+    #{poke b2BodyDef, position} p f2+    #{poke b2BodyDef, rotation} p f3+    #{poke b2BodyDef, linearVelocity} p f4+    #{poke b2BodyDef, angularVelocity} p f5+    #{poke b2BodyDef, linearDamping} p f6+    #{poke b2BodyDef, angularDamping} p f7+    #{poke b2BodyDef, gravityScale} p f8+    #{poke b2BodyDef, sleepThreshold} p f9+    #{poke b2BodyDef, safetyFactor} p f10+    #{poke b2BodyDef, name} p f11+    #{poke b2BodyDef, userData} p f12+    #{poke b2BodyDef, motionLocks} p f13+    #{poke b2BodyDef, enableSleep} p f14+    #{poke b2BodyDef, isAwake} p f15+    #{poke b2BodyDef, isBullet} p f16+    #{poke b2BodyDef, isEnabled} p f17+    #{poke b2BodyDef, allowFastRotation} p f18+    #{poke b2BodyDef, enableContactRecycling} p f19+    #{poke b2BodyDef, internalValue} p f20++-- | 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+  , angularZ :: CBool+  -- ^ Prevent rotation around the z-axis+  }+  deriving (Eq, Show)++instance Storable MotionLocks where+  sizeOf _ = #{size b2MotionLocks}+  alignment _ = #{alignment b2MotionLocks}+  peek p =+    MotionLocks+      <$> #{peek b2MotionLocks, linearX} p+      <*> #{peek b2MotionLocks, linearY} p+      <*> #{peek b2MotionLocks, angularZ} p+  poke p (MotionLocks f1 f2 f3) = do+    #{poke b2MotionLocks, linearX} p f1+    #{poke b2MotionLocks, linearY} p f2+    #{poke b2MotionLocks, angularZ} p f3++{- | 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 b2_staticBody}+pattern KinematicBody = BodyType #{const b2_kinematicBody}+pattern DynamicBody = BodyType #{const b2_dynamicBody}++foreign import ccall unsafe "hs_b2DefaultBodyDef"+  c_b2DefaultBodyDef :: Ptr BodyDef -> IO ()++defaultBodyDef :: IO BodyDef+defaultBodyDef = allocaOut c_b2DefaultBodyDef+++foreign import ccall unsafe "b2CreateBody"+  c_b2CreateBody :: WorldId -> Ptr BodyDef -> IO BodyId++foreign import ccall unsafe "b2DestroyBody"+  c_b2DestroyBody :: BodyId -> IO ()++foreign import ccall unsafe "b2Body_IsValid"+  c_b2Body_IsValid :: BodyId -> IO CBool++foreign import ccall unsafe "b2Body_GetType"+  c_b2Body_GetType :: BodyId -> IO CInt++foreign import ccall unsafe "b2Body_SetType"+  c_b2Body_SetType :: BodyId -> CInt -> IO ()++foreign import ccall unsafe "b2Body_SetName"+  c_b2Body_SetName :: BodyId -> CString -> IO ()++foreign import ccall unsafe "b2Body_GetName"+  c_b2Body_GetName :: BodyId -> IO CString++foreign import ccall unsafe "b2Body_SetUserData"+  c_b2Body_SetUserData :: BodyId -> Ptr () -> IO ()++foreign import ccall unsafe "b2Body_GetUserData"+  c_b2Body_GetUserData :: BodyId -> IO (Ptr ())++foreign import ccall unsafe "hsg_b2Body_GetPosition"+  c_b2Body_GetPosition :: BodyId -> Ptr Pos -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetRotation"+  c_b2Body_GetRotation :: BodyId -> Ptr Rot -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetTransform"+  c_b2Body_GetTransform :: BodyId -> Ptr WorldTransform -> IO ()++foreign import ccall unsafe "hsg_b2Body_SetTransform"+  c_b2Body_SetTransform :: BodyId -> Ptr Pos -> Ptr Rot -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetLocalPoint"+  c_b2Body_GetLocalPoint :: BodyId -> Ptr Pos -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetWorldPoint"+  c_b2Body_GetWorldPoint :: BodyId -> Ptr Vec2 -> Ptr Pos -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetLocalVector"+  c_b2Body_GetLocalVector :: BodyId -> Ptr Vec2 -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetWorldVector"+  c_b2Body_GetWorldVector :: BodyId -> Ptr Vec2 -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetLinearVelocity"+  c_b2Body_GetLinearVelocity :: BodyId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "b2Body_GetAngularVelocity"+  c_b2Body_GetAngularVelocity :: BodyId -> IO Float++foreign import ccall unsafe "hsg_b2Body_SetLinearVelocity"+  c_b2Body_SetLinearVelocity :: BodyId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "b2Body_SetAngularVelocity"+  c_b2Body_SetAngularVelocity :: BodyId -> Float -> IO ()++foreign import ccall unsafe "hsg_b2Body_SetTargetTransform"+  c_b2Body_SetTargetTransform :: BodyId -> Ptr WorldTransform -> Float -> CBool -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetLocalPointVelocity"+  c_b2Body_GetLocalPointVelocity :: BodyId -> Ptr Vec2 -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetWorldPointVelocity"+  c_b2Body_GetWorldPointVelocity :: BodyId -> Ptr Pos -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2Body_ApplyForce"+  c_b2Body_ApplyForce :: BodyId -> Ptr Vec2 -> Ptr Pos -> CBool -> IO ()++foreign import ccall unsafe "hsg_b2Body_ApplyForceToCenter"+  c_b2Body_ApplyForceToCenter :: BodyId -> Ptr Vec2 -> CBool -> IO ()++foreign import ccall unsafe "b2Body_ApplyTorque"+  c_b2Body_ApplyTorque :: BodyId -> Float -> CBool -> IO ()++foreign import ccall unsafe "b2Body_ClearForces"+  c_b2Body_ClearForces :: BodyId -> IO ()++foreign import ccall unsafe "hsg_b2Body_ApplyLinearImpulse"+  c_b2Body_ApplyLinearImpulse :: BodyId -> Ptr Vec2 -> Ptr Pos -> CBool -> IO ()++foreign import ccall unsafe "hsg_b2Body_ApplyLinearImpulseToCenter"+  c_b2Body_ApplyLinearImpulseToCenter :: BodyId -> Ptr Vec2 -> CBool -> IO ()++foreign import ccall unsafe "b2Body_ApplyAngularImpulse"+  c_b2Body_ApplyAngularImpulse :: BodyId -> Float -> CBool -> IO ()++foreign import ccall unsafe "b2Body_GetMass"+  c_b2Body_GetMass :: BodyId -> IO Float++foreign import ccall unsafe "b2Body_GetRotationalInertia"+  c_b2Body_GetRotationalInertia :: BodyId -> IO Float++foreign import ccall unsafe "hsg_b2Body_GetLocalCenter"+  c_b2Body_GetLocalCenter :: BodyId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetWorldCenter"+  c_b2Body_GetWorldCenter :: BodyId -> Ptr Pos -> IO ()++foreign import ccall unsafe "hsg_b2Body_SetMassData"+  c_b2Body_SetMassData :: BodyId -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetMassData"+  c_b2Body_GetMassData :: BodyId -> Ptr MassData -> IO ()++foreign import ccall unsafe "b2Body_UpdateMassFromShapes"+  c_b2Body_UpdateMassFromShapes :: BodyId -> IO ()++foreign import ccall unsafe "b2Body_SetLinearDamping"+  c_b2Body_SetLinearDamping :: BodyId -> Float -> IO ()++foreign import ccall unsafe "b2Body_GetLinearDamping"+  c_b2Body_GetLinearDamping :: BodyId -> IO Float++foreign import ccall unsafe "b2Body_SetAngularDamping"+  c_b2Body_SetAngularDamping :: BodyId -> Float -> IO ()++foreign import ccall unsafe "b2Body_GetAngularDamping"+  c_b2Body_GetAngularDamping :: BodyId -> IO Float++foreign import ccall unsafe "b2Body_SetGravityScale"+  c_b2Body_SetGravityScale :: BodyId -> Float -> IO ()++foreign import ccall unsafe "b2Body_GetGravityScale"+  c_b2Body_GetGravityScale :: BodyId -> IO Float++foreign import ccall unsafe "b2Body_IsAwake"+  c_b2Body_IsAwake :: BodyId -> IO CBool++foreign import ccall unsafe "b2Body_SetAwake"+  c_b2Body_SetAwake :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b2Body_WakeTouching"+  c_b2Body_WakeTouching :: BodyId -> IO ()++foreign import ccall unsafe "b2Body_EnableSleeping"+  c_b2Body_EnableSleeping :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b2Body_IsSleepingEnabled"+  c_b2Body_IsSleepingEnabled :: BodyId -> IO CBool++foreign import ccall unsafe "b2Body_SetSleepThreshold"+  c_b2Body_SetSleepThreshold :: BodyId -> Float -> IO ()++foreign import ccall unsafe "b2Body_GetSleepThreshold"+  c_b2Body_GetSleepThreshold :: BodyId -> IO Float++foreign import ccall unsafe "b2Body_SetSafetyFactor"+  c_b2Body_SetSafetyFactor :: BodyId -> Float -> IO ()++foreign import ccall unsafe "b2Body_GetSafetyFactor"+  c_b2Body_GetSafetyFactor :: BodyId -> IO Float++foreign import ccall unsafe "b2Body_IsEnabled"+  c_b2Body_IsEnabled :: BodyId -> IO CBool++foreign import ccall unsafe "b2Body_Disable"+  c_b2Body_Disable :: BodyId -> IO ()++foreign import ccall unsafe "b2Body_Enable"+  c_b2Body_Enable :: BodyId -> IO ()++foreign import ccall unsafe "hsg_b2Body_SetMotionLocks"+  c_b2Body_SetMotionLocks :: BodyId -> Ptr MotionLocks -> IO ()++foreign import ccall unsafe "hsg_b2Body_GetMotionLocks"+  c_b2Body_GetMotionLocks :: BodyId -> Ptr MotionLocks -> IO ()++foreign import ccall unsafe "b2Body_SetBullet"+  c_b2Body_SetBullet :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b2Body_IsBullet"+  c_b2Body_IsBullet :: BodyId -> IO CBool++foreign import ccall unsafe "b2Body_EnableContactRecycling"+  c_b2Body_EnableContactRecycling :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b2Body_IsContactRecyclingEnabled"+  c_b2Body_IsContactRecyclingEnabled :: BodyId -> IO CBool++foreign import ccall unsafe "b2Body_EnableContactEvents"+  c_b2Body_EnableContactEvents :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b2Body_EnableHitEvents"+  c_b2Body_EnableHitEvents :: BodyId -> CBool -> IO ()++foreign import ccall unsafe "b2Body_GetWorld"+  c_b2Body_GetWorld :: BodyId -> IO WorldId++foreign import ccall unsafe "b2Body_GetShapeCount"+  c_b2Body_GetShapeCount :: BodyId -> IO CInt++foreign import ccall unsafe "b2Body_GetShapes"+  c_b2Body_GetShapes :: BodyId -> Ptr ShapeId -> CInt -> IO CInt++foreign import ccall unsafe "b2Body_GetJointCount"+  c_b2Body_GetJointCount :: BodyId -> IO CInt++foreign import ccall unsafe "b2Body_GetJoints"+  c_b2Body_GetJoints :: BodyId -> Ptr JointId -> CInt -> IO CInt++foreign import ccall unsafe "b2Body_GetContactCapacity"+  c_b2Body_GetContactCapacity :: BodyId -> IO CInt++foreign import ccall unsafe "b2Body_GetContactData"+  c_b2Body_GetContactData :: BodyId -> Ptr ContactData -> CInt -> IO CInt++foreign import ccall unsafe "hsg_b2Body_ComputeBounds"+  c_b2Body_ComputeBounds :: BodyId -> Ptr AABB -> IO ()++foreign import ccall unsafe "b2Body_GetMinExtent"+  c_b2Body_GetMinExtent :: BodyId -> IO Float++foreign import ccall unsafe "b2Body_GetMaxExtent"+  c_b2Body_GetMaxExtent :: BodyId -> IO Float++foreign import ccall unsafe "b2Body_GetMaxExtentFromOrigin"+  c_b2Body_GetMaxExtentFromOrigin :: BodyId -> IO Float++{- | 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.++> b2BodyDef bodyDef = b2DefaultBodyDef();+> b2BodyId myBodyId = b2CreateBody(myWorldId, &bodyDef);++__Warning:__ This function is locked during callbacks.++Binds @b2CreateBody@.+-}+create+  :: WorldId+  -> BodyDef+  -> IO BodyId+create a0 a1 =+  with a1 $ \p1 ->+  c_b2CreateBody 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 @b2DestroyBody@.+-}+destroy+  :: BodyId+  -> IO ()+destroy a0 =+  c_b2DestroyBody 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 @b2Body_IsValid@.+-}+isValid+  :: BodyId+  -> IO Bool+isValid a0 =+  toBool <$> (c_b2Body_IsValid a0)++{- | Get the body type: static, kinematic, or dynamic++Binds @b2Body_GetType@.+-}+getType+  :: BodyId+  -> IO BodyType+getType a0 =+  coerce <$> (c_b2Body_GetType a0)++{- | Change the body type. This is an expensive operation. This automatically updates the mass+properties regardless of the automatic mass setting.++Binds @b2Body_SetType@.+-}+setType+  :: BodyId+  -> BodyType+  -> IO ()+setType a0 a1 =+  c_b2Body_SetType a0 (coerce a1)++{- | Set the body name. See @B2_NAME_LENGTH@.++Binds @b2Body_SetName@.+-}+setName+  :: BodyId+  -> CString+  -- ^ @name@+  -> IO ()+setName a0 a1 =+  c_b2Body_SetName a0 a1++{- | Get the body name.++Binds @b2Body_GetName@.+-}+getName+  :: BodyId+  -> IO CString+getName a0 =+  c_b2Body_GetName a0++{- | Set the user data for a body++Binds @b2Body_SetUserData@.+-}+setUserData+  :: BodyId+  -> Ptr ()+  -- ^ @userData@+  -> IO ()+setUserData a0 a1 =+  c_b2Body_SetUserData a0 a1++{- | Get the user data stored in a body++Binds @b2Body_GetUserData@.+-}+getUserData+  :: BodyId+  -> IO (Ptr ())+getUserData a0 =+  c_b2Body_GetUserData a0++{- | Get the world position of a body. This is the location of the body origin.++Binds @b2Body_GetPosition@.+-}+getPosition+  :: BodyId+  -> IO Pos+getPosition a0 =+  alloca $ \pOut -> c_b2Body_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_b2Body_GetPosition a0 out++{- | Get the world rotation of a body as a cosine\/sine pair (complex number)++Binds @b2Body_GetRotation@.+-}+getRotation+  :: BodyId+  -> IO Rot+getRotation a0 =+  alloca $ \pOut -> c_b2Body_GetRotation a0 pOut >> peek pOut++-- | Like 'getRotation', but the result is written into a caller-supplied buffer.+getRotationInto+  :: BodyId+  -> Ptr Rot+  -- ^ Result buffer.+  -> IO ()+getRotationInto a0 out =+  c_b2Body_GetRotation a0 out++{- | Get the world transform of a body.++Binds @b2Body_GetTransform@.+-}+getTransform+  :: BodyId+  -> IO WorldTransform+getTransform a0 =+  alloca $ \pOut -> c_b2Body_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_b2Body_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 @b2BodyDef::position@ and @b2BodyDef::rotation@.++Binds @b2Body_SetTransform@.+-}+setTransform+  :: BodyId+  -> Pos+  -- ^ @position@+  -> Rot+  -- ^ @rotation@+  -> IO ()+setTransform a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2Body_SetTransform a0 p1 p2++{- | Get a local point on a body given a world point++Binds @b2Body_GetLocalPoint@.+-}+getLocalPoint+  :: BodyId+  -> Pos+  -- ^ @worldPoint@+  -> IO Vec2+getLocalPoint a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2Body_GetLocalPoint a0 p1 pOut >> peek pOut++{- | Get a world point on a body given a local point++Binds @b2Body_GetWorldPoint@.+-}+getWorldPoint+  :: BodyId+  -> Vec2+  -- ^ @localPoint@+  -> IO Pos+getWorldPoint a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2Body_GetWorldPoint a0 p1 pOut >> peek pOut++{- | Get a local vector on a body given a world vector++Binds @b2Body_GetLocalVector@.+-}+getLocalVector+  :: BodyId+  -> Vec2+  -- ^ @worldVector@+  -> IO Vec2+getLocalVector a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2Body_GetLocalVector a0 p1 pOut >> peek pOut++{- | Get a world vector on a body given a local vector++Binds @b2Body_GetWorldVector@.+-}+getWorldVector+  :: BodyId+  -> Vec2+  -- ^ @localVector@+  -> IO Vec2+getWorldVector a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2Body_GetWorldVector a0 p1 pOut >> peek pOut++{- | Get the linear velocity of a body\'s center of mass. Usually in meters per second.++Binds @b2Body_GetLinearVelocity@.+-}+getLinearVelocity+  :: BodyId+  -> IO Vec2+getLinearVelocity a0 =+  alloca $ \pOut -> c_b2Body_GetLinearVelocity a0 pOut >> peek pOut++-- | Like 'getLinearVelocity', but the result is written into a caller-supplied buffer.+getLinearVelocityInto+  :: BodyId+  -> Ptr Vec2+  -- ^ Result buffer.+  -> IO ()+getLinearVelocityInto a0 out =+  c_b2Body_GetLinearVelocity a0 out++{- | Get the angular velocity of a body in radians per second++Binds @b2Body_GetAngularVelocity@.+-}+getAngularVelocity+  :: BodyId+  -> IO Float+getAngularVelocity a0 =+  c_b2Body_GetAngularVelocity a0++{- | Set the linear velocity of a body. Usually in meters per second.++Binds @b2Body_SetLinearVelocity@.+-}+setLinearVelocity+  :: BodyId+  -> Vec2+  -- ^ @linearVelocity@+  -> IO ()+setLinearVelocity a0 a1 =+  with a1 $ \p1 ->+  c_b2Body_SetLinearVelocity a0 p1++{- | Set the angular velocity of a body in radians per second++Binds @b2Body_SetAngularVelocity@.+-}+setAngularVelocity+  :: BodyId+  -> Float+  -- ^ @angularVelocity@+  -> IO ()+setAngularVelocity a0 a1 =+  c_b2Body_SetAngularVelocity a0 a1++{- | Set the velocity to reach the given transform after a given time step. The result will be close+but may not be exact. This is meant for kinematic bodies. The target is not applied if the+velocity would be below the sleep threshold and the body is currently asleep.++Binds @b2Body_SetTargetTransform@.+-}+setTargetTransform+  :: BodyId+  -- ^ @bodyId@: The body id+  -> WorldTransform+  -- ^ @target@: The target transform for the body+  -> Float+  -- ^ @timeStep@: The time step of the next call to @b2World_Step@+  -> Bool+  -- ^ @wake@: Option to wake the body or not+  -> IO ()+setTargetTransform a0 a1 a2 a3 =+  with a1 $ \p1 ->+  c_b2Body_SetTargetTransform a0 p1 a2 (fromBool a3)++{- | Get the linear velocity of a local point attached to a body. Usually in meters per second.++Binds @b2Body_GetLocalPointVelocity@.+-}+getLocalPointVelocity+  :: BodyId+  -> Vec2+  -- ^ @localPoint@+  -> IO Vec2+getLocalPointVelocity a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2Body_GetLocalPointVelocity a0 p1 pOut >> peek pOut++{- | Get the linear velocity of a world point attached to a body. Usually in meters per second.++Binds @b2Body_GetWorldPointVelocity@.+-}+getWorldPointVelocity+  :: BodyId+  -> Pos+  -- ^ @worldPoint@+  -> IO Vec2+getWorldPointVelocity a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2Body_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 @b2Body_ApplyForce@.+-}+applyForce+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec2+  -- ^ @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_b2Body_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 @b2Body_ApplyForceToCenter@.+-}+applyForceToCenter+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec2+  -- ^ @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_b2Body_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 @b2Body_ApplyTorque@.+-}+applyTorque+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Float+  -- ^ @torque@: about the z-axis (out of the screen), usually in N*m.+  -> Bool+  -- ^ @wake@: also wake up the body+  -> IO ()+applyTorque a0 a1 a2 =+  c_b2Body_ApplyTorque a0 a1 (fromBool a2)++{- | Clear the force and torque on this body. Forces and torques are automatically cleared after each+world step. So this only needs to be called if the application wants to remove the effect of+previous calls to apply forces and torques before the world step is called.++Binds @b2Body_ClearForces@.+-}+clearForces+  :: BodyId+  -- ^ @bodyId@: The body id+  -> IO ()+clearForces a0 =+  c_b2Body_ClearForces a0++{- | 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 @b2Body_ApplyLinearImpulse@.+-}+applyLinearImpulse+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec2+  -- ^ @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_b2Body_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 @b2Body_ApplyLinearImpulseToCenter@.+-}+applyLinearImpulseToCenter+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Vec2+  -- ^ @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_b2Body_ApplyLinearImpulseToCenter a0 p1 (fromBool a2)++{- | Apply an angular impulse. 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 @b2Body_ApplyAngularImpulse@.+-}+applyAngularImpulse+  :: BodyId+  -- ^ @bodyId@: The body id+  -> Float+  -- ^ @impulse@: the angular impulse, usually in units of kg*m*m\/s+  -> Bool+  -- ^ @wake@: also wake up the body+  -> IO ()+applyAngularImpulse a0 a1 a2 =+  c_b2Body_ApplyAngularImpulse a0 a1 (fromBool a2)++{- | Get the mass of the body, usually in kilograms++Binds @b2Body_GetMass@.+-}+getMass+  :: BodyId+  -> IO Float+getMass a0 =+  c_b2Body_GetMass a0++{- | Get the rotational inertia of the body, usually in kg*m^2++Binds @b2Body_GetRotationalInertia@.+-}+getRotationalInertia+  :: BodyId+  -> IO Float+getRotationalInertia a0 =+  c_b2Body_GetRotationalInertia a0++{- | Get the center of mass position of the body in local space.++Binds @b2Body_GetLocalCenter@.+-}+getLocalCenter+  :: BodyId+  -> IO Vec2+getLocalCenter a0 =+  alloca $ \pOut -> c_b2Body_GetLocalCenter a0 pOut >> peek pOut++-- | Like 'getLocalCenter', but the result is written into a caller-supplied buffer.+getLocalCenterInto+  :: BodyId+  -> Ptr Vec2+  -- ^ Result buffer.+  -> IO ()+getLocalCenterInto a0 out =+  c_b2Body_GetLocalCenter a0 out++{- | Get the center of mass position of the body in world space.++Binds @b2Body_GetWorldCenter@.+-}+getWorldCenter+  :: BodyId+  -> IO Pos+getWorldCenter a0 =+  alloca $ \pOut -> c_b2Body_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_b2Body_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. If motion locks are used to fix rotation then the rotational inertia value is+ignored and rotational inertia remains fixed at zero.++Binds @b2Body_SetMassData@.+-}+setMassData+  :: BodyId+  -> MassData+  -> IO ()+setMassData a0 a1 =+  with a1 $ \p1 ->+  c_b2Body_SetMassData a0 p1++{- | Get the mass data for a body++Binds @b2Body_GetMassData@.+-}+getMassData+  :: BodyId+  -> IO MassData+getMassData a0 =+  alloca $ \pOut -> c_b2Body_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_b2Body_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 @b2Body_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. Note that sensor shapes may have mass.++Binds @b2Body_UpdateMassFromShapes@.+-}+updateMassFromShapes+  :: BodyId+  -> IO ()+updateMassFromShapes a0 =+  c_b2Body_UpdateMassFromShapes a0++{- | Adjust the linear damping. Normally this is set in @b2BodyDef@ before creation.++Binds @b2Body_SetLinearDamping@.+-}+setLinearDamping+  :: BodyId+  -> Float+  -- ^ @linearDamping@+  -> IO ()+setLinearDamping a0 a1 =+  c_b2Body_SetLinearDamping a0 a1++{- | Get the current linear damping.++Binds @b2Body_GetLinearDamping@.+-}+getLinearDamping+  :: BodyId+  -> IO Float+getLinearDamping a0 =+  c_b2Body_GetLinearDamping a0++{- | Adjust the angular damping. Normally this is set in @b2BodyDef@ before creation.++Binds @b2Body_SetAngularDamping@.+-}+setAngularDamping+  :: BodyId+  -> Float+  -- ^ @angularDamping@+  -> IO ()+setAngularDamping a0 a1 =+  c_b2Body_SetAngularDamping a0 a1++{- | Get the current angular damping.++Binds @b2Body_GetAngularDamping@.+-}+getAngularDamping+  :: BodyId+  -> IO Float+getAngularDamping a0 =+  c_b2Body_GetAngularDamping a0++{- | Adjust the gravity scale. Normally this is set in @b2BodyDef@ before creation.++See also @b2BodyDef::gravityScale@.++Binds @b2Body_SetGravityScale@.+-}+setGravityScale+  :: BodyId+  -> Float+  -- ^ @gravityScale@+  -> IO ()+setGravityScale a0 a1 =+  c_b2Body_SetGravityScale a0 a1++{- | Get the current gravity scale++Binds @b2Body_GetGravityScale@.+-}+getGravityScale+  :: BodyId+  -> IO Float+getGravityScale a0 =+  c_b2Body_GetGravityScale a0++{- | Returns: true if this body is awake++Binds @b2Body_IsAwake@.+-}+isAwake+  :: BodyId+  -> IO Bool+isAwake a0 =+  toBool <$> (c_b2Body_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 @b2Body_SetAwake@.+-}+setAwake+  :: BodyId+  -> Bool+  -- ^ @awake@+  -> IO ()+setAwake a0 a1 =+  c_b2Body_SetAwake a0 (fromBool a1)++{- | Wake bodies touching this body. Works for static bodies.++Binds @b2Body_WakeTouching@.+-}+wakeTouching+  :: BodyId+  -> IO ()+wakeTouching a0 =+  c_b2Body_WakeTouching a0++{- | Enable or disable sleeping for this body. If sleeping is disabled the body will wake (and the+entire island).++Binds @b2Body_EnableSleeping@.+-}+enableSleeping+  :: BodyId+  -> Bool+  -- ^ @enableSleep@+  -> IO ()+enableSleeping a0 a1 =+  c_b2Body_EnableSleeping a0 (fromBool a1)++{- | Returns true if sleeping is enabled for this body++Binds @b2Body_IsSleepingEnabled@.+-}+isSleepingEnabled+  :: BodyId+  -> IO Bool+isSleepingEnabled a0 =+  toBool <$> (c_b2Body_IsSleepingEnabled a0)++{- | Set the sleep threshold, usually in meters per second++Binds @b2Body_SetSleepThreshold@.+-}+setSleepThreshold+  :: BodyId+  -> Float+  -- ^ @sleepThreshold@+  -> IO ()+setSleepThreshold a0 a1 =+  c_b2Body_SetSleepThreshold a0 a1++{- | Get the sleep threshold, usually in meters per second.++Binds @b2Body_GetSleepThreshold@.+-}+getSleepThreshold+  :: BodyId+  -> IO Float+getSleepThreshold a0 =+  c_b2Body_GetSleepThreshold a0++{- | Set the continuous collision safety factor. Smaller is safer but can lead to hitching.+Recommended range \[0.01, 0.5\]. Non-dimensional.++Binds @b2Body_SetSafetyFactor@.+-}+setSafetyFactor+  :: BodyId+  -> Float+  -- ^ @safetyFactor@+  -> IO ()+setSafetyFactor a0 a1 =+  c_b2Body_SetSafetyFactor a0 a1++{- | Get the continuous collision safety factor. Non-dimensional.++Binds @b2Body_GetSafetyFactor@.+-}+getSafetyFactor+  :: BodyId+  -> IO Float+getSafetyFactor a0 =+  c_b2Body_GetSafetyFactor a0++{- | Returns true if this body is enabled++Binds @b2Body_IsEnabled@.+-}+isEnabled+  :: BodyId+  -> IO Bool+isEnabled a0 =+  toBool <$> (c_b2Body_IsEnabled a0)++{- | Disable a body by removing it completely from the simulation. This is expensive.++Binds @b2Body_Disable@.+-}+disable+  :: BodyId+  -> IO ()+disable a0 =+  c_b2Body_Disable a0++{- | Enable a body by adding it to the simulation. This is expensive.++Binds @b2Body_Enable@.+-}+enable+  :: BodyId+  -> IO ()+enable a0 =+  c_b2Body_Enable a0++{- | Set the motion locks on this body. This causes mass properties to be recomputed because fixed+rotation sets the rotational inertia to zero.++Binds @b2Body_SetMotionLocks@.+-}+setMotionLocks+  :: BodyId+  -> MotionLocks+  -> IO ()+setMotionLocks a0 a1 =+  with a1 $ \p1 ->+  c_b2Body_SetMotionLocks a0 p1++{- | Get the motion locks for this body.++Binds @b2Body_GetMotionLocks@.+-}+getMotionLocks+  :: BodyId+  -> IO MotionLocks+getMotionLocks a0 =+  alloca $ \pOut -> c_b2Body_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_b2Body_GetMotionLocks a0 out++{- | Set this body to be a bullet. A bullet does continuous collision detection against dynamic+and kinematic bodies (but not other bullets). This only works on dynamic bodies.++Binds @b2Body_SetBullet@.+-}+setBullet+  :: BodyId+  -> Bool+  -- ^ @flag@+  -> IO ()+setBullet a0 a1 =+  c_b2Body_SetBullet a0 (fromBool a1)++{- | Is this body a bullet?++Binds @b2Body_IsBullet@.+-}+isBullet+  :: BodyId+  -> IO Bool+isBullet a0 =+  toBool <$> (c_b2Body_IsBullet 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 @b2BodyDef::enableContactRecycling@.++Binds @b2Body_EnableContactRecycling@.+-}+enableContactRecycling+  :: BodyId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableContactRecycling a0 a1 =+  c_b2Body_EnableContactRecycling a0 (fromBool a1)++{- | Is contact recycling enabled on this body?++Binds @b2Body_IsContactRecyclingEnabled@.+-}+isContactRecyclingEnabled+  :: BodyId+  -> IO Bool+isContactRecyclingEnabled a0 =+  toBool <$> (c_b2Body_IsContactRecyclingEnabled a0)++{- | Enable\/disable contact events on all shapes.++See also @b2ShapeDef::enableContactEvents@.++__Warning:__ changing this at runtime may cause mismatched begin\/end touch events++Binds @b2Body_EnableContactEvents@.+-}+enableContactEvents+  :: BodyId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableContactEvents a0 a1 =+  c_b2Body_EnableContactEvents a0 (fromBool a1)++{- | Enable\/disable hit events on all shapes++See also @b2ShapeDef::enableHitEvents@.++Binds @b2Body_EnableHitEvents@.+-}+enableHitEvents+  :: BodyId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableHitEvents a0 a1 =+  c_b2Body_EnableHitEvents a0 (fromBool a1)++{- | Get the world that owns this body++Binds @b2Body_GetWorld@.+-}+getWorld+  :: BodyId+  -> IO WorldId+getWorld a0 =+  c_b2Body_GetWorld a0++{- | Get the number of shapes on this body++Binds @b2Body_GetShapeCount@.+-}+getShapeCount+  :: BodyId+  -> IO Int+getShapeCount a0 =+  fromIntegral <$> (c_b2Body_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 @b2Body_GetShapes@.+-}+getShapes+  :: BodyId+  -> Ptr ShapeId+  -- ^ @shapeArray@+  -> Int+  -- ^ @capacity@+  -> IO Int+getShapes a0 a1 a2 =+  fromIntegral <$> (c_b2Body_GetShapes a0 a1 (fromIntegral a2))++{- | Get the number of joints on this body++Binds @b2Body_GetJointCount@.+-}+getJointCount+  :: BodyId+  -> IO Int+getJointCount a0 =+  fromIntegral <$> (c_b2Body_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 @b2Body_GetJoints@.+-}+getJoints+  :: BodyId+  -> Ptr JointId+  -- ^ @jointArray@+  -> Int+  -- ^ @capacity@+  -> IO Int+getJoints a0 a1 a2 =+  fromIntegral <$> (c_b2Body_GetJoints a0 a1 (fromIntegral a2))++{- | Get the maximum capacity required for retrieving all the touching contacts on a body++Binds @b2Body_GetContactCapacity@.+-}+getContactCapacity+  :: BodyId+  -> IO Int+getContactCapacity a0 =+  fromIntegral <$> (c_b2Body_GetContactCapacity a0)++{- | Get the touching contact data for a body.++__Note:__ Box2D 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 @b2Body_GetContactData@.+-}+getContactData+  :: BodyId+  -> Ptr ContactData+  -> Int+  -- ^ @capacity@+  -> IO Int+getContactData a0 a1 a2 =+  fromIntegral <$> (c_b2Body_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 @b2Body_ComputeBounds@.+-}+computeBounds+  :: BodyId+  -> IO AABB+computeBounds a0 =+  alloca $ \pOut -> c_b2Body_ComputeBounds a0 pOut >> peek pOut++-- | Like 'computeBounds', but the result is written into a caller-supplied buffer.+computeBoundsInto+  :: BodyId+  -> Ptr AABB+  -- ^ Result buffer.+  -> IO ()+computeBoundsInto a0 out =+  c_b2Body_ComputeBounds a0 out++{- | The minimum distance from any point on the body shapes to the center of mass.++Binds @b2Body_GetMinExtent@.+-}+getMinExtent+  :: BodyId+  -> IO Float+getMinExtent a0 =+  c_b2Body_GetMinExtent a0++{- | The maximum distance from any point on the body shapes to the center of mass.++Binds @b2Body_GetMaxExtent@.+-}+getMaxExtent+  :: BodyId+  -> IO Float+getMaxExtent a0 =+  c_b2Body_GetMaxExtent a0++{- | The maximum distance from any point on the body shapes to the body origin. Conservative.++Binds @b2Body_GetMaxExtentFromOrigin@.+-}+getMaxExtentFromOrigin+  :: BodyId+  -> IO Float+getMaxExtentFromOrigin a0 =+  c_b2Body_GetMaxExtentFromOrigin a0
+ src/Box2D/Callbacks.hs view
@@ -0,0 +1,739 @@+{-# 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+"Box2D.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++Box2D 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 Box2D.Callbacks+  ( -- * Leaf callbacks+    CustomFilter+  , mkCustomFilterFcn+  , withCustomFilterFcn+  , Friction+  , mkFrictionCallback+  , withFrictionCallback+  , Restitution+  , mkRestitutionCallback+  , withRestitutionCallback++    -- * Query and cast visitors+  , OverlapResult+  , withOverlapResultFcn+  , CastResult+  , withCastResultFcn+  , PreSolve+  , PreContinuous+  , withPreSolveFcns+  , PlaneResultVisitor+  , withPlaneResultFcn++    -- * Dynamic-tree visitors+  , TreeQuery+  , withTreeQueryFcn+  , TreeRayCast+  , withTreeRayCastFcn+  , TreeBoxCast+  , withTreeBoxCastFcn++    -- * Debug draw+  , Draw (..)+  , noDraw+  , toWorld+  , DrawTarget+  , withDraw+  , draw+  , withDrawPtr+  , Cmd (..)+  , withRecorder++    -- * A multithreaded task system+  , TaskSystem (..)+  , withThreadPoolTaskSystem+  ) where++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 qualified as VS+import Foreign+import Foreign.C.String (CString)+import Foreign.C.Types (CBool (..), CInt (..))+import GHC.Foreign qualified+import GHC.IO.Encoding.Failure (CodingFailureMode (..))+import GHC.IO.Encoding.UTF8 (mkUTF8)++import Box2D.Collision (Manifold, PlaneResult)+import Box2D.DebugDraw (DebugDraw (..), HexColor (..))+import Box2D.DynamicTree (BoxCastInput)+import Box2D.Geometry (RayCastInput)+import Box2D.Id (ShapeId (..), WorldId)+import Box2D.Internal (copyVector)+import Box2D.Internal.DrawClosure qualified as DrawClosure+import Box2D.MathFunctions qualified as MathFunctions+import Box2D.MathTypes (AABB, Pos, Vec2, WorldTransform, transformWorldPoint)+import Box2D.Tags+  ( CastResultCallbackFcn+  , CustomFilterCallbackFcn+  , DrawBoundsFcn+  , DrawCircleFcn+  , DrawLineFcn+  , DrawPointFcn+  , DrawPolygonFcn+  , DrawSolidCapsuleFcn+  , DrawSolidCircleFcn+  , DrawSolidPolygonFcn+  , DrawStringFcn+  , DrawTransformFcn+  , EnqueueTaskCallbackFcn+  , FinishTaskCallbackFcn+  , FrictionCallbackFcn+  , OverlapResultCallbackFcn+  , PlaneResultCallbackFcn+  , PreContinuousCallbackFcn+  , PreSolveCallbackFcn+  , RestitutionCallbackFcn+  , TreeBoxCastCallbackFcn+  , TreeQueryCallbackFcn+  , TreeRayCastCallbackFcn+  )+import Box2D.World qualified as World++-- Leaf callbacks ----------------------------------------------------------++{- | Decide whether two shapes should collide (see+'Box2D.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 CustomFilterCallbackFcn)+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 CustomFilterCallbackFcn -> IO a) -> IO a+withCustomFilterFcn f = bracket (mkCustomFilterFcn f) freeHaskellFunPtr++{- | Combine the friction of two materials (see+'Box2D.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 FrictionCallbackFcn)+mkFrictionCallback f = castFunPtr <$> wrapFriction f++-- | Run an action with a friction callback that is freed on exit.+withFrictionCallback :: Friction -> (FunPtr FrictionCallbackFcn -> IO a) -> IO a+withFrictionCallback f = bracket (mkFrictionCallback f) freeHaskellFunPtr++{- | Combine the restitution of two materials (see+'Box2D.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 RestitutionCallbackFcn)+mkRestitutionCallback f = castFunPtr <$> wrapRestitution f++-- | Run an action with a restitution callback that is freed on exit.+withRestitutionCallback :: Restitution -> (FunPtr RestitutionCallbackFcn -> IO a) -> IO a+withRestitutionCallback f = bracket (mkRestitutionCallback f) freeHaskellFunPtr++-- Query and cast visitors --------------------------------------------------+--+-- The cast and pre-continuous callbacks receive b2Pos/b2Vec2 arguments *by value*,+-- which a GHC "wrapper" cannot accept. Each of those routes through a fixed C+-- trampoline (see box2d_shim.h) that takes the structs by value and forwards+-- pointers to the Haskell closure, carried in an hs_b2Closure 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 'Box2D.World.overlapAABB'); both are released on exit.+-}+withOverlapResultFcn :: OverlapResult -> (FunPtr OverlapResultCallbackFcn -> 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 and fraction. The returned fraction controls the+query: 0 terminates, the given fraction clips the ray, 1 continues.+-}+type CastResult = ShapeId -> Pos -> Vec2 -> Float -> IO Float++type CastResultC = ShapeId -> Ptr Pos -> Ptr Vec2 -> Float -> Ptr () -> IO Float++foreign import ccall "wrapper"+  wrapCastResult :: CastResultC -> IO (FunPtr CastResultC)++foreign import ccall unsafe "&hs_b2CastResultTrampoline"+  p_castResultTrampoline :: FunPtr CastResultCallbackFcn++{- | Run an action with a cast visitor and the context to pass alongside it+(e.g. to 'Box2D.World.castRay'); both are released on exit.+-}+withCastResultFcn :: CastResult -> (FunPtr CastResultCallbackFcn -> 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 _ctx = do+      point <- peek pPoint+      normal <- peek pNormal+      f sid point normal frac++{- | Manifold editor, called after a contact manifold is updated and before+it goes to the solver. The manifold may be modified in place (e.g. the+normal); poke it back with no points to disable the contact for this step.+Fired from solver worker threads when the world is multithreaded, so it must+be thread-safe. Requires @enablePreSolveEvents@ on the shapes.+-}+type PreSolve = ShapeId -> ShapeId -> Ptr Manifold -> IO ()++type PreSolveC = ShapeId -> ShapeId -> Ptr Manifold -> Ptr () -> IO ()++foreign import ccall "wrapper"+  wrapPreSolve :: PreSolveC -> IO (FunPtr PreSolveC)++{- | Time-of-impact gate, called when continuous collision computes an+impact point and normal: return 'False' to skip the event. Same threading+and shape-flag requirements as 'PreSolve'.+-}+type PreContinuous = ShapeId -> ShapeId -> Pos -> Vec2 -> IO Bool++type PreContinuousC = ShapeId -> ShapeId -> Ptr Pos -> Ptr Vec2 -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  wrapPreContinuous :: PreContinuousC -> IO (FunPtr PreContinuousC)++foreign import ccall unsafe "&hs_b2PreContinuousTrampoline"+  p_preContinuousTrampoline :: FunPtr PreContinuousCallbackFcn++{- | Run an action with the pre-solve and pre-continuous callbacks and the+context they share (to 'Box2D.World.setPreSolveCallback'). A missing side is+passed as 'nullFunPtr', which the engine skips. Unhook the callbacks from the+world before the action returns: all are released on exit.+-}+withPreSolveFcns+  :: Maybe PreSolve+  -> Maybe PreContinuous+  -> (FunPtr PreSolveCallbackFcn -> FunPtr PreContinuousCallbackFcn -> Ptr () -> IO a)+  -> IO a+withPreSolveFcns preSolve preContinuous act =+  withOptionalFunPtr (wrapPreSolve . ignoringContext) preSolve $ \solveFn ->+    case preContinuous of+      Nothing ->+        act (castFunPtr solveFn) nullFunPtr nullPtr+      Just f ->+        bracket (wrapPreContinuous $ gate f) freeHaskellFunPtr $ \hsFn ->+          withClosure (castFunPtr hsFn) $ act (castFunPtr solveFn) p_preContinuousTrampoline+  where+    ignoringContext f a b manifold _ctx = f a b manifold+    gate f a b pPoint pNormal _ctx = do+      point <- peek pPoint+      normal <- peek pNormal+      fromBool <$> f a b point normal++withOptionalFunPtr :: (hs -> IO (FunPtr c)) -> Maybe hs -> (FunPtr c -> IO a) -> IO a+withOptionalFunPtr _ Nothing act = act nullFunPtr+withOptionalFunPtr wrap (Just f) act = bracket (wrap f) freeHaskellFunPtr act++{- | Mover collision visitor: one collision plane per convex shape the mover+capsule touches (see 'Box2D.World.collideMover'); return 'True' to keep+gathering planes.+-}+type PlaneResultVisitor = ShapeId -> PlaneResult -> IO Bool++type PlaneResultC = ShapeId -> Ptr PlaneResult -> 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 'Box2D.World.collideMover'); both are released on exit.+-}+withPlaneResultFcn :: PlaneResultVisitor -> (FunPtr PlaneResultCallbackFcn -> Ptr () -> IO a) -> IO a+withPlaneResultFcn f act =+  bracket (wrapPlaneResult go) freeHaskellFunPtr $ \p ->+    act (castFunPtr p) nullPtr+  where+    go sid pPlane _ctx = do+      plane <- peek pPlane+      fromBool <$> f sid plane++-- 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 'Box2D.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++{- | 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 'Box2D.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' is+still an opaque tag with no 'Storable').+-}+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 'Box2D.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_b2Closure@ 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 ----------------------------------------------------------------+--+-- b2DebugDraw is a table of ten callbacks sharing a single context field, so+-- the slots cannot each carry their own hs_b2Closure. Every slot instead gets+-- a fixed C trampoline (see box2d_shim.h) reading its own function out of one+-- wider hs_b2DrawClosure, 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') except where a slot also takes+a 'WorldTransform', which hands over its 'Vec2' geometry in that frame;+'toWorld' converts.+-}+data Draw = Draw+  { polygon :: Maybe (WorldTransform -> VS.Vector Vec2 -> HexColor -> IO ())+  -- ^ Closed polygon, vertices in CCW order and in the transform's frame.+  , solidPolygon :: Maybe (WorldTransform -> VS.Vector Vec2 -> Float -> HexColor -> IO ())+  {- ^ Solid closed polygon in CCW order, rounded by a corner radius; the+  vertices are in the transform's frame.+  -}+  , circle :: Maybe (Pos -> Float -> HexColor -> IO ())+  , solidCircle :: Maybe (WorldTransform -> Vec2 -> Float -> HexColor -> IO ())+  -- ^ The center is in the transform's frame.+  , solidCapsule :: Maybe (Pos -> Pos -> Float -> HexColor -> IO ())+  , line :: Maybe (Pos -> Pos -> HexColor -> IO ())+  , transform :: Maybe (WorldTransform -> IO ())+  -- ^ Choose your own length scale.+  , point :: Maybe (Pos -> Float -> HexColor -> IO ())+  , string :: Maybe (Pos -> String -> HexColor -> IO ())+  -- ^ Decoded as UTF-8, substituting @U+FFFD@ for undecodable bytes.+  , bounds :: Maybe (AABB -> HexColor -> IO ())+  }++-- | Every slot unset.+noDraw :: Draw+noDraw =+  Draw+    { polygon = Nothing+    , solidPolygon = Nothing+    , circle = Nothing+    , solidCircle = Nothing+    , solidCapsule = Nothing+    , line = Nothing+    , transform = Nothing+    , point = Nothing+    , string = Nothing+    , bounds = Nothing+    }++{- | Move a 'Vec2' out of a 'WorldTransform'\'s frame into world coordinates,+for the slots that hand over local geometry.+-}+toWorld :: WorldTransform -> Vec2 -> Pos+toWorld = transformWorldPoint++type DrawPolygonC = Ptr WorldTransform -> Ptr Vec2 -> CInt -> HexColor -> Ptr () -> IO ()+type DrawSolidPolygonC = Ptr WorldTransform -> Ptr Vec2 -> CInt -> Float -> HexColor -> Ptr () -> IO ()+type DrawCircleC = Ptr Pos -> Float -> HexColor -> Ptr () -> IO ()+type DrawSolidCircleC = Ptr WorldTransform -> Ptr Vec2 -> Float -> HexColor -> Ptr () -> IO ()+type DrawSolidCapsuleC = Ptr Pos -> Ptr Pos -> Float -> HexColor -> Ptr () -> IO ()+type DrawLineC = Ptr Pos -> Ptr Pos -> HexColor -> Ptr () -> IO ()+type DrawTransformC = Ptr WorldTransform -> Ptr () -> IO ()+type DrawPointC = Ptr Pos -> Float -> HexColor -> Ptr () -> IO ()+type DrawStringC = Ptr Pos -> CString -> HexColor -> Ptr () -> IO ()+type DrawBoundsC = Ptr AABB -> HexColor -> Ptr () -> IO ()++foreign import ccall "wrapper" wrapDrawPolygon :: DrawPolygonC -> IO (FunPtr DrawPolygonC)+foreign import ccall "wrapper" wrapDrawSolidPolygon :: DrawSolidPolygonC -> IO (FunPtr DrawSolidPolygonC)+foreign import ccall "wrapper" wrapDrawCircle :: DrawCircleC -> IO (FunPtr DrawCircleC)+foreign import ccall "wrapper" wrapDrawSolidCircle :: DrawSolidCircleC -> IO (FunPtr DrawSolidCircleC)+foreign import ccall "wrapper" wrapDrawSolidCapsule :: DrawSolidCapsuleC -> IO (FunPtr DrawSolidCapsuleC)+foreign import ccall "wrapper" wrapDrawLine :: DrawLineC -> IO (FunPtr DrawLineC)+foreign import ccall "wrapper" wrapDrawTransform :: DrawTransformC -> IO (FunPtr DrawTransformC)+foreign import ccall "wrapper" wrapDrawPoint :: DrawPointC -> IO (FunPtr DrawPointC)+foreign import ccall "wrapper" wrapDrawString :: DrawStringC -> IO (FunPtr DrawStringC)+foreign import ccall "wrapper" wrapDrawBounds :: DrawBoundsC -> IO (FunPtr DrawBoundsC)++foreign import ccall unsafe "&hs_b2DrawPolygonTrampoline" p_drawPolygon :: FunPtr DrawPolygonFcn+foreign import ccall unsafe "&hs_b2DrawSolidPolygonTrampoline" p_drawSolidPolygon :: FunPtr DrawSolidPolygonFcn+foreign import ccall unsafe "&hs_b2DrawCircleTrampoline" p_drawCircle :: FunPtr DrawCircleFcn+foreign import ccall unsafe "&hs_b2DrawSolidCircleTrampoline" p_drawSolidCircle :: FunPtr DrawSolidCircleFcn+foreign import ccall unsafe "&hs_b2DrawSolidCapsuleTrampoline" p_drawSolidCapsule :: FunPtr DrawSolidCapsuleFcn+foreign import ccall unsafe "&hs_b2DrawLineTrampoline" p_drawLine :: FunPtr DrawLineFcn+foreign import ccall unsafe "&hs_b2DrawTransformTrampoline" p_drawTransform :: FunPtr DrawTransformFcn+foreign import ccall unsafe "&hs_b2DrawPointTrampoline" p_drawPoint :: FunPtr DrawPointFcn+foreign import ccall unsafe "&hs_b2DrawStringTrampoline" p_drawString :: FunPtr DrawStringFcn+foreign import ccall unsafe "&hs_b2DrawBoundsTrampoline" p_drawBounds :: FunPtr DrawBoundsFcn++{- | A wired-up 'DebugDraw', ready to hand to 'draw'.++Opaque because the underlying @b2DebugDraw@ and its ten 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 'Box2D.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 "Box2D.Callbacks.withDraw: DebugDraw.drawingBounds is not a valid AABB"+  failed <- newIORef Nothing+  allocaBytes DrawClosure.size $ \closure ->+    withSlots+      [ mkSlot (DrawClosure.polygon closure) wrapDrawPolygon d.polygon $+          \f pXf pVs n color _ -> trapping failed $ do+            xf <- peek pXf+            vs <- copyVector (fromIntegral n) pVs+            f xf vs color+      , mkSlot (DrawClosure.solidPolygon closure) wrapDrawSolidPolygon d.solidPolygon $+          \f pXf pVs n radius color _ -> trapping failed $ do+            xf <- peek pXf+            vs <- copyVector (fromIntegral n) pVs+            f xf vs radius color+      , mkSlot (DrawClosure.circle closure) wrapDrawCircle d.circle $+          \f pCenter radius color _ -> trapping failed $ do+            center <- peek pCenter+            f center radius color+      , mkSlot (DrawClosure.solidCircle closure) wrapDrawSolidCircle d.solidCircle $+          \f pXf pCenter radius color _ -> trapping failed $ do+            xf <- peek pXf+            center <- peek pCenter+            f xf center radius color+      , mkSlot (DrawClosure.solidCapsule closure) wrapDrawSolidCapsule d.solidCapsule $+          \f pP1 pP2 radius color _ -> trapping failed $ do+            p1 <- peek pP1+            p2 <- peek pP2+            f p1 p2 radius color+      , mkSlot (DrawClosure.line closure) wrapDrawLine d.line $+          \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.string closure) wrapDrawString d.string $+          \f pP cs color _ -> trapping failed $ do+            p <- peek pP+            s <- peekUtf8 cs+            f p s color+      , mkSlot (DrawClosure.bounds closure) wrapDrawBounds d.bounds $+          \f pAabb color _ -> trapping failed $ do+            aabb <- peek pAabb+            f aabb color+      ]+      $ do+        DrawClosure.context closure base.context+        with+          base+            { drawPolygonFcn = p_drawPolygon+            , drawSolidPolygonFcn = p_drawSolidPolygon+            , drawCircleFcn = p_drawCircle+            , drawSolidCircleFcn = p_drawSolidCircle+            , drawSolidCapsuleFcn = p_drawSolidCapsule+            , drawLineFcn = p_drawLine+            , drawTransformFcn = p_drawTransform+            , drawPointFcn = p_drawPoint+            , drawStringFcn = p_drawString+            , drawBoundsFcn = p_drawBounds+            , 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 @b2World_Draw@ via+'Box2D.World.draw'.+-}+draw :: WorldId -> DrawTarget -> IO ()+draw worldId target = withDrawPtr target (World.draw worldId)++{- | Hand a 'DrawTarget'\'s @b2DebugDraw@ to a C call other than+'draw' — @b2Replay_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 -> try act >>= either (writeIORef ref . Just) pure++-- | Take and clear a trapped callback exception.+rethrowTrapped :: DrawTarget -> IO ()+rethrowTrapped target =+  atomicModifyIORef' target.failure (Nothing,) >>= mapM_ throwIO++-- | One debug-draw primitive, as the engine emitted it.+data Cmd+  = DrawPolygon WorldTransform (VS.Vector Vec2) HexColor+  | DrawSolidPolygon WorldTransform (VS.Vector Vec2) Float HexColor+  | DrawCircle Pos Float HexColor+  | DrawSolidCircle WorldTransform Vec2 Float HexColor+  | DrawSolidCapsule Pos Pos Float HexColor+  | DrawLine Pos Pos HexColor+  | DrawTransform WorldTransform+  | DrawPoint Pos Float HexColor+  | DrawString Pos String HexColor+  | DrawBounds AABB 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.+-}+withRecorder :: DebugDraw -> (DrawTarget -> IO a) -> IO (a, [Cmd])+withRecorder base act = do+  acc <- newIORef []+  let+    record c = modifyIORef' acc (c :)+    recording =+      Draw+        { polygon = Just $ \xf vs c -> record (DrawPolygon xf vs c)+        , solidPolygon = Just $ \xf vs r c -> record (DrawSolidPolygon xf vs r c)+        , circle = Just $ \p r c -> record (DrawCircle p r c)+        , solidCircle = Just $ \xf p r c -> record (DrawSolidCircle xf p r c)+        , solidCapsule = Just $ \p1 p2 r c -> record (DrawSolidCapsule p1 p2 r c)+        , line = Just $ \p1 p2 c -> record (DrawLine p1 p2 c)+        , transform = Just $ record . DrawTransform+        , point = Just $ \p s c -> record (DrawPoint p s c)+        , string = Just $ \p s c -> record (DrawString p s c)+        , bounds = Just $ \a c -> record (DrawBounds a c)+        }+  r <- withDraw recording base act+  cmds <- readIORef acc+  pure (r, reverse cmds)++{- | One @hs_b2DrawClosure@ 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_b2DrawClosure@ 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.++Box2D 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 EnqueueTaskCallbackFcn+  , finish :: FunPtr FinishTaskCallbackFcn+  , workerCount :: Int+  }++-- 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 () -> 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 Box2D 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 = 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/Box2D/Chain.hsc view
@@ -0,0 +1,296 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.Chain+  ( ChainDef (..)+  , defaultChainDef+  , create+  , destroy+  , getWorld+  , getSegmentCount+  , getSegments+  , setAllSurfaceMaterials+  , setSurfaceMaterial+  , getSurfaceMaterial+  , getSurfaceMaterialInto+  , isValid+  , enableContactEvents+  , enableHitEvents+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (BodyId (..), ChainId (..), ShapeId (..), WorldId (..))+import Box2D.MathTypes (Vec2)+import Box2D.Shape (Filter, SurfaceMaterial)++#include "box2d/box2d.h"+++{- | Used to create a chain of line segments. This is designed to eliminate ghost collisions with+some limitations.++- chains are one-sided+- chains have no mass and should be used on static bodies+- chains have a counter-clockwise winding order (normal points right of segment direction)+- chains are either a loop or open+- the distance between any two points must be greater than @B2_LINEAR_SLOP@+- a chain shape should not self intersect (this is not validated)+- an open chain shape needs a leading and trailing ghost point.+- you may overlap two open chains using the ghost points to get smooth collision.+- a chain shape creates multiple line segment shapes on the body++<https://en.wikipedia.org/wiki/Polygonal_chain>+Must be initialized using @b2DefaultChainDef()@.++__Warning:__ Do not use chain shapes unless you understand the limitations. This is an advanced feature.+-}+data ChainDef = ChainDef+  { userData :: Ptr ()+  -- ^ Use this to store application specific shape data.+  , points :: Ptr Vec2+  -- ^ Array of chain points. Each consecutive pair forms a segment. For a loop the last and first+  -- point form a segment. Cloned.+  , pointCount :: CInt+  -- ^ The point count. At least 2 for an open chain and at least 3 for a loop.+  -- segmentCount = isLoop ? pointCount : pointCount - 1+  , ghost1 :: Vec2+  -- ^ Leading ghost point for an open chain. Distance from first point must be greater than+  -- @B2_LINEAR_SLOP@. Ignored for loops.+  , ghost2 :: Vec2+  -- ^ Ending ghost point for an open chain. Distance from last point must be greater than+  -- @B2_LINEAR_SLOP@. Ignored for loops.+  , materials :: Ptr SurfaceMaterial+  -- ^ One material for the whole chain or one for each segment. Cloned.+  , materialCount :: CInt+  -- ^ The material count. Must be 1 or the segment count.+  , filter :: Filter+  -- ^ Contact filtering data.+  , isLoop :: CBool+  -- ^ Indicates a closed chain formed by connecting the first and last point.+  , enableSensorEvents :: CBool+  -- ^ Enable sensors to detect this chain. False by default.+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable ChainDef where+  sizeOf _ = #{size b2ChainDef}+  alignment _ = #{alignment b2ChainDef}+  peek p =+    ChainDef+      <$> #{peek b2ChainDef, userData} p+      <*> #{peek b2ChainDef, points} p+      <*> #{peek b2ChainDef, pointCount} p+      <*> #{peek b2ChainDef, ghost1} p+      <*> #{peek b2ChainDef, ghost2} p+      <*> #{peek b2ChainDef, materials} p+      <*> #{peek b2ChainDef, materialCount} p+      <*> #{peek b2ChainDef, filter} p+      <*> #{peek b2ChainDef, isLoop} p+      <*> #{peek b2ChainDef, enableSensorEvents} p+      <*> #{peek b2ChainDef, internalValue} p+  poke p (ChainDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11) = do+    #{poke b2ChainDef, userData} p f1+    #{poke b2ChainDef, points} p f2+    #{poke b2ChainDef, pointCount} p f3+    #{poke b2ChainDef, ghost1} p f4+    #{poke b2ChainDef, ghost2} p f5+    #{poke b2ChainDef, materials} p f6+    #{poke b2ChainDef, materialCount} p f7+    #{poke b2ChainDef, filter} p f8+    #{poke b2ChainDef, isLoop} p f9+    #{poke b2ChainDef, enableSensorEvents} p f10+    #{poke b2ChainDef, internalValue} p f11++foreign import ccall unsafe "hs_b2DefaultChainDef"+  c_b2DefaultChainDef :: Ptr ChainDef -> IO ()++defaultChainDef :: IO ChainDef+defaultChainDef = allocaOut c_b2DefaultChainDef+++foreign import ccall unsafe "b2CreateChain"+  c_b2CreateChain :: BodyId -> Ptr ChainDef -> IO ChainId++foreign import ccall unsafe "b2DestroyChain"+  c_b2DestroyChain :: ChainId -> IO ()++foreign import ccall unsafe "b2Chain_GetWorld"+  c_b2Chain_GetWorld :: ChainId -> IO WorldId++foreign import ccall unsafe "b2Chain_GetSegmentCount"+  c_b2Chain_GetSegmentCount :: ChainId -> IO CInt++foreign import ccall unsafe "b2Chain_GetSegments"+  c_b2Chain_GetSegments :: ChainId -> Ptr ShapeId -> CInt -> IO CInt++foreign import ccall unsafe "b2Chain_SetAllSurfaceMaterials"+  c_b2Chain_SetAllSurfaceMaterials :: ChainId -> Ptr SurfaceMaterial -> IO ()++foreign import ccall unsafe "b2Chain_SetSurfaceMaterial"+  c_b2Chain_SetSurfaceMaterial :: ChainId -> Ptr SurfaceMaterial -> CInt -> IO ()++foreign import ccall unsafe "hsg_b2Chain_GetSurfaceMaterial"+  c_b2Chain_GetSurfaceMaterial :: ChainId -> CInt -> Ptr SurfaceMaterial -> IO ()++foreign import ccall unsafe "b2Chain_IsValid"+  c_b2Chain_IsValid :: ChainId -> IO CBool++foreign import ccall unsafe "hs_b2Chain_EnableContactEvents"+  c_b2Chain_EnableContactEvents :: ChainId -> CBool -> IO ()++foreign import ccall unsafe "hs_b2Chain_EnableHitEvents"+  c_b2Chain_EnableHitEvents :: ChainId -> CBool -> IO ()++{- | Create a chain shape++See also @b2ChainDef@ for details.++Binds @b2CreateChain@.+-}+create+  :: BodyId+  -> ChainDef+  -> IO ChainId+create a0 a1 =+  with a1 $ \p1 ->+  c_b2CreateChain a0 p1++{- | Destroy a chain shape++Binds @b2DestroyChain@.+-}+destroy+  :: ChainId+  -> IO ()+destroy a0 =+  c_b2DestroyChain a0++{- | Get the world that owns this chain shape++Binds @b2Chain_GetWorld@.+-}+getWorld+  :: ChainId+  -> IO WorldId+getWorld a0 =+  c_b2Chain_GetWorld a0++{- | Get the number of segments on this chain++Binds @b2Chain_GetSegmentCount@.+-}+getSegmentCount+  :: ChainId+  -> IO Int+getSegmentCount a0 =+  fromIntegral <$> (c_b2Chain_GetSegmentCount a0)++{- | Fill a user array with chain segment shape ids up to the specified capacity. Returns the actual+number of segments returned.++Binds @b2Chain_GetSegments@.+-}+getSegments+  :: ChainId+  -> Ptr ShapeId+  -- ^ @segmentArray@+  -> Int+  -- ^ @capacity@+  -> IO Int+getSegments a0 a1 a2 =+  fromIntegral <$> (c_b2Chain_GetSegments a0 a1 (fromIntegral a2))++{- | Set the chain material on all segments.++Binds @b2Chain_SetAllSurfaceMaterials@.+-}+setAllSurfaceMaterials+  :: ChainId+  -> SurfaceMaterial+  -> IO ()+setAllSurfaceMaterials a0 a1 =+  with a1 $ \p1 ->+  c_b2Chain_SetAllSurfaceMaterials a0 p1++{- | Set a chain material by segment index.++Binds @b2Chain_SetSurfaceMaterial@.+-}+setSurfaceMaterial+  :: ChainId+  -> SurfaceMaterial+  -> Int+  -- ^ @segmentIndex@+  -> IO ()+setSurfaceMaterial a0 a1 a2 =+  with a1 $ \p1 ->+  c_b2Chain_SetSurfaceMaterial a0 p1 (fromIntegral a2)++{- | Get a chain material by segment index.++Binds @b2Chain_GetSurfaceMaterial@.+-}+getSurfaceMaterial+  :: ChainId+  -> Int+  -- ^ @segmentIndex@+  -> IO SurfaceMaterial+getSurfaceMaterial a0 a1 =+  alloca $ \pOut -> c_b2Chain_GetSurfaceMaterial a0 (fromIntegral a1) pOut >> peek pOut++-- | Like 'getSurfaceMaterial', but the result is written into a caller-supplied buffer.+getSurfaceMaterialInto+  :: ChainId+  -> Int+  -- ^ @segmentIndex@+  -> Ptr SurfaceMaterial+  -- ^ Result buffer.+  -> IO ()+getSurfaceMaterialInto a0 a1 out =+  c_b2Chain_GetSurfaceMaterial a0 (fromIntegral a1) out++{- | Chain identifier validation. Provides validation for up to 64K allocations.++Binds @b2Chain_IsValid@.+-}+isValid+  :: ChainId+  -> IO Bool+isValid a0 =+  toBool <$> (c_b2Chain_IsValid a0)++{- | Enable\/disable contact events on every segment shape of a chain.+Chain creation turns segment contact events off, so this visits+each segment in C instead of one FFI round-trip per segment.++Provided by the hand-written shim @hs_b2Chain_EnableContactEvents@; no upstream equivalent.+-}+enableContactEvents+  :: ChainId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableContactEvents a0 a1 =+  c_b2Chain_EnableContactEvents a0 (fromBool a1)++{- | Enable\/disable hit events on every segment shape of a chain.+Chain creation turns segment hit events off, so this visits+each segment in C instead of one FFI round-trip per segment.++Provided by the hand-written shim @hs_b2Chain_EnableHitEvents@; no upstream equivalent.+-}+enableHitEvents+  :: ChainId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableHitEvents a0 a1 =+  c_b2Chain_EnableHitEvents a0 (fromBool a1)
+ src/Box2D/Collision.hsc view
@@ -0,0 +1,1366 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.Collision+  ( PlaneSolverResult (..)+  , CollisionPlane (..)+  , PlaneResult (..)+  , LocalManifoldPoint (..)+  , Manifold (..)+  , ManifoldPoint (..)+  , validateRecording+  , isValidRay+  , makePolygon+  , makeOffsetPolygon+  , makeOffsetRoundedPolygon+  , makeSquare+  , makeSquareInto+  , makeBox+  , makeBoxInto+  , makeRoundedBox+  , makeRoundedBoxInto+  , makeOffsetBox+  , makeOffsetRoundedBox+  , transformPolygon+  , computeCircleMass+  , computeCapsuleMass+  , computePolygonMass+  , computeCircleBounds+  , computeCapsuleBounds+  , computePolygonBounds+  , computeSegmentBounds+  , pointInCircle+  , pointInCapsule+  , pointInPolygon+  , castRayCircle+  , castRayCapsule+  , castRaySegment+  , castRayPolygon+  , castShapeCircle+  , castShapeCapsule+  , castShapeSegment+  , castShapePolygon+  , computeHull+  , computeHullInto+  , validateHull+  , computeSegmentDistance+  , computeShapeDistance+  , castShape+  , castShapeInto+  , makeProxy+  , makeProxyInto+  , makeOffsetProxy+  , getSweepTransform+  , computeTimeOfImpact+  , collideCircles+  , collideCapsuleAndCircle+  , collideSegmentAndCircle+  , collidePolygonAndCircle+  , collideCapsules+  , collideSegmentAndCapsule+  , collidePolygonAndCapsule+  , collidePolygons+  , collideSegmentAndPolygon+  , collideChainSegmentAndCircle+  , collideChainSegmentAndCapsule+  , collideChainSegmentAndPolygon+  , solvePlanes+  , clipVector+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Data.Vector.Storable qualified as VS+import Box2D.Internal (copyBounded, pokeBounded)+import Box2D.Distance (DistanceInput, DistanceOutput, SegmentDistanceResult, ShapeCastPairInput, Simplex, SimplexCache, Sweep, TOIInput, TOIOutput)+import Box2D.Geometry (Capsule, CastOutput, ChainSegment, Circle, Hull, MassData, Polygon, RayCastInput, Segment, ShapeCastInput, ShapeProxy)+import Box2D.MathTypes (AABB, Plane, Rot, Transform, Vec2, WorldTransform)+import Box2D.Tags (LocalManifold)++#include "box2d/box2d.h"+++-- | Result returned by @b2SolvePlanes@.+data PlaneSolverResult = PlaneSolverResult+  { delta :: Vec2+  -- ^ 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 b2PlaneSolverResult}+  alignment _ = #{alignment b2PlaneSolverResult}+  peek p =+    PlaneSolverResult+      <$> #{peek b2PlaneSolverResult, delta} p+      <*> #{peek b2PlaneSolverResult, iterationCount} p+  poke p (PlaneSolverResult f1 f2) = do+    #{poke b2PlaneSolverResult, delta} p f1+    #{poke b2PlaneSolverResult, iterationCount} p f2++{- | These are collision planes that can be fed to @b2SolvePlanes@. Normally this is assembled by the+user from plane results in @b2PlaneResult@.+-}+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 @b2SolvePlanes@. Usually in meters.+  , clipVelocity :: CBool+  -- ^ Indicates if @b2ClipVector@ should clip against this plane. Should be false for soft+  -- collision.+  }+  deriving (Eq, Show)++instance Storable CollisionPlane where+  sizeOf _ = #{size b2CollisionPlane}+  alignment _ = #{alignment b2CollisionPlane}+  peek p =+    CollisionPlane+      <$> #{peek b2CollisionPlane, plane} p+      <*> #{peek b2CollisionPlane, pushLimit} p+      <*> #{peek b2CollisionPlane, push} p+      <*> #{peek b2CollisionPlane, clipVelocity} p+  poke p (CollisionPlane f1 f2 f3 f4) = do+    #{poke b2CollisionPlane, plane} p f1+    #{poke b2CollisionPlane, pushLimit} p f2+    #{poke b2CollisionPlane, push} p f3+    #{poke b2CollisionPlane, clipVelocity} p f4++{- | These are the collision planes returned from @b2World_CollideMover@. The plane and point are+relative to the query origin, matching the mover capsule.+-}+data PlaneResult = PlaneResult+  { plane :: Plane+  -- ^ The collision plane between the mover and a convex shape+  , point :: Vec2+  -- ^ The collision point on the shape.+  , hit :: CBool+  -- ^ Did the collision register a hit? If not this plane should be ignored.+  }+  deriving (Eq, Show)++instance Storable PlaneResult where+  sizeOf _ = #{size b2PlaneResult}+  alignment _ = #{alignment b2PlaneResult}+  peek p =+    PlaneResult+      <$> #{peek b2PlaneResult, plane} p+      <*> #{peek b2PlaneResult, point} p+      <*> #{peek b2PlaneResult, hit} p+  poke p (PlaneResult f1 f2 f3) = do+    #{poke b2PlaneResult, plane} p f1+    #{poke b2PlaneResult, point} p f2+    #{poke b2PlaneResult, hit} p f3++-- | Contact manifold point in local coordinates (frame A).+data LocalManifoldPoint = LocalManifoldPoint+  { point :: Vec2+  -- ^ Contact point in frame A+  , separation :: Float+  -- ^ The separation of the contact point, negative if penetrating. May be positive or negative.+  , id :: Word16+  -- ^ Uniquely identifies a contact point between two shapes+  }+  deriving (Eq, Show)++instance Storable LocalManifoldPoint where+  sizeOf _ = #{size b2LocalManifoldPoint}+  alignment _ = #{alignment b2LocalManifoldPoint}+  peek p =+    LocalManifoldPoint+      <$> #{peek b2LocalManifoldPoint, point} p+      <*> #{peek b2LocalManifoldPoint, separation} p+      <*> #{peek b2LocalManifoldPoint, id} p+  poke p (LocalManifoldPoint f1 f2 f3) = do+    #{poke b2LocalManifoldPoint, point} p f1+    #{poke b2LocalManifoldPoint, separation} p f2+    #{poke b2LocalManifoldPoint, id} p f3++{- | A contact manifold describes the contact points between colliding shapes.++__Note:__ Box2D uses speculative collision so some contact points may be separated.+-}+data Manifold = Manifold+  { normal :: Vec2+  -- ^ The unit normal vector in world space, points from shape A to shape B.+  , rollingImpulse :: Float+  -- ^ Angular impulse applied for rolling resistance. N * m * s = kg * m^2 \/ s+  , points :: VS.Vector ManifoldPoint+  -- ^ The manifold points, up to two are possible in 2D.+  }+  deriving (Eq, Show)++instance Storable Manifold where+  sizeOf _ = #{size b2Manifold}+  alignment _ = #{alignment b2Manifold}+  peek p = do+    nPointCount <- #{peek b2Manifold, pointCount} p :: IO CInt+    Manifold+      <$> #{peek b2Manifold, normal} p+      <*> #{peek b2Manifold, rollingImpulse} p+      <*> copyBounded (#{const 2}) nPointCount (#{ptr b2Manifold, points} p)+  poke p x = do+    #{poke b2Manifold, normal} p x.normal+    #{poke b2Manifold, rollingImpulse} p x.rollingImpulse+    pokeBounded (#{const 2}) (#{ptr b2Manifold, points} p) x.points+    #{poke b2Manifold, pointCount} p (fromIntegral (minimum [(#{const 2}), 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. Box2D uses speculative collision so some+contact points may be separated. You may use the totalNormalImpulse to determine if there was an+interaction during the time step.+-}+data ManifoldPoint = ManifoldPoint+  { anchorA :: Vec2+  -- ^ Location of the contact point relative to bodyA\'s center of mass in world space. This can be+  -- converted to a world point using:+  -- @b2Pos@ worldPointA = b2OffsetPos(b2Body_GetWorldCenter(myBodyIdA), anchorA);+  , anchorB :: Vec2+  -- ^ Similar for body B.+  , separation :: Float+  -- ^ The separation of the contact point, negative if penetrating+  , normalImpulse :: Float+  -- ^ The impulse along the manifold normal vector.+  , tangentImpulse :: Float+  -- ^ The friction impulse+  , totalNormalImpulse :: Float+  -- ^ The total normal impulse applied across sub-stepping and restitution. This is important to+  -- identify speculative contact points that had an interaction in the time step. This includes+  -- the warm starting impulse, the sub-step delta impulse, and the restitution impulse.+  , normalVelocity :: Float+  -- ^ Relative normal velocity pre-solve. Negative when approaching. This is only computed if hit+  -- events are enabled. It may be computed otherwise as a side effect of SIMD bundling, but that+  -- is not guaranteed.+  , baseSeparation :: Float+  -- ^ Cached separation used for contact recycling+  , id :: Word16+  -- ^ Uniquely identifies a contact point between two shapes+  , persisted :: CBool+  -- ^ Did this contact point exist the previous step?+  }+  deriving (Eq, Show)++instance Storable ManifoldPoint where+  sizeOf _ = #{size b2ManifoldPoint}+  alignment _ = #{alignment b2ManifoldPoint}+  peek p =+    ManifoldPoint+      <$> #{peek b2ManifoldPoint, anchorA} p+      <*> #{peek b2ManifoldPoint, anchorB} p+      <*> #{peek b2ManifoldPoint, separation} p+      <*> #{peek b2ManifoldPoint, normalImpulse} p+      <*> #{peek b2ManifoldPoint, tangentImpulse} p+      <*> #{peek b2ManifoldPoint, totalNormalImpulse} p+      <*> #{peek b2ManifoldPoint, normalVelocity} p+      <*> #{peek b2ManifoldPoint, baseSeparation} p+      <*> #{peek b2ManifoldPoint, id} p+      <*> #{peek b2ManifoldPoint, persisted} p+  poke p (ManifoldPoint f1 f2 f3 f4 f5 f6 f7 f8 f9 f10) = do+    #{poke b2ManifoldPoint, anchorA} p f1+    #{poke b2ManifoldPoint, anchorB} p f2+    #{poke b2ManifoldPoint, separation} p f3+    #{poke b2ManifoldPoint, normalImpulse} p f4+    #{poke b2ManifoldPoint, tangentImpulse} p f5+    #{poke b2ManifoldPoint, totalNormalImpulse} p f6+    #{poke b2ManifoldPoint, normalVelocity} p f7+    #{poke b2ManifoldPoint, baseSeparation} p f8+    #{poke b2ManifoldPoint, id} p f9+    #{poke b2ManifoldPoint, persisted} p f10+++foreign import ccall unsafe "b2ValidateRecording"+  c_b2ValidateRecording :: Ptr () -> CInt -> CInt -> IO CBool++foreign import ccall unsafe "b2IsValidRay"+  c_b2IsValidRay :: Ptr RayCastInput -> IO CBool++foreign import ccall unsafe "hsg_b2MakePolygon"+  c_b2MakePolygon :: Ptr Hull -> Float -> Ptr Polygon -> IO ()++foreign import ccall unsafe "hsg_b2MakeOffsetPolygon"+  c_b2MakeOffsetPolygon :: Ptr Hull -> Ptr Vec2 -> Ptr Rot -> Ptr Polygon -> IO ()++foreign import ccall unsafe "hsg_b2MakeOffsetRoundedPolygon"+  c_b2MakeOffsetRoundedPolygon :: Ptr Hull -> Ptr Vec2 -> Ptr Rot -> Float -> Ptr Polygon -> IO ()++foreign import ccall unsafe "hsg_b2MakeSquare"+  c_b2MakeSquare :: Float -> Ptr Polygon -> IO ()++foreign import ccall unsafe "hsg_b2MakeBox"+  c_b2MakeBox :: Float -> Float -> Ptr Polygon -> IO ()++foreign import ccall unsafe "hsg_b2MakeRoundedBox"+  c_b2MakeRoundedBox :: Float -> Float -> Float -> Ptr Polygon -> IO ()++foreign import ccall unsafe "hsg_b2MakeOffsetBox"+  c_b2MakeOffsetBox :: Float -> Float -> Ptr Vec2 -> Ptr Rot -> Ptr Polygon -> IO ()++foreign import ccall unsafe "hsg_b2MakeOffsetRoundedBox"+  c_b2MakeOffsetRoundedBox :: Float -> Float -> Ptr Vec2 -> Ptr Rot -> Float -> Ptr Polygon -> IO ()++foreign import ccall unsafe "hsg_b2TransformPolygon"+  c_b2TransformPolygon :: Ptr Transform -> Ptr Polygon -> Ptr Polygon -> IO ()++foreign import ccall unsafe "hsg_b2ComputeCircleMass"+  c_b2ComputeCircleMass :: Ptr Circle -> Float -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b2ComputeCapsuleMass"+  c_b2ComputeCapsuleMass :: Ptr Capsule -> Float -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b2ComputePolygonMass"+  c_b2ComputePolygonMass :: Ptr Polygon -> Float -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b2ComputeCircleBounds"+  c_b2ComputeCircleBounds :: Ptr Circle -> Ptr WorldTransform -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b2ComputeCapsuleBounds"+  c_b2ComputeCapsuleBounds :: Ptr Capsule -> Ptr WorldTransform -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b2ComputePolygonBounds"+  c_b2ComputePolygonBounds :: Ptr Polygon -> Ptr WorldTransform -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b2ComputeSegmentBounds"+  c_b2ComputeSegmentBounds :: Ptr Segment -> Ptr WorldTransform -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b2PointInCircle"+  c_b2PointInCircle :: Ptr Circle -> Ptr Vec2 -> IO CBool++foreign import ccall unsafe "hsg_b2PointInCapsule"+  c_b2PointInCapsule :: Ptr Capsule -> Ptr Vec2 -> IO CBool++foreign import ccall unsafe "hsg_b2PointInPolygon"+  c_b2PointInPolygon :: Ptr Polygon -> Ptr Vec2 -> IO CBool++foreign import ccall unsafe "hsg_b2CastRayCircle"+  c_b2CastRayCircle :: Ptr Circle -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b2CastRayCapsule"+  c_b2CastRayCapsule :: Ptr Capsule -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b2CastRaySegment"+  c_b2CastRaySegment :: Ptr Segment -> Ptr RayCastInput -> CBool -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b2CastRayPolygon"+  c_b2CastRayPolygon :: Ptr Polygon -> Ptr RayCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b2CastShapeCircle"+  c_b2CastShapeCircle :: Ptr Circle -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b2CastShapeCapsule"+  c_b2CastShapeCapsule :: Ptr Capsule -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b2CastShapeSegment"+  c_b2CastShapeSegment :: Ptr Segment -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b2CastShapePolygon"+  c_b2CastShapePolygon :: Ptr Polygon -> Ptr ShapeCastInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b2ComputeHull"+  c_b2ComputeHull :: Ptr Vec2 -> CInt -> Ptr Hull -> IO ()++foreign import ccall unsafe "b2ValidateHull"+  c_b2ValidateHull :: Ptr Hull -> IO CBool++foreign import ccall unsafe "hsg_b2ComputeSegmentDistance"+  c_b2ComputeSegmentDistance :: Ptr Vec2 -> Ptr Vec2 -> Ptr Vec2 -> Ptr Vec2 -> Ptr SegmentDistanceResult -> IO ()++foreign import ccall unsafe "hsg_b2ComputeShapeDistance"+  c_b2ComputeShapeDistance :: Ptr DistanceInput -> Ptr SimplexCache -> Ptr Simplex -> CInt -> Ptr DistanceOutput -> IO ()++foreign import ccall unsafe "hsg_b2CastShape"+  c_b2CastShape :: Ptr ShapeCastPairInput -> Ptr CastOutput -> IO ()++foreign import ccall unsafe "hsg_b2MakeProxy"+  c_b2MakeProxy :: Ptr Vec2 -> CInt -> Float -> Ptr ShapeProxy -> IO ()++foreign import ccall unsafe "hsg_b2MakeOffsetProxy"+  c_b2MakeOffsetProxy :: Ptr Vec2 -> CInt -> Float -> Ptr Vec2 -> Ptr Rot -> Ptr ShapeProxy -> IO ()++foreign import ccall unsafe "hsg_b2GetSweepTransform"+  c_b2GetSweepTransform :: Ptr Sweep -> Float -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b2ComputeTimeOfImpact"+  c_b2ComputeTimeOfImpact :: Ptr TOIInput -> Ptr TOIOutput -> IO ()++foreign import ccall unsafe "hsg_b2CollideCircles"+  c_b2CollideCircles :: Ptr Circle -> Ptr Circle -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollideCapsuleAndCircle"+  c_b2CollideCapsuleAndCircle :: Ptr Capsule -> Ptr Circle -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollideSegmentAndCircle"+  c_b2CollideSegmentAndCircle :: Ptr Segment -> Ptr Circle -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollidePolygonAndCircle"+  c_b2CollidePolygonAndCircle :: Ptr Polygon -> Ptr Circle -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollideCapsules"+  c_b2CollideCapsules :: Ptr Capsule -> Ptr Capsule -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollideSegmentAndCapsule"+  c_b2CollideSegmentAndCapsule :: Ptr Segment -> Ptr Capsule -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollidePolygonAndCapsule"+  c_b2CollidePolygonAndCapsule :: Ptr Polygon -> Ptr Capsule -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollidePolygons"+  c_b2CollidePolygons :: Ptr Polygon -> Ptr Polygon -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollideSegmentAndPolygon"+  c_b2CollideSegmentAndPolygon :: Ptr Segment -> Ptr Polygon -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollideChainSegmentAndCircle"+  c_b2CollideChainSegmentAndCircle :: Ptr ChainSegment -> Ptr Circle -> Ptr Transform -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollideChainSegmentAndCapsule"+  c_b2CollideChainSegmentAndCapsule :: Ptr ChainSegment -> Ptr Capsule -> Ptr Transform -> Ptr SimplexCache -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2CollideChainSegmentAndPolygon"+  c_b2CollideChainSegmentAndPolygon :: Ptr ChainSegment -> Ptr Polygon -> Ptr Transform -> Ptr SimplexCache -> Ptr LocalManifold -> IO ()++foreign import ccall unsafe "hsg_b2SolvePlanes"+  c_b2SolvePlanes :: Ptr Vec2 -> Ptr CollisionPlane -> CInt -> Ptr PlaneSolverResult -> IO ()++foreign import ccall unsafe "hsg_b2ClipVector"+  c_b2ClipVector :: Ptr Vec2 -> Ptr CollisionPlane -> CInt -> Ptr Vec2 -> IO ()++{- | Replay a recording by re-running the engine and asserting recorded ids and state match.++Returns: true if replay completed without divergence, false on any mismatch++Binds @b2ValidateRecording@.+-}+validateRecording+  :: Ptr ()+  -- ^ @data@: Recorded bytes.+  -> Int+  -- ^ @size@: Number of recorded bytes+  -> Int+  -- ^ @workerCount@: Worker count to use for replay. 0 uses the serial single-worker fallback.+  -> IO Bool+validateRecording a0 a1 a2 =+  toBool <$> (c_b2ValidateRecording a0 (fromIntegral a1) (fromIntegral a2))++{- | Validate ray cast input data (NaN, etc)++Binds @b2IsValidRay@.+-}+isValidRay+  :: RayCastInput+  -> IO Bool+isValidRay a0 =+  with a0 $ \p0 ->+  toBool <$> (c_b2IsValidRay p0)++{- | Make a convex polygon from a convex hull. This will assert if the hull is not valid.++__Warning:__ Do not manually fill in the hull data, it must come directly from @b2ComputeHull@++Binds @b2MakePolygon@.+-}+makePolygon+  :: Hull+  -> Float+  -- ^ @radius@+  -> IO Polygon+makePolygon a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2MakePolygon p0 a1 pOut >> peek pOut++{- | Make an offset convex polygon from a convex hull. This will assert if the hull is not valid.++__Warning:__ Do not manually fill in the hull data, it must come directly from @b2ComputeHull@++Binds @b2MakeOffsetPolygon@.+-}+makeOffsetPolygon+  :: Hull+  -> Vec2+  -- ^ @position@+  -> Rot+  -- ^ @rotation@+  -> IO Polygon+makeOffsetPolygon a0 a1 a2 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  alloca $ \pOut -> c_b2MakeOffsetPolygon p0 p1 p2 pOut >> peek pOut++{- | Make an offset convex polygon from a convex hull with a radius. This will assert if the hull is+not valid.++__Warning:__ Do not manually fill in the hull data, it must come directly from @b2ComputeHull@++Binds @b2MakeOffsetRoundedPolygon@.+-}+makeOffsetRoundedPolygon+  :: Hull+  -> Vec2+  -- ^ @position@+  -> Rot+  -- ^ @rotation@+  -> Float+  -- ^ @radius@+  -> IO Polygon+makeOffsetRoundedPolygon a0 a1 a2 a3 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  alloca $ \pOut -> c_b2MakeOffsetRoundedPolygon p0 p1 p2 a3 pOut >> peek pOut++{- | Make a square polygon, bypassing the need for a convex hull.++Binds @b2MakeSquare@.+-}+makeSquare+  :: Float+  -- ^ @halfWidth@: the half-width+  -> IO Polygon+makeSquare a0 =+  alloca $ \pOut -> c_b2MakeSquare a0 pOut >> peek pOut++-- | Like 'makeSquare', but the result is written into a caller-supplied buffer.+makeSquareInto+  :: Float+  -- ^ @halfWidth@: the half-width+  -> Ptr Polygon+  -- ^ Result buffer.+  -> IO ()+makeSquareInto a0 out =+  c_b2MakeSquare a0 out++{- | Make a box (rectangle) polygon, bypassing the need for a convex hull.++Binds @b2MakeBox@.+-}+makeBox+  :: Float+  -- ^ @halfWidth@: the half-width (x-axis)+  -> Float+  -- ^ @halfHeight@: the half-height (y-axis)+  -> IO Polygon+makeBox a0 a1 =+  alloca $ \pOut -> c_b2MakeBox a0 a1 pOut >> peek pOut++-- | Like 'makeBox', but the result is written into a caller-supplied buffer.+makeBoxInto+  :: Float+  -- ^ @halfWidth@: the half-width (x-axis)+  -> Float+  -- ^ @halfHeight@: the half-height (y-axis)+  -> Ptr Polygon+  -- ^ Result buffer.+  -> IO ()+makeBoxInto a0 a1 out =+  c_b2MakeBox a0 a1 out++{- | Make a rounded box, bypassing the need for a convex hull.++Binds @b2MakeRoundedBox@.+-}+makeRoundedBox+  :: Float+  -- ^ @halfWidth@: the half-width (x-axis)+  -> Float+  -- ^ @halfHeight@: the half-height (y-axis)+  -> Float+  -- ^ @radius@: the radius of the rounded extension+  -> IO Polygon+makeRoundedBox a0 a1 a2 =+  alloca $ \pOut -> c_b2MakeRoundedBox a0 a1 a2 pOut >> peek pOut++-- | Like 'makeRoundedBox', but the result is written into a caller-supplied buffer.+makeRoundedBoxInto+  :: Float+  -- ^ @halfWidth@: the half-width (x-axis)+  -> Float+  -- ^ @halfHeight@: the half-height (y-axis)+  -> Float+  -- ^ @radius@: the radius of the rounded extension+  -> Ptr Polygon+  -- ^ Result buffer.+  -> IO ()+makeRoundedBoxInto a0 a1 a2 out =+  c_b2MakeRoundedBox a0 a1 a2 out++{- | Make an offset box, bypassing the need for a convex hull.++Binds @b2MakeOffsetBox@.+-}+makeOffsetBox+  :: Float+  -- ^ @halfWidth@: the half-width (x-axis)+  -> Float+  -- ^ @halfHeight@: the half-height (y-axis)+  -> Vec2+  -- ^ @center@: the local center of the box+  -> Rot+  -- ^ @rotation@: the local rotation of the box+  -> IO Polygon+makeOffsetBox a0 a1 a2 a3 =+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  alloca $ \pOut -> c_b2MakeOffsetBox a0 a1 p2 p3 pOut >> peek pOut++{- | Make an offset rounded box, bypassing the need for a convex hull.++Binds @b2MakeOffsetRoundedBox@.+-}+makeOffsetRoundedBox+  :: Float+  -- ^ @halfWidth@: the half-width (x-axis)+  -> Float+  -- ^ @halfHeight@: the half-height (y-axis)+  -> Vec2+  -- ^ @center@: the local center of the box+  -> Rot+  -- ^ @rotation@: the local rotation of the box+  -> Float+  -- ^ @radius@: the radius of the rounded extension+  -> IO Polygon+makeOffsetRoundedBox a0 a1 a2 a3 a4 =+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  alloca $ \pOut -> c_b2MakeOffsetRoundedBox a0 a1 p2 p3 a4 pOut >> peek pOut++{- | Transform a polygon. This is useful for transferring a shape from one body to another.++Binds @b2TransformPolygon@.+-}+transformPolygon+  :: Transform+  -> Polygon+  -> IO Polygon+transformPolygon a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2TransformPolygon p0 p1 pOut >> peek pOut++{- | Compute mass properties of a circle++Binds @b2ComputeCircleMass@.+-}+computeCircleMass+  :: Circle+  -- ^ @shape@+  -> Float+  -- ^ @density@+  -> IO MassData+computeCircleMass a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2ComputeCircleMass p0 a1 pOut >> peek pOut++{- | Compute mass properties of a capsule++Binds @b2ComputeCapsuleMass@.+-}+computeCapsuleMass+  :: Capsule+  -- ^ @shape@+  -> Float+  -- ^ @density@+  -> IO MassData+computeCapsuleMass a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2ComputeCapsuleMass p0 a1 pOut >> peek pOut++{- | Compute mass properties of a polygon++Binds @b2ComputePolygonMass@.+-}+computePolygonMass+  :: Polygon+  -- ^ @shape@+  -> Float+  -- ^ @density@+  -> IO MassData+computePolygonMass a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2ComputePolygonMass p0 a1 pOut >> peek pOut++{- | Compute the bounding box of a transformed circle++Binds @b2ComputeCircleBounds@.+-}+computeCircleBounds+  :: Circle+  -- ^ @shape@+  -> WorldTransform+  -> IO AABB+computeCircleBounds a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2ComputeCircleBounds p0 p1 pOut >> peek pOut++{- | Compute the bounding box of a transformed capsule++Binds @b2ComputeCapsuleBounds@.+-}+computeCapsuleBounds+  :: Capsule+  -- ^ @shape@+  -> WorldTransform+  -> IO AABB+computeCapsuleBounds a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2ComputeCapsuleBounds p0 p1 pOut >> peek pOut++{- | Compute the bounding box of a transformed polygon++Binds @b2ComputePolygonBounds@.+-}+computePolygonBounds+  :: Polygon+  -- ^ @shape@+  -> WorldTransform+  -> IO AABB+computePolygonBounds a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2ComputePolygonBounds p0 p1 pOut >> peek pOut++{- | Compute the bounding box of a transformed line segment++Binds @b2ComputeSegmentBounds@.+-}+computeSegmentBounds+  :: Segment+  -- ^ @shape@+  -> WorldTransform+  -> IO AABB+computeSegmentBounds a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2ComputeSegmentBounds p0 p1 pOut >> peek pOut++{- | Test a point for overlap with a circle in local space++Binds @b2PointInCircle@.+-}+pointInCircle+  :: Circle+  -- ^ @shape@+  -> Vec2+  -- ^ @point@+  -> IO Bool+pointInCircle a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  toBool <$> (c_b2PointInCircle p0 p1)++{- | Test a point for overlap with a capsule in local space++Binds @b2PointInCapsule@.+-}+pointInCapsule+  :: Capsule+  -- ^ @shape@+  -> Vec2+  -- ^ @point@+  -> IO Bool+pointInCapsule a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  toBool <$> (c_b2PointInCapsule p0 p1)++{- | Test a point for overlap with a convex polygon in local space++Binds @b2PointInPolygon@.+-}+pointInPolygon+  :: Polygon+  -- ^ @shape@+  -> Vec2+  -- ^ @point@+  -> IO Bool+pointInPolygon a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  toBool <$> (c_b2PointInPolygon p0 p1)++{- | Ray cast versus circle shape in local space.++Binds @b2CastRayCircle@.+-}+castRayCircle+  :: Circle+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+castRayCircle a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2CastRayCircle p0 p1 pOut >> peek pOut++{- | Ray cast versus capsule shape in local space.++Binds @b2CastRayCapsule@.+-}+castRayCapsule+  :: Capsule+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+castRayCapsule a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2CastRayCapsule p0 p1 pOut >> peek pOut++{- | Ray cast versus segment shape in local space. Optionally treat the segment as one-sided with+hits from the left side being treated as a miss.++Binds @b2CastRaySegment@.+-}+castRaySegment+  :: Segment+  -- ^ @shape@+  -> RayCastInput+  -> Bool+  -- ^ @oneSided@+  -> IO CastOutput+castRaySegment a0 a1 a2 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2CastRaySegment p0 p1 (fromBool a2) pOut >> peek pOut++{- | Ray cast versus polygon shape in local space.++Binds @b2CastRayPolygon@.+-}+castRayPolygon+  :: Polygon+  -- ^ @shape@+  -> RayCastInput+  -> IO CastOutput+castRayPolygon a0 a1 =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2CastRayPolygon p0 p1 pOut >> peek pOut++{- | Shape cast versus a circle.++Binds @b2CastShapeCircle@.+-}+castShapeCircle+  :: Circle+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+castShapeCircle a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2CastShapeCircle p0 a1 pOut >> peek pOut++{- | Shape cast versus a capsule.++Binds @b2CastShapeCapsule@.+-}+castShapeCapsule+  :: Capsule+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+castShapeCapsule a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2CastShapeCapsule p0 a1 pOut >> peek pOut++{- | Shape cast versus a line segment.++Binds @b2CastShapeSegment@.+-}+castShapeSegment+  :: Segment+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+castShapeSegment a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2CastShapeSegment p0 a1 pOut >> peek pOut++{- | Shape cast versus a convex polygon.++Binds @b2CastShapePolygon@.+-}+castShapePolygon+  :: Polygon+  -- ^ @shape@+  -> Ptr ShapeCastInput+  -> IO CastOutput+castShapePolygon a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2CastShapePolygon p0 a1 pOut >> peek pOut++{- | Compute the convex hull of a set of points. Returns an empty hull if it fails. Some failure+cases:++- all points very close together+- all points on a line+- less than 3 points+- more than @B2_MAX_POLYGON_VERTICES@ points++This welds close points and removes collinear points.++__Warning:__ Do not modify a hull once it has been computed++Binds @b2ComputeHull@.+-}+computeHull+  :: Ptr Vec2+  -- ^ @points@+  -> Int+  -- ^ @count@+  -> IO Hull+computeHull a0 a1 =+  alloca $ \pOut -> c_b2ComputeHull a0 (fromIntegral a1) pOut >> peek pOut++-- | Like 'computeHull', but the result is written into a caller-supplied buffer.+computeHullInto+  :: Ptr Vec2+  -- ^ @points@+  -> Int+  -- ^ @count@+  -> Ptr Hull+  -- ^ Result buffer.+  -> IO ()+computeHullInto a0 a1 out =+  c_b2ComputeHull a0 (fromIntegral a1) out++{- | This determines if a hull is valid. Checks for:++- convexity+- collinear points++This is expensive and should not be called at runtime.++Binds @b2ValidateHull@.+-}+validateHull+  :: Hull+  -> IO Bool+validateHull a0 =+  with a0 $ \p0 ->+  toBool <$> (c_b2ValidateHull p0)++{- | Compute the distance between two line segments, clamping at the end points if needed.++Binds @b2ComputeSegmentDistance@.+-}+computeSegmentDistance+  :: Vec2+  -- ^ @p1@+  -> Vec2+  -- ^ @q1@+  -> Vec2+  -- ^ @p2@+  -> Vec2+  -- ^ @q2@+  -> Ptr SegmentDistanceResult+  -- ^ Result buffer.+  -> IO ()+computeSegmentDistance a0 a1 a2 a3 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  c_b2ComputeSegmentDistance p0 p1 p2 p3 out++{- | Compute the closest points between two shapes represented as point clouds. @b2SimplexCache@ cache+is input\/output. On the first call set @b2SimplexCache.count@ to zero. The underlying GJK+algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0+for these.++Binds @b2ComputeShapeDistance@.+-}+computeShapeDistance+  :: Ptr DistanceInput+  -> Ptr SimplexCache+  -> Ptr Simplex+  -- ^ @simplexes@+  -> Int+  -- ^ @simplexCapacity@+  -> Ptr DistanceOutput+  -- ^ Result buffer.+  -> IO ()+computeShapeDistance a0 a1 a2 a3 out =+  c_b2ComputeShapeDistance 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 return a zero fraction in the output.++Binds @b2CastShape@.+-}+castShape+  :: Ptr ShapeCastPairInput+  -> IO CastOutput+castShape a0 =+  alloca $ \pOut -> c_b2CastShape a0 pOut >> peek pOut++-- | Like 'castShape', but the result is written into a caller-supplied buffer.+castShapeInto+  :: Ptr ShapeCastPairInput+  -> Ptr CastOutput+  -- ^ Result buffer.+  -> IO ()+castShapeInto a0 out =+  c_b2CastShape a0 out++{- | Make a proxy for use in overlap, shape cast, and related functions. This is a deep copy of the+points.++Binds @b2MakeProxy@.+-}+makeProxy+  :: Ptr Vec2+  -- ^ @points@+  -> Int+  -- ^ @count@+  -> Float+  -- ^ @radius@+  -> IO ShapeProxy+makeProxy a0 a1 a2 =+  alloca $ \pOut -> c_b2MakeProxy a0 (fromIntegral a1) a2 pOut >> peek pOut++-- | Like 'makeProxy', but the result is written into a caller-supplied buffer.+makeProxyInto+  :: Ptr Vec2+  -- ^ @points@+  -> Int+  -- ^ @count@+  -> Float+  -- ^ @radius@+  -> Ptr ShapeProxy+  -- ^ Result buffer.+  -> IO ()+makeProxyInto a0 a1 a2 out =+  c_b2MakeProxy a0 (fromIntegral a1) a2 out++{- | Make a proxy with a transform. This is a deep copy of the points.++Binds @b2MakeOffsetProxy@.+-}+makeOffsetProxy+  :: Ptr Vec2+  -- ^ @points@+  -> Int+  -- ^ @count@+  -> Float+  -- ^ @radius@+  -> Vec2+  -- ^ @position@+  -> Rot+  -- ^ @rotation@+  -> IO ShapeProxy+makeOffsetProxy a0 a1 a2 a3 a4 =+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  alloca $ \pOut -> c_b2MakeOffsetProxy a0 (fromIntegral a1) a2 p3 p4 pOut >> peek pOut++{- | Evaluate the transform sweep at a specific time.++Binds @b2GetSweepTransform@.+-}+getSweepTransform+  :: Sweep+  -> Float+  -- ^ @time@+  -> IO Transform+getSweepTransform a0 a1 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2GetSweepTransform p0 a1 pOut >> peek pOut++{- | Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction+between \[0,maxFraction\]. 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 @b2ComputeTimeOfImpact@.+-}+computeTimeOfImpact+  :: Ptr TOIInput+  -> Ptr TOIOutput+  -- ^ Result buffer.+  -> IO ()+computeTimeOfImpact a0 out =+  c_b2ComputeTimeOfImpact a0 out++{- | Compute the contact manifold between two circles++Binds @b2CollideCircles@.+-}+collideCircles+  :: Circle+  -- ^ @circleA@+  -> Circle+  -- ^ @circleB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collideCircles a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollideCircles p0 p1 p2 out++{- | Compute the contact manifold between a capsule and circle++Binds @b2CollideCapsuleAndCircle@.+-}+collideCapsuleAndCircle+  :: Capsule+  -- ^ @capsuleA@+  -> Circle+  -- ^ @circleB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collideCapsuleAndCircle a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollideCapsuleAndCircle p0 p1 p2 out++{- | Compute the contact manifold between a segment and a circle++Binds @b2CollideSegmentAndCircle@.+-}+collideSegmentAndCircle+  :: Segment+  -- ^ @segmentA@+  -> Circle+  -- ^ @circleB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collideSegmentAndCircle a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollideSegmentAndCircle p0 p1 p2 out++{- | Compute the contact manifold between a polygon and a circle++Binds @b2CollidePolygonAndCircle@.+-}+collidePolygonAndCircle+  :: Polygon+  -- ^ @polygonA@+  -> Circle+  -- ^ @circleB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collidePolygonAndCircle a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollidePolygonAndCircle p0 p1 p2 out++{- | Compute the contact manifold between two capsules++Binds @b2CollideCapsules@.+-}+collideCapsules+  :: Capsule+  -- ^ @capsuleA@+  -> Capsule+  -- ^ @capsuleB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collideCapsules a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollideCapsules p0 p1 p2 out++{- | Compute the contact manifold between a segment and a capsule++Binds @b2CollideSegmentAndCapsule@.+-}+collideSegmentAndCapsule+  :: Segment+  -- ^ @segmentA@+  -> Capsule+  -- ^ @capsuleB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collideSegmentAndCapsule a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollideSegmentAndCapsule p0 p1 p2 out++{- | Compute the contact manifold between a polygon and capsule++Binds @b2CollidePolygonAndCapsule@.+-}+collidePolygonAndCapsule+  :: Polygon+  -- ^ @polygonA@+  -> Capsule+  -- ^ @capsuleB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collidePolygonAndCapsule a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollidePolygonAndCapsule p0 p1 p2 out++{- | Compute the contact manifold between two polygons++Binds @b2CollidePolygons@.+-}+collidePolygons+  :: Polygon+  -- ^ @polygonA@+  -> Polygon+  -- ^ @polygonB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collidePolygons a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollidePolygons p0 p1 p2 out++{- | Compute the contact manifold between a segment and a polygon++Binds @b2CollideSegmentAndPolygon@.+-}+collideSegmentAndPolygon+  :: Segment+  -- ^ @segmentA@+  -> Polygon+  -- ^ @polygonB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collideSegmentAndPolygon a0 a1 a2 out =+  with a0 $ \p0 ->+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollideSegmentAndPolygon p0 p1 p2 out++{- | Compute the contact manifold between a chain segment and a circle++Binds @b2CollideChainSegmentAndCircle@.+-}+collideChainSegmentAndCircle+  :: Ptr ChainSegment+  -- ^ @segmentA@+  -> Circle+  -- ^ @circleB@+  -> Transform+  -- ^ @xf@+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collideChainSegmentAndCircle a0 a1 a2 out =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollideChainSegmentAndCircle a0 p1 p2 out++{- | Compute the contact manifold between a chain segment and a capsule++Binds @b2CollideChainSegmentAndCapsule@.+-}+collideChainSegmentAndCapsule+  :: Ptr ChainSegment+  -- ^ @segmentA@+  -> Capsule+  -- ^ @capsuleB@+  -> Transform+  -- ^ @xf@+  -> Ptr SimplexCache+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collideChainSegmentAndCapsule a0 a1 a2 a3 out =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollideChainSegmentAndCapsule a0 p1 p2 a3 out++{- | Compute the contact manifold between a chain segment and a rounded polygon++Binds @b2CollideChainSegmentAndPolygon@.+-}+collideChainSegmentAndPolygon+  :: Ptr ChainSegment+  -- ^ @segmentA@+  -> Polygon+  -- ^ @polygonB@+  -> Transform+  -- ^ @xf@+  -> Ptr SimplexCache+  -> Ptr LocalManifold+  -- ^ Result buffer.+  -> IO ()+collideChainSegmentAndPolygon a0 a1 a2 a3 out =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CollideChainSegmentAndPolygon a0 p1 p2 a3 out++{- | Solves the position of a mover that satisfies the given collision planes.++Binds @b2SolvePlanes@.+-}+solvePlanes+  :: Vec2+  -- ^ @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_b2SolvePlanes 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 @b2ClipVector@.+-}+clipVector+  :: Vec2+  -- ^ @vector@+  -> Ptr CollisionPlane+  -- ^ @planes@+  -> Int+  -- ^ @count@+  -> IO Vec2+clipVector a0 a1 a2 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2ClipVector p0 a1 (fromIntegral a2) pOut >> peek pOut
+ src/Box2D/Contact.hsc view
@@ -0,0 +1,82 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.Contact+  ( ContactData (..)+  , isValid+  , getData+  )+  where++import Foreign+import Foreign.C.Types (CBool (..))+import Box2D.Collision (Manifold)+import Box2D.Id (ContactId, ShapeId)++#include "box2d/box2d.h"+++{- | The contact data for two shapes. By convention the manifold normal points from shape A to shape+B.++See also @b2Shape_GetContactData()@ and @b2Body_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 @b2Contact_IsValid@ before using it for other functions.+  , shapeIdA :: ShapeId+  -- ^ The first shape id.+  , shapeIdB :: ShapeId+  -- ^ The second shape id.+  , manifold :: Manifold+  -- ^ The manifold copied from the contact.+  }+  deriving (Eq, Show)++instance Storable ContactData where+  sizeOf _ = #{size b2ContactData}+  alignment _ = #{alignment b2ContactData}+  peek p =+    ContactData+      <$> #{peek b2ContactData, contactId} p+      <*> #{peek b2ContactData, shapeIdA} p+      <*> #{peek b2ContactData, shapeIdB} p+      <*> #{peek b2ContactData, manifold} p+  poke p (ContactData f1 f2 f3 f4) = do+    #{poke b2ContactData, contactId} p f1+    #{poke b2ContactData, shapeIdA} p f2+    #{poke b2ContactData, shapeIdB} p f3+    #{poke b2ContactData, manifold} p f4+++foreign import ccall unsafe "hsg_b2Contact_IsValid"+  c_b2Contact_IsValid :: Ptr ContactId -> IO CBool++foreign import ccall unsafe "hsg_b2Contact_GetData"+  c_b2Contact_GetData :: Ptr ContactId -> Ptr ContactData -> IO ()++{- | Contact identifier validation. Provides validation for up to 2^32 allocations.++Binds @b2Contact_IsValid@.+-}+isValid+  :: ContactId+  -> IO Bool+isValid a0 =+  with a0 $ \p0 ->+  toBool <$> (c_b2Contact_IsValid p0)++{- | Get the data for a contact. The manifold may have no points if the contact is not touching.++Binds @b2Contact_GetData@.+-}+getData+  :: ContactId+  -> IO ContactData+getData a0 =+  with a0 $ \p0 ->+  alloca $ \pOut -> c_b2Contact_GetData p0 pOut >> peek pOut
+ src/Box2D/DebugDraw.hsc view
@@ -0,0 +1,606 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.DebugDraw+  ( DebugDraw (..)+  , 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 Box2D.Internal (allocaOut)+import Box2D.MathTypes (AABB)+import Box2D.Tags (DrawBoundsFcn, DrawCircleFcn, DrawLineFcn, DrawPointFcn, DrawPolygonFcn, DrawSolidCapsuleFcn, DrawSolidCircleFcn, DrawSolidPolygonFcn, DrawStringFcn, DrawTransformFcn)++#include "box2d/box2d.h"+++{- | This struct holds callbacks you can implement to draw a Box2D 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. Initialize with+@b2DefaultDebugDraw@.+-}+data DebugDraw = DebugDraw+  { drawPolygonFcn :: FunPtr DrawPolygonFcn+  -- ^ Draw a closed polygon provided in CCW order.+  , drawSolidPolygonFcn :: FunPtr DrawSolidPolygonFcn+  -- ^ Draw a solid closed polygon provided in CCW order.+  , drawCircleFcn :: FunPtr DrawCircleFcn+  -- ^ Draw a circle.+  , drawSolidCircleFcn :: FunPtr DrawSolidCircleFcn+  -- ^ Draw a solid circle.+  , drawSolidCapsuleFcn :: FunPtr DrawSolidCapsuleFcn+  -- ^ Draw a solid capsule.+  , drawLineFcn :: FunPtr DrawLineFcn+  -- ^ Draw a line segment.+  , drawTransformFcn :: FunPtr DrawTransformFcn+  -- ^ Draw a transform. Choose your own length scale.+  , drawPointFcn :: FunPtr DrawPointFcn+  -- ^ Draw a point.+  , drawStringFcn :: FunPtr DrawStringFcn+  -- ^ Draw a string in world space+  , drawBoundsFcn :: FunPtr DrawBoundsFcn+  -- ^ Draw a bounding box. With double precision enabled, the single precision bounding box gets+  -- increasing padding when moving far from the origin.+  , drawingBounds :: AABB+  -- ^ World bounds used for debug draw. An empty box draws nothing.+  , forceScale :: Float+  -- ^ Scale to use when drawing forces+  , jointScale :: Float+  -- ^ Global scaling for joint drawing+  , drawContacts :: CBool+  -- ^ Option to draw contact points+  , drawAnchorA :: CBool+  -- ^ Draw anchor A for contact points (instead of anchorB)+  , drawShapes :: CBool+  -- ^ Option to draw shapes+  , drawChainNormals :: CBool+  -- ^ Option to draw chain shape normals+  , 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+  , drawBodyNames :: CBool+  -- ^ Option to draw body names+  , drawGraphColors :: CBool+  -- ^ Option to visualize the graph coloring used for contacts and joints+  , drawContactFeatures :: CBool+  -- ^ Option to draw contact feature ids+  , drawContactNormals :: CBool+  -- ^ Option to draw contact normals+  , drawContactForces :: CBool+  -- ^ Option to draw contact normal forces+  , drawFrictionForces :: CBool+  -- ^ Option to draw contact friction forces+  , drawIslands :: CBool+  -- ^ Option to draw islands as bounding boxes+  , context :: Ptr ()+  }+  deriving (Eq, Show)++instance Storable DebugDraw where+  sizeOf _ = #{size b2DebugDraw}+  alignment _ = #{alignment b2DebugDraw}+  peek p =+    DebugDraw+      <$> #{peek b2DebugDraw, DrawPolygonFcn} p+      <*> #{peek b2DebugDraw, DrawSolidPolygonFcn} p+      <*> #{peek b2DebugDraw, DrawCircleFcn} p+      <*> #{peek b2DebugDraw, DrawSolidCircleFcn} p+      <*> #{peek b2DebugDraw, DrawSolidCapsuleFcn} p+      <*> #{peek b2DebugDraw, DrawLineFcn} p+      <*> #{peek b2DebugDraw, DrawTransformFcn} p+      <*> #{peek b2DebugDraw, DrawPointFcn} p+      <*> #{peek b2DebugDraw, DrawStringFcn} p+      <*> #{peek b2DebugDraw, DrawBoundsFcn} p+      <*> #{peek b2DebugDraw, drawingBounds} p+      <*> #{peek b2DebugDraw, forceScale} p+      <*> #{peek b2DebugDraw, jointScale} p+      <*> #{peek b2DebugDraw, drawContacts} p+      <*> #{peek b2DebugDraw, drawAnchorA} p+      <*> #{peek b2DebugDraw, drawShapes} p+      <*> #{peek b2DebugDraw, drawChainNormals} p+      <*> #{peek b2DebugDraw, drawJoints} p+      <*> #{peek b2DebugDraw, drawJointExtras} p+      <*> #{peek b2DebugDraw, drawBounds} p+      <*> #{peek b2DebugDraw, drawMass} p+      <*> #{peek b2DebugDraw, drawBodyNames} p+      <*> #{peek b2DebugDraw, drawGraphColors} p+      <*> #{peek b2DebugDraw, drawContactFeatures} p+      <*> #{peek b2DebugDraw, drawContactNormals} p+      <*> #{peek b2DebugDraw, drawContactForces} p+      <*> #{peek b2DebugDraw, drawFrictionForces} p+      <*> #{peek b2DebugDraw, drawIslands} p+      <*> #{peek b2DebugDraw, 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 f28 f29) = do+    #{poke b2DebugDraw, DrawPolygonFcn} p f1+    #{poke b2DebugDraw, DrawSolidPolygonFcn} p f2+    #{poke b2DebugDraw, DrawCircleFcn} p f3+    #{poke b2DebugDraw, DrawSolidCircleFcn} p f4+    #{poke b2DebugDraw, DrawSolidCapsuleFcn} p f5+    #{poke b2DebugDraw, DrawLineFcn} p f6+    #{poke b2DebugDraw, DrawTransformFcn} p f7+    #{poke b2DebugDraw, DrawPointFcn} p f8+    #{poke b2DebugDraw, DrawStringFcn} p f9+    #{poke b2DebugDraw, DrawBoundsFcn} p f10+    #{poke b2DebugDraw, drawingBounds} p f11+    #{poke b2DebugDraw, forceScale} p f12+    #{poke b2DebugDraw, jointScale} p f13+    #{poke b2DebugDraw, drawContacts} p f14+    #{poke b2DebugDraw, drawAnchorA} p f15+    #{poke b2DebugDraw, drawShapes} p f16+    #{poke b2DebugDraw, drawChainNormals} p f17+    #{poke b2DebugDraw, drawJoints} p f18+    #{poke b2DebugDraw, drawJointExtras} p f19+    #{poke b2DebugDraw, drawBounds} p f20+    #{poke b2DebugDraw, drawMass} p f21+    #{poke b2DebugDraw, drawBodyNames} p f22+    #{poke b2DebugDraw, drawGraphColors} p f23+    #{poke b2DebugDraw, drawContactFeatures} p f24+    #{poke b2DebugDraw, drawContactNormals} p f25+    #{poke b2DebugDraw, drawContactForces} p f26+    #{poke b2DebugDraw, drawFrictionForces} p f27+    #{poke b2DebugDraw, drawIslands} p f28+    #{poke b2DebugDraw, context} p f29++{- | 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 b2_colorAliceBlue}+pattern ColorAntiqueWhite = HexColor #{const b2_colorAntiqueWhite}+pattern ColorAqua = HexColor #{const b2_colorAqua}+pattern ColorAquamarine = HexColor #{const b2_colorAquamarine}+pattern ColorAzure = HexColor #{const b2_colorAzure}+pattern ColorBeige = HexColor #{const b2_colorBeige}+pattern ColorBisque = HexColor #{const b2_colorBisque}+pattern ColorBlack = HexColor #{const b2_colorBlack}+pattern ColorBlanchedAlmond = HexColor #{const b2_colorBlanchedAlmond}+pattern ColorBlue = HexColor #{const b2_colorBlue}+pattern ColorBlueViolet = HexColor #{const b2_colorBlueViolet}+pattern ColorBrown = HexColor #{const b2_colorBrown}+pattern ColorBurlywood = HexColor #{const b2_colorBurlywood}+pattern ColorCadetBlue = HexColor #{const b2_colorCadetBlue}+pattern ColorChartreuse = HexColor #{const b2_colorChartreuse}+pattern ColorChocolate = HexColor #{const b2_colorChocolate}+pattern ColorCoral = HexColor #{const b2_colorCoral}+pattern ColorCornflowerBlue = HexColor #{const b2_colorCornflowerBlue}+pattern ColorCornsilk = HexColor #{const b2_colorCornsilk}+pattern ColorCrimson = HexColor #{const b2_colorCrimson}+pattern ColorCyan = HexColor #{const b2_colorCyan}+pattern ColorDarkBlue = HexColor #{const b2_colorDarkBlue}+pattern ColorDarkCyan = HexColor #{const b2_colorDarkCyan}+pattern ColorDarkGoldenRod = HexColor #{const b2_colorDarkGoldenRod}+pattern ColorDarkGray = HexColor #{const b2_colorDarkGray}+pattern ColorDarkGreen = HexColor #{const b2_colorDarkGreen}+pattern ColorDarkKhaki = HexColor #{const b2_colorDarkKhaki}+pattern ColorDarkMagenta = HexColor #{const b2_colorDarkMagenta}+pattern ColorDarkOliveGreen = HexColor #{const b2_colorDarkOliveGreen}+pattern ColorDarkOrange = HexColor #{const b2_colorDarkOrange}+pattern ColorDarkOrchid = HexColor #{const b2_colorDarkOrchid}+pattern ColorDarkRed = HexColor #{const b2_colorDarkRed}+pattern ColorDarkSalmon = HexColor #{const b2_colorDarkSalmon}+pattern ColorDarkSeaGreen = HexColor #{const b2_colorDarkSeaGreen}+pattern ColorDarkSlateBlue = HexColor #{const b2_colorDarkSlateBlue}+pattern ColorDarkSlateGray = HexColor #{const b2_colorDarkSlateGray}+pattern ColorDarkTurquoise = HexColor #{const b2_colorDarkTurquoise}+pattern ColorDarkViolet = HexColor #{const b2_colorDarkViolet}+pattern ColorDeepPink = HexColor #{const b2_colorDeepPink}+pattern ColorDeepSkyBlue = HexColor #{const b2_colorDeepSkyBlue}+pattern ColorDimGray = HexColor #{const b2_colorDimGray}+pattern ColorDodgerBlue = HexColor #{const b2_colorDodgerBlue}+pattern ColorFireBrick = HexColor #{const b2_colorFireBrick}+pattern ColorFloralWhite = HexColor #{const b2_colorFloralWhite}+pattern ColorForestGreen = HexColor #{const b2_colorForestGreen}+pattern ColorFuchsia = HexColor #{const b2_colorFuchsia}+pattern ColorGainsboro = HexColor #{const b2_colorGainsboro}+pattern ColorGhostWhite = HexColor #{const b2_colorGhostWhite}+pattern ColorGold = HexColor #{const b2_colorGold}+pattern ColorGoldenRod = HexColor #{const b2_colorGoldenRod}+pattern ColorGray = HexColor #{const b2_colorGray}+pattern ColorGreen = HexColor #{const b2_colorGreen}+pattern ColorGreenYellow = HexColor #{const b2_colorGreenYellow}+pattern ColorHoneyDew = HexColor #{const b2_colorHoneyDew}+pattern ColorHotPink = HexColor #{const b2_colorHotPink}+pattern ColorIndianRed = HexColor #{const b2_colorIndianRed}+pattern ColorIndigo = HexColor #{const b2_colorIndigo}+pattern ColorIvory = HexColor #{const b2_colorIvory}+pattern ColorKhaki = HexColor #{const b2_colorKhaki}+pattern ColorLavender = HexColor #{const b2_colorLavender}+pattern ColorLavenderBlush = HexColor #{const b2_colorLavenderBlush}+pattern ColorLawnGreen = HexColor #{const b2_colorLawnGreen}+pattern ColorLemonChiffon = HexColor #{const b2_colorLemonChiffon}+pattern ColorLightBlue = HexColor #{const b2_colorLightBlue}+pattern ColorLightCoral = HexColor #{const b2_colorLightCoral}+pattern ColorLightCyan = HexColor #{const b2_colorLightCyan}+pattern ColorLightGoldenRodYellow = HexColor #{const b2_colorLightGoldenRodYellow}+pattern ColorLightGray = HexColor #{const b2_colorLightGray}+pattern ColorLightGreen = HexColor #{const b2_colorLightGreen}+pattern ColorLightPink = HexColor #{const b2_colorLightPink}+pattern ColorLightSalmon = HexColor #{const b2_colorLightSalmon}+pattern ColorLightSeaGreen = HexColor #{const b2_colorLightSeaGreen}+pattern ColorLightSkyBlue = HexColor #{const b2_colorLightSkyBlue}+pattern ColorLightSlateGray = HexColor #{const b2_colorLightSlateGray}+pattern ColorLightSteelBlue = HexColor #{const b2_colorLightSteelBlue}+pattern ColorLightYellow = HexColor #{const b2_colorLightYellow}+pattern ColorLime = HexColor #{const b2_colorLime}+pattern ColorLimeGreen = HexColor #{const b2_colorLimeGreen}+pattern ColorLinen = HexColor #{const b2_colorLinen}+pattern ColorMagenta = HexColor #{const b2_colorMagenta}+pattern ColorMaroon = HexColor #{const b2_colorMaroon}+pattern ColorMediumAquaMarine = HexColor #{const b2_colorMediumAquaMarine}+pattern ColorMediumBlue = HexColor #{const b2_colorMediumBlue}+pattern ColorMediumOrchid = HexColor #{const b2_colorMediumOrchid}+pattern ColorMediumPurple = HexColor #{const b2_colorMediumPurple}+pattern ColorMediumSeaGreen = HexColor #{const b2_colorMediumSeaGreen}+pattern ColorMediumSlateBlue = HexColor #{const b2_colorMediumSlateBlue}+pattern ColorMediumSpringGreen = HexColor #{const b2_colorMediumSpringGreen}+pattern ColorMediumTurquoise = HexColor #{const b2_colorMediumTurquoise}+pattern ColorMediumVioletRed = HexColor #{const b2_colorMediumVioletRed}+pattern ColorMidnightBlue = HexColor #{const b2_colorMidnightBlue}+pattern ColorMintCream = HexColor #{const b2_colorMintCream}+pattern ColorMistyRose = HexColor #{const b2_colorMistyRose}+pattern ColorMoccasin = HexColor #{const b2_colorMoccasin}+pattern ColorNavajoWhite = HexColor #{const b2_colorNavajoWhite}+pattern ColorNavy = HexColor #{const b2_colorNavy}+pattern ColorOldLace = HexColor #{const b2_colorOldLace}+pattern ColorOlive = HexColor #{const b2_colorOlive}+pattern ColorOliveDrab = HexColor #{const b2_colorOliveDrab}+pattern ColorOrange = HexColor #{const b2_colorOrange}+pattern ColorOrangeRed = HexColor #{const b2_colorOrangeRed}+pattern ColorOrchid = HexColor #{const b2_colorOrchid}+pattern ColorPaleGoldenRod = HexColor #{const b2_colorPaleGoldenRod}+pattern ColorPaleGreen = HexColor #{const b2_colorPaleGreen}+pattern ColorPaleTurquoise = HexColor #{const b2_colorPaleTurquoise}+pattern ColorPaleVioletRed = HexColor #{const b2_colorPaleVioletRed}+pattern ColorPapayaWhip = HexColor #{const b2_colorPapayaWhip}+pattern ColorPeachPuff = HexColor #{const b2_colorPeachPuff}+pattern ColorPeru = HexColor #{const b2_colorPeru}+pattern ColorPink = HexColor #{const b2_colorPink}+pattern ColorPlum = HexColor #{const b2_colorPlum}+pattern ColorPowderBlue = HexColor #{const b2_colorPowderBlue}+pattern ColorPurple = HexColor #{const b2_colorPurple}+pattern ColorRebeccaPurple = HexColor #{const b2_colorRebeccaPurple}+pattern ColorRed = HexColor #{const b2_colorRed}+pattern ColorRosyBrown = HexColor #{const b2_colorRosyBrown}+pattern ColorRoyalBlue = HexColor #{const b2_colorRoyalBlue}+pattern ColorSaddleBrown = HexColor #{const b2_colorSaddleBrown}+pattern ColorSalmon = HexColor #{const b2_colorSalmon}+pattern ColorSandyBrown = HexColor #{const b2_colorSandyBrown}+pattern ColorSeaGreen = HexColor #{const b2_colorSeaGreen}+pattern ColorSeaShell = HexColor #{const b2_colorSeaShell}+pattern ColorSienna = HexColor #{const b2_colorSienna}+pattern ColorSilver = HexColor #{const b2_colorSilver}+pattern ColorSkyBlue = HexColor #{const b2_colorSkyBlue}+pattern ColorSlateBlue = HexColor #{const b2_colorSlateBlue}+pattern ColorSlateGray = HexColor #{const b2_colorSlateGray}+pattern ColorSnow = HexColor #{const b2_colorSnow}+pattern ColorSpringGreen = HexColor #{const b2_colorSpringGreen}+pattern ColorSteelBlue = HexColor #{const b2_colorSteelBlue}+pattern ColorTan = HexColor #{const b2_colorTan}+pattern ColorTeal = HexColor #{const b2_colorTeal}+pattern ColorThistle = HexColor #{const b2_colorThistle}+pattern ColorTomato = HexColor #{const b2_colorTomato}+pattern ColorTurquoise = HexColor #{const b2_colorTurquoise}+pattern ColorViolet = HexColor #{const b2_colorViolet}+pattern ColorWheat = HexColor #{const b2_colorWheat}+pattern ColorWhite = HexColor #{const b2_colorWhite}+pattern ColorWhiteSmoke = HexColor #{const b2_colorWhiteSmoke}+pattern ColorYellow = HexColor #{const b2_colorYellow}+pattern ColorYellowGreen = HexColor #{const b2_colorYellowGreen}+pattern ColorBox2DRed = HexColor #{const b2_colorBox2DRed}+pattern ColorBox2DBlue = HexColor #{const b2_colorBox2DBlue}+pattern ColorBox2DGreen = HexColor #{const b2_colorBox2DGreen}+pattern ColorBox2DYellow = HexColor #{const b2_colorBox2DYellow}++foreign import ccall unsafe "hs_b2DefaultDebugDraw"+  c_b2DefaultDebugDraw :: Ptr DebugDraw -> IO ()++defaultDebugDraw :: IO DebugDraw+defaultDebugDraw = allocaOut c_b2DefaultDebugDraw+++
+ src/Box2D/Distance.hsc view
@@ -0,0 +1,370 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.Distance+  ( TOIOutput (..)+  , TOIState (..)+  , pattern ToiStateUnknown+  , pattern ToiStateFailed+  , pattern ToiStateOverlapped+  , pattern ToiStateHit+  , pattern ToiStateSeparated+  , TOIInput (..)+  , Sweep (..)+  , ShapeCastPairInput (..)+  , Simplex (..)+  , SimplexVertex (..)+  , DistanceOutput (..)+  , DistanceInput (..)+  , SimplexCache (..)+  , SegmentDistanceResult (..)+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Data.Vector.Storable qualified as VS+import Box2D.Internal (copyBounded, pokeBounded)+import Box2D.Geometry (ShapeProxy)+import Box2D.MathTypes (Rot, Transform, Vec2)++#include "box2d/box2d.h"+++-- | Time of impact output+data TOIOutput = TOIOutput+  { state :: CInt+  -- ^ The type of result+  , point :: Vec2+  -- ^ The hit point+  , normal :: Vec2+  -- ^ The hit normal+  , fraction :: Float+  -- ^ The sweep time of the collision+  }+  deriving (Eq, Show)++instance Storable TOIOutput where+  sizeOf _ = #{size b2TOIOutput}+  alignment _ = #{alignment b2TOIOutput}+  peek p =+    TOIOutput+      <$> #{peek b2TOIOutput, state} p+      <*> #{peek b2TOIOutput, point} p+      <*> #{peek b2TOIOutput, normal} p+      <*> #{peek b2TOIOutput, fraction} p+  poke p (TOIOutput f1 f2 f3 f4) = do+    #{poke b2TOIOutput, state} p f1+    #{poke b2TOIOutput, point} p f2+    #{poke b2TOIOutput, normal} p f3+    #{poke b2TOIOutput, fraction} p f4++-- | 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 b2_toiStateUnknown}+pattern ToiStateFailed = TOIState #{const b2_toiStateFailed}+pattern ToiStateOverlapped = TOIState #{const b2_toiStateOverlapped}+pattern ToiStateHit = TOIState #{const b2_toiStateHit}+pattern ToiStateSeparated = TOIState #{const b2_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, maxFraction\]+  }+  deriving (Eq, Show)++instance Storable TOIInput where+  sizeOf _ = #{size b2TOIInput}+  alignment _ = #{alignment b2TOIInput}+  peek p =+    TOIInput+      <$> #{peek b2TOIInput, proxyA} p+      <*> #{peek b2TOIInput, proxyB} p+      <*> #{peek b2TOIInput, sweepA} p+      <*> #{peek b2TOIInput, sweepB} p+      <*> #{peek b2TOIInput, maxFraction} p+  poke p (TOIInput f1 f2 f3 f4 f5) = do+    #{poke b2TOIInput, proxyA} p f1+    #{poke b2TOIInput, proxyB} p f2+    #{poke b2TOIInput, sweepA} p f3+    #{poke b2TOIInput, sweepB} p f4+    #{poke b2TOIInput, 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 :: Vec2+  -- ^ Local center of mass position+  , c1 :: Vec2+  -- ^ Starting center of mass world position+  , c2 :: Vec2+  -- ^ Ending center of mass world position+  , q1 :: Rot+  -- ^ Starting world rotation+  , q2 :: Rot+  -- ^ Ending world rotation+  }+  deriving (Eq, Show)++instance Storable Sweep where+  sizeOf _ = #{size b2Sweep}+  alignment _ = #{alignment b2Sweep}+  peek p =+    Sweep+      <$> #{peek b2Sweep, localCenter} p+      <*> #{peek b2Sweep, c1} p+      <*> #{peek b2Sweep, c2} p+      <*> #{peek b2Sweep, q1} p+      <*> #{peek b2Sweep, q2} p+  poke p (Sweep f1 f2 f3 f4 f5) = do+    #{poke b2Sweep, localCenter} p f1+    #{poke b2Sweep, c1} p f2+    #{poke b2Sweep, c2} p f3+    #{poke b2Sweep, q1} p f4+    #{poke b2Sweep, q2} p f5++-- | Input parameters for @b2CastShape@+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 :: Vec2+  -- ^ 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 b2ShapeCastPairInput}+  alignment _ = #{alignment b2ShapeCastPairInput}+  peek p =+    ShapeCastPairInput+      <$> #{peek b2ShapeCastPairInput, proxyA} p+      <*> #{peek b2ShapeCastPairInput, proxyB} p+      <*> #{peek b2ShapeCastPairInput, transform} p+      <*> #{peek b2ShapeCastPairInput, translationB} p+      <*> #{peek b2ShapeCastPairInput, maxFraction} p+      <*> #{peek b2ShapeCastPairInput, canEncroach} p+  poke p (ShapeCastPairInput f1 f2 f3 f4 f5 f6) = do+    #{poke b2ShapeCastPairInput, proxyA} p f1+    #{poke b2ShapeCastPairInput, proxyB} p f2+    #{poke b2ShapeCastPairInput, transform} p f3+    #{poke b2ShapeCastPairInput, translationB} p f4+    #{poke b2ShapeCastPairInput, maxFraction} p f5+    #{poke b2ShapeCastPairInput, canEncroach} p f6++-- | Simplex from the GJK algorithm+data Simplex = Simplex+  { v1 :: SimplexVertex+  -- ^ vertices+  , v2 :: SimplexVertex+  -- ^ vertices+  , v3 :: SimplexVertex+  -- ^ vertices+  , count :: CInt+  -- ^ number of valid vertices+  }+  deriving (Eq, Show)++instance Storable Simplex where+  sizeOf _ = #{size b2Simplex}+  alignment _ = #{alignment b2Simplex}+  peek p =+    Simplex+      <$> #{peek b2Simplex, v1} p+      <*> #{peek b2Simplex, v2} p+      <*> #{peek b2Simplex, v3} p+      <*> #{peek b2Simplex, count} p+  poke p (Simplex f1 f2 f3 f4) = do+    #{poke b2Simplex, v1} p f1+    #{poke b2Simplex, v2} p f2+    #{poke b2Simplex, v3} p f3+    #{poke b2Simplex, count} p f4++-- | Simplex vertex for debugging the GJK algorithm+data SimplexVertex = SimplexVertex+  { wA :: Vec2+  -- ^ support point in proxyA+  , wB :: Vec2+  -- ^ support point in proxyB+  , w :: Vec2+  -- ^ wB - wA+  , a :: Float+  -- ^ barycentric coordinate for closest point+  , indexA :: CInt+  -- ^ wA index+  , indexB :: CInt+  -- ^ wB index+  }+  deriving (Eq, Show)++instance Storable SimplexVertex where+  sizeOf _ = #{size b2SimplexVertex}+  alignment _ = #{alignment b2SimplexVertex}+  peek p =+    SimplexVertex+      <$> #{peek b2SimplexVertex, wA} p+      <*> #{peek b2SimplexVertex, wB} p+      <*> #{peek b2SimplexVertex, w} p+      <*> #{peek b2SimplexVertex, a} p+      <*> #{peek b2SimplexVertex, indexA} p+      <*> #{peek b2SimplexVertex, indexB} p+  poke p (SimplexVertex f1 f2 f3 f4 f5 f6) = do+    #{poke b2SimplexVertex, wA} p f1+    #{poke b2SimplexVertex, wB} p f2+    #{poke b2SimplexVertex, w} p f3+    #{poke b2SimplexVertex, a} p f4+    #{poke b2SimplexVertex, indexA} p f5+    #{poke b2SimplexVertex, indexB} p f6++-- | Output for @b2ComputeShapeDistance@+data DistanceOutput = DistanceOutput+  { pointA :: Vec2+  -- ^ Closest point on shapeA, in shape A\'s frame+  , pointB :: Vec2+  -- ^ Closest point on shapeB, in shape A\'s frame+  , normal :: Vec2+  -- ^ 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 b2DistanceOutput}+  alignment _ = #{alignment b2DistanceOutput}+  peek p =+    DistanceOutput+      <$> #{peek b2DistanceOutput, pointA} p+      <*> #{peek b2DistanceOutput, pointB} p+      <*> #{peek b2DistanceOutput, normal} p+      <*> #{peek b2DistanceOutput, distance} p+      <*> #{peek b2DistanceOutput, iterations} p+      <*> #{peek b2DistanceOutput, simplexCount} p+  poke p (DistanceOutput f1 f2 f3 f4 f5 f6) = do+    #{poke b2DistanceOutput, pointA} p f1+    #{poke b2DistanceOutput, pointB} p f2+    #{poke b2DistanceOutput, normal} p f3+    #{poke b2DistanceOutput, distance} p f4+    #{poke b2DistanceOutput, iterations} p f5+    #{poke b2DistanceOutput, simplexCount} p f6++-- | Input for @b2ComputeShapeDistance@+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 (b2InvMulTransforms(+  -- 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 b2DistanceInput}+  alignment _ = #{alignment b2DistanceInput}+  peek p =+    DistanceInput+      <$> #{peek b2DistanceInput, proxyA} p+      <*> #{peek b2DistanceInput, proxyB} p+      <*> #{peek b2DistanceInput, transform} p+      <*> #{peek b2DistanceInput, useRadii} p+  poke p (DistanceInput f1 f2 f3 f4) = do+    #{poke b2DistanceInput, proxyA} p f1+    #{poke b2DistanceInput, proxyB} p f2+    #{poke b2DistanceInput, transform} p f3+    #{poke b2DistanceInput, useRadii} p f4++{- | Used to warm start the GJK simplex. If you call @b2ComputeShapeDistance@ multiple times with+nearby transforms this might improve performance. The distance cache must be initialized to zero+on the first call.+-}+data SimplexCache = SimplexCache+  { 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 b2SimplexCache}+  alignment _ = #{alignment b2SimplexCache}+  peek p = do+    nCount <- #{peek b2SimplexCache, count} p :: IO Word16+    SimplexCache+      <$> copyBounded (#{const 3}) nCount (#{ptr b2SimplexCache, indexA} p)+      <*> copyBounded (#{const 3}) nCount (#{ptr b2SimplexCache, indexB} p)+  poke p x = do+    pokeBounded (#{const 3}) (#{ptr b2SimplexCache, indexA} p) x.indexA+    pokeBounded (#{const 3}) (#{ptr b2SimplexCache, indexB} p) x.indexB+    #{poke b2SimplexCache, count} p (fromIntegral (minimum [(#{const 3}), VS.length x.indexA, VS.length x.indexB]) :: Word16)++-- | Result of computing the distance between two line segments+data SegmentDistanceResult = SegmentDistanceResult+  { closest1 :: Vec2+  -- ^ The closest point on the first segment+  , closest2 :: Vec2+  -- ^ The closest point on the second segment+  , fraction1 :: Float+  -- ^ The barycentric coordinate on the first segment+  , fraction2 :: Float+  -- ^ The barycentric coordinate on the second segment+  , distanceSquared :: Float+  -- ^ The squared distance between the closest points+  }+  deriving (Eq, Show)++instance Storable SegmentDistanceResult where+  sizeOf _ = #{size b2SegmentDistanceResult}+  alignment _ = #{alignment b2SegmentDistanceResult}+  peek p =+    SegmentDistanceResult+      <$> #{peek b2SegmentDistanceResult, closest1} p+      <*> #{peek b2SegmentDistanceResult, closest2} p+      <*> #{peek b2SegmentDistanceResult, fraction1} p+      <*> #{peek b2SegmentDistanceResult, fraction2} p+      <*> #{peek b2SegmentDistanceResult, distanceSquared} p+  poke p (SegmentDistanceResult f1 f2 f3 f4 f5) = do+    #{poke b2SegmentDistanceResult, closest1} p f1+    #{poke b2SegmentDistanceResult, closest2} p f2+    #{poke b2SegmentDistanceResult, fraction1} p f3+    #{poke b2SegmentDistanceResult, fraction2} p f4+    #{poke b2SegmentDistanceResult, distanceSquared} p f5+++
+ src/Box2D/DistanceJoint.hsc view
@@ -0,0 +1,482 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.DistanceJoint+  ( DistanceJointDef (..)+  , defaultDistanceJointDef+  , create+  , setRestLength+  , getRestLength+  , enableSpring+  , isSpringEnabled+  , setSpringForceRange+  , getSpringForceRange+  , setSpringHertz+  , setSpringDampingRatio+  , getSpringHertz+  , getSpringDampingRatio+  , getSpringForce+  , enableLimit+  , isLimitEnabled+  , setLimits+  , getLowerLimit+  , getUpperLimit+  , getCurrentLength+  , enableMotor+  , isMotorEnabled+  , setTargetMotorSpeed+  , getTargetMotorSpeed+  , setMaxMotorForce+  , getMaxMotorForce+  , getCurrentMotorForce+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (JointId (..), WorldId (..))+import Box2D.Joint (JointDef)++#include "box2d/box2d.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 for limit. Clamped to a stable minimum value.+  , maxLength :: Float+  -- ^ Maximum length for limit. 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+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable DistanceJointDef where+  sizeOf _ = #{size b2DistanceJointDef}+  alignment _ = #{alignment b2DistanceJointDef}+  peek p =+    DistanceJointDef+      <$> #{peek b2DistanceJointDef, base} p+      <*> #{peek b2DistanceJointDef, length} p+      <*> #{peek b2DistanceJointDef, enableSpring} p+      <*> #{peek b2DistanceJointDef, lowerSpringForce} p+      <*> #{peek b2DistanceJointDef, upperSpringForce} p+      <*> #{peek b2DistanceJointDef, hertz} p+      <*> #{peek b2DistanceJointDef, dampingRatio} p+      <*> #{peek b2DistanceJointDef, enableLimit} p+      <*> #{peek b2DistanceJointDef, minLength} p+      <*> #{peek b2DistanceJointDef, maxLength} p+      <*> #{peek b2DistanceJointDef, enableMotor} p+      <*> #{peek b2DistanceJointDef, maxMotorForce} p+      <*> #{peek b2DistanceJointDef, motorSpeed} p+      <*> #{peek b2DistanceJointDef, internalValue} p+  poke p (DistanceJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13 f14) = do+    #{poke b2DistanceJointDef, base} p f1+    #{poke b2DistanceJointDef, length} p f2+    #{poke b2DistanceJointDef, enableSpring} p f3+    #{poke b2DistanceJointDef, lowerSpringForce} p f4+    #{poke b2DistanceJointDef, upperSpringForce} p f5+    #{poke b2DistanceJointDef, hertz} p f6+    #{poke b2DistanceJointDef, dampingRatio} p f7+    #{poke b2DistanceJointDef, enableLimit} p f8+    #{poke b2DistanceJointDef, minLength} p f9+    #{poke b2DistanceJointDef, maxLength} p f10+    #{poke b2DistanceJointDef, enableMotor} p f11+    #{poke b2DistanceJointDef, maxMotorForce} p f12+    #{poke b2DistanceJointDef, motorSpeed} p f13+    #{poke b2DistanceJointDef, internalValue} p f14++foreign import ccall unsafe "hs_b2DefaultDistanceJointDef"+  c_b2DefaultDistanceJointDef :: Ptr DistanceJointDef -> IO ()++defaultDistanceJointDef :: IO DistanceJointDef+defaultDistanceJointDef = allocaOut c_b2DefaultDistanceJointDef+++foreign import ccall unsafe "b2CreateDistanceJoint"+  c_b2CreateDistanceJoint :: WorldId -> Ptr DistanceJointDef -> IO JointId++foreign import ccall unsafe "b2DistanceJoint_SetRestLength"+  c_b2DistanceJoint_SetRestLength :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2DistanceJoint_GetRestLength"+  c_b2DistanceJoint_GetRestLength :: JointId -> IO Float++foreign import ccall unsafe "b2DistanceJoint_EnableSpring"+  c_b2DistanceJoint_EnableSpring :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2DistanceJoint_IsSpringEnabled"+  c_b2DistanceJoint_IsSpringEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2DistanceJoint_SetSpringForceRange"+  c_b2DistanceJoint_SetSpringForceRange :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b2DistanceJoint_GetSpringForceRange"+  c_b2DistanceJoint_GetSpringForceRange :: JointId -> Ptr Float -> Ptr Float -> IO ()++foreign import ccall unsafe "b2DistanceJoint_SetSpringHertz"+  c_b2DistanceJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2DistanceJoint_SetSpringDampingRatio"+  c_b2DistanceJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2DistanceJoint_GetSpringHertz"+  c_b2DistanceJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b2DistanceJoint_GetSpringDampingRatio"+  c_b2DistanceJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b2DistanceJoint_GetSpringForce"+  c_b2DistanceJoint_GetSpringForce :: JointId -> IO Float++foreign import ccall unsafe "b2DistanceJoint_EnableLimit"+  c_b2DistanceJoint_EnableLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2DistanceJoint_IsLimitEnabled"+  c_b2DistanceJoint_IsLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2DistanceJoint_SetLimits"+  c_b2DistanceJoint_SetLimits :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b2DistanceJoint_GetLowerLimit"+  c_b2DistanceJoint_GetLowerLimit :: JointId -> IO Float++foreign import ccall unsafe "b2DistanceJoint_GetUpperLimit"+  c_b2DistanceJoint_GetUpperLimit :: JointId -> IO Float++foreign import ccall unsafe "b2DistanceJoint_GetCurrentLength"+  c_b2DistanceJoint_GetCurrentLength :: JointId -> IO Float++foreign import ccall unsafe "b2DistanceJoint_EnableMotor"+  c_b2DistanceJoint_EnableMotor :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2DistanceJoint_IsMotorEnabled"+  c_b2DistanceJoint_IsMotorEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2DistanceJoint_SetTargetMotorSpeed"+  c_b2DistanceJoint_SetTargetMotorSpeed :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2DistanceJoint_GetTargetMotorSpeed"+  c_b2DistanceJoint_GetTargetMotorSpeed :: JointId -> IO Float++foreign import ccall unsafe "b2DistanceJoint_SetMaxMotorForce"+  c_b2DistanceJoint_SetMaxMotorForce :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2DistanceJoint_GetMaxMotorForce"+  c_b2DistanceJoint_GetMaxMotorForce :: JointId -> IO Float++foreign import ccall unsafe "b2DistanceJoint_GetCurrentMotorForce"+  c_b2DistanceJoint_GetCurrentMotorForce :: JointId -> IO Float++{- | Create a distance joint++See also @b2DistanceJointDef@ for details.++Binds @b2CreateDistanceJoint@.+-}+create+  :: WorldId+  -> DistanceJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b2CreateDistanceJoint a0 p1++{- | Set the rest length of a distance joint++Binds @b2DistanceJoint_SetRestLength@.+-}+setRestLength+  :: JointId+  -- ^ @jointId@: The id for a distance joint+  -> Float+  -- ^ @length@: The new distance joint length+  -> IO ()+setRestLength a0 a1 =+  c_b2DistanceJoint_SetRestLength a0 a1++{- | Get the rest length of a distance joint++Binds @b2DistanceJoint_GetRestLength@.+-}+getRestLength+  :: JointId+  -> IO Float+getRestLength a0 =+  c_b2DistanceJoint_GetRestLength a0++{- | Enable\/disable the distance joint spring. When disabled the distance joint is rigid.++Binds @b2DistanceJoint_EnableSpring@.+-}+enableSpring+  :: JointId+  -> Bool+  -- ^ @enableSpring@+  -> IO ()+enableSpring a0 a1 =+  c_b2DistanceJoint_EnableSpring a0 (fromBool a1)++{- | Is the distance joint spring enabled?++Binds @b2DistanceJoint_IsSpringEnabled@.+-}+isSpringEnabled+  :: JointId+  -> IO Bool+isSpringEnabled a0 =+  toBool <$> (c_b2DistanceJoint_IsSpringEnabled a0)++{- | Set the force range for the spring.++Binds @b2DistanceJoint_SetSpringForceRange@.+-}+setSpringForceRange+  :: JointId+  -> Float+  -- ^ @lowerForce@+  -> Float+  -- ^ @upperForce@+  -> IO ()+setSpringForceRange a0 a1 a2 =+  c_b2DistanceJoint_SetSpringForceRange a0 a1 a2++{- | Get the force range for the spring.++Binds @b2DistanceJoint_GetSpringForceRange@.+-}+getSpringForceRange+  :: JointId+  -> Ptr Float+  -- ^ @lowerForce@+  -> Ptr Float+  -- ^ @upperForce@+  -> IO ()+getSpringForceRange a0 a1 a2 =+  c_b2DistanceJoint_GetSpringForceRange a0 a1 a2++{- | Set the spring stiffness in Hertz++Binds @b2DistanceJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b2DistanceJoint_SetSpringHertz a0 a1++{- | Set the spring damping ratio, non-dimensional++Binds @b2DistanceJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b2DistanceJoint_SetSpringDampingRatio a0 a1++{- | Get the spring Hertz++Binds @b2DistanceJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b2DistanceJoint_GetSpringHertz a0++{- | Get the spring damping ratio++Binds @b2DistanceJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b2DistanceJoint_GetSpringDampingRatio a0++{- | Get the current spring force.++Binds @b2DistanceJoint_GetSpringForce@.+-}+getSpringForce+  :: JointId+  -> IO Float+getSpringForce a0 =+  c_b2DistanceJoint_GetSpringForce 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 @b2DistanceJoint_EnableLimit@.+-}+enableLimit+  :: JointId+  -> Bool+  -- ^ @enableLimit@+  -> IO ()+enableLimit a0 a1 =+  c_b2DistanceJoint_EnableLimit a0 (fromBool a1)++{- | Is the distance joint limit enabled?++Binds @b2DistanceJoint_IsLimitEnabled@.+-}+isLimitEnabled+  :: JointId+  -> IO Bool+isLimitEnabled a0 =+  toBool <$> (c_b2DistanceJoint_IsLimitEnabled a0)++{- | Set the minimum and maximum length parameters of a distance joint++Binds @b2DistanceJoint_SetLimits@.+-}+setLimits+  :: JointId+  -> Float+  -- ^ @minLength@+  -> Float+  -- ^ @maxLength@+  -> IO ()+setLimits a0 a1 a2 =+  c_b2DistanceJoint_SetLimits a0 a1 a2++{- | Get the distance joint minimum length++Binds @b2DistanceJoint_GetLowerLimit@.+-}+getLowerLimit+  :: JointId+  -> IO Float+getLowerLimit a0 =+  c_b2DistanceJoint_GetLowerLimit a0++{- | Get the distance joint maximum length++Binds @b2DistanceJoint_GetUpperLimit@.+-}+getUpperLimit+  :: JointId+  -> IO Float+getUpperLimit a0 =+  c_b2DistanceJoint_GetUpperLimit a0++{- | Get the current length of a distance joint++Binds @b2DistanceJoint_GetCurrentLength@.+-}+getCurrentLength+  :: JointId+  -> IO Float+getCurrentLength a0 =+  c_b2DistanceJoint_GetCurrentLength a0++{- | Enable\/disable the distance joint motor++Binds @b2DistanceJoint_EnableMotor@.+-}+enableMotor+  :: JointId+  -> Bool+  -- ^ @enableMotor@+  -> IO ()+enableMotor a0 a1 =+  c_b2DistanceJoint_EnableMotor a0 (fromBool a1)++{- | Is the distance joint motor enabled?++Binds @b2DistanceJoint_IsMotorEnabled@.+-}+isMotorEnabled+  :: JointId+  -> IO Bool+isMotorEnabled a0 =+  toBool <$> (c_b2DistanceJoint_IsMotorEnabled a0)++{- | Set the distance joint motor speed, usually in meters per second++Binds @b2DistanceJoint_SetTargetMotorSpeed@.+-}+setTargetMotorSpeed+  :: JointId+  -> Float+  -- ^ @motorSpeed@+  -> IO ()+setTargetMotorSpeed a0 a1 =+  c_b2DistanceJoint_SetTargetMotorSpeed a0 a1++{- | Get the distance joint motor speed, usually in meters per second++Binds @b2DistanceJoint_GetTargetMotorSpeed@.+-}+getTargetMotorSpeed+  :: JointId+  -> IO Float+getTargetMotorSpeed a0 =+  c_b2DistanceJoint_GetTargetMotorSpeed a0++{- | Set the distance joint maximum motor force, usually in newtons++Binds @b2DistanceJoint_SetMaxMotorForce@.+-}+setMaxMotorForce+  :: JointId+  -> Float+  -- ^ @force@+  -> IO ()+setMaxMotorForce a0 a1 =+  c_b2DistanceJoint_SetMaxMotorForce a0 a1++{- | Get the distance joint maximum motor force, usually in newtons++Binds @b2DistanceJoint_GetMaxMotorForce@.+-}+getMaxMotorForce+  :: JointId+  -> IO Float+getMaxMotorForce a0 =+  c_b2DistanceJoint_GetMaxMotorForce a0++{- | Get the distance joint current motor force, usually in newtons++Binds @b2DistanceJoint_GetCurrentMotorForce@.+-}+getCurrentMotorForce+  :: JointId+  -> IO Float+getCurrentMotorForce a0 =+  c_b2DistanceJoint_GetCurrentMotorForce a0
+ src/Box2D/DynamicTree.hsc view
@@ -0,0 +1,442 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.DynamicTree+  ( BoxCastInput (..)+  , TreeStats (..)+  , create+  , destroy+  , createProxy+  , destroyProxy+  , moveProxy+  , setCategoryBits+  , getCategoryBits+  , overlapAABB+  , overlapAABBAll+  , castRay+  , castBox+  , castBoxInto+  , getHeight+  , getAreaRatio+  , getRootBounds+  , getRootBoundsInto+  , getProxyCount+  , getByteCount+  , getUserData+  , getProxyBounds+  , getProxyBoundsInto+  , validate+  )+  where++import Foreign+import Foreign.C.Types (CInt (..), CSize (..))+import Box2D.Geometry (RayCastInput)+import Box2D.MathTypes (AABB, Vec2)+import Box2D.Tags (DynamicTree, TreeBoxCastCallbackFcn, TreeQueryCallbackFcn, TreeRayCastCallbackFcn)++#include "box2d/box2d.h"+++-- | Input for casting an AABB through a dynamic tree+data BoxCastInput = BoxCastInput+  { box :: AABB+  -- ^ The AABB to cast, in the tree\'s frame.+  , translation :: Vec2+  -- ^ The sweep translation.+  , maxFraction :: Float+  -- ^ The maximum fraction of the translation to consider, typically 1.+  }+  deriving (Eq, Show)++instance Storable BoxCastInput where+  sizeOf _ = #{size b2BoxCastInput}+  alignment _ = #{alignment b2BoxCastInput}+  peek p =+    BoxCastInput+      <$> #{peek b2BoxCastInput, box} p+      <*> #{peek b2BoxCastInput, translation} p+      <*> #{peek b2BoxCastInput, maxFraction} p+  poke p (BoxCastInput f1 f2 f3) = do+    #{poke b2BoxCastInput, box} p f1+    #{poke b2BoxCastInput, translation} p f2+    #{poke b2BoxCastInput, maxFraction} p f3++-- | 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 b2TreeStats}+  alignment _ = #{alignment b2TreeStats}+  peek p =+    TreeStats+      <$> #{peek b2TreeStats, nodeVisits} p+      <*> #{peek b2TreeStats, leafVisits} p+  poke p (TreeStats f1 f2) = do+    #{poke b2TreeStats, nodeVisits} p f1+    #{poke b2TreeStats, leafVisits} p f2+++foreign import ccall unsafe "b2CreateDynamicTree"+  c_b2CreateDynamicTree :: CInt -> IO (Ptr DynamicTree)++foreign import ccall unsafe "b2DestroyDynamicTree"+  c_b2DestroyDynamicTree :: Ptr DynamicTree -> IO ()++foreign import ccall unsafe "hsg_b2CreateTreeProxy"+  c_b2CreateTreeProxy :: Ptr DynamicTree -> Ptr AABB -> Word64 -> Word64 -> IO CInt++foreign import ccall unsafe "b2DestroyTreeProxy"+  c_b2DestroyTreeProxy :: Ptr DynamicTree -> CInt -> IO ()++foreign import ccall unsafe "hsg_b2DynamicTree_MoveProxy"+  c_b2DynamicTree_MoveProxy :: Ptr DynamicTree -> CInt -> Ptr AABB -> IO ()++foreign import ccall unsafe "b2DynamicTree_SetCategoryBits"+  c_b2DynamicTree_SetCategoryBits :: Ptr DynamicTree -> CInt -> Word64 -> IO ()++foreign import ccall unsafe "b2DynamicTree_GetCategoryBits"+  c_b2DynamicTree_GetCategoryBits :: Ptr DynamicTree -> CInt -> IO Word64++foreign import ccall safe "hsg_b2DynamicTree_OverlapAABB"+  c_b2DynamicTree_OverlapAABB :: Ptr DynamicTree -> Ptr AABB -> Word64 -> FunPtr TreeQueryCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b2DynamicTree_OverlapAABBAll"+  c_b2DynamicTree_OverlapAABBAll :: Ptr DynamicTree -> Ptr AABB -> FunPtr TreeQueryCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b2DynamicTree_CastRay"+  c_b2DynamicTree_CastRay :: Ptr DynamicTree -> Ptr RayCastInput -> Word64 -> FunPtr TreeRayCastCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b2DynamicTree_CastBox"+  c_b2DynamicTree_CastBox :: Ptr DynamicTree -> Ptr BoxCastInput -> Word64 -> FunPtr TreeBoxCastCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall unsafe "b2DynamicTree_GetHeight"+  c_b2DynamicTree_GetHeight :: Ptr DynamicTree -> IO CInt++foreign import ccall unsafe "b2DynamicTree_GetAreaRatio"+  c_b2DynamicTree_GetAreaRatio :: Ptr DynamicTree -> IO Float++foreign import ccall unsafe "hsg_b2DynamicTree_GetRootBounds"+  c_b2DynamicTree_GetRootBounds :: Ptr DynamicTree -> Ptr AABB -> IO ()++foreign import ccall unsafe "b2DynamicTree_GetProxyCount"+  c_b2DynamicTree_GetProxyCount :: Ptr DynamicTree -> IO CInt++foreign import ccall unsafe "b2DynamicTree_GetByteCount"+  c_b2DynamicTree_GetByteCount :: Ptr DynamicTree -> IO CSize++foreign import ccall unsafe "b2DynamicTree_GetUserData"+  c_b2DynamicTree_GetUserData :: Ptr DynamicTree -> CInt -> IO Word64++foreign import ccall unsafe "hsg_b2DynamicTree_GetProxyBounds"+  c_b2DynamicTree_GetProxyBounds :: Ptr DynamicTree -> CInt -> Ptr AABB -> IO ()++foreign import ccall unsafe "b2DynamicTree_Validate"+  c_b2DynamicTree_Validate :: Ptr DynamicTree -> IO ()++{- | Constructing the tree initializes the node pool.++Binds @b2CreateDynamicTree@.+-}+create+  :: Int+  -- ^ @proxyCapacity@+  -> IO (Ptr DynamicTree)+create a0 =+  c_b2CreateDynamicTree (fromIntegral a0)++{- | Destroy the tree, freeing the node pool.++Binds @b2DestroyDynamicTree@.+-}+destroy+  :: Ptr DynamicTree+  -> IO ()+destroy a0 =+  c_b2DestroyDynamicTree a0++{- | Create a proxy. Provide an AABB and a userData value.++Binds @b2CreateTreeProxy@.+-}+createProxy+  :: Ptr DynamicTree+  -> AABB+  -> Word64+  -- ^ @categoryBits@+  -> Word64+  -- ^ @userData@+  -> IO Int+createProxy a0 a1 a2 a3 =+  with a1 $ \p1 ->+  fromIntegral <$> (c_b2CreateTreeProxy a0 p1 a2 a3)++{- | Destroy a proxy. This asserts if the id is invalid.++Binds @b2DestroyTreeProxy@.+-}+destroyProxy+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> IO ()+destroyProxy a0 a1 =+  c_b2DestroyTreeProxy a0 (fromIntegral a1)++{- | Move a proxy to a new AABB by removing and reinserting into the tree.++Binds @b2DynamicTree_MoveProxy@.+-}+moveProxy+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> AABB+  -> IO ()+moveProxy a0 a1 a2 =+  with a2 $ \p2 ->+  c_b2DynamicTree_MoveProxy a0 (fromIntegral a1) p2++{- | Modify the category bits on a proxy.++Binds @b2DynamicTree_SetCategoryBits@.+-}+setCategoryBits+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> Word64+  -- ^ @categoryBits@+  -> IO ()+setCategoryBits a0 a1 a2 =+  c_b2DynamicTree_SetCategoryBits a0 (fromIntegral a1) a2++{- | Get the category bits on a proxy.++Binds @b2DynamicTree_GetCategoryBits@.+-}+getCategoryBits+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> IO Word64+getCategoryBits a0 a1 =+  c_b2DynamicTree_GetCategoryBits a0 (fromIntegral a1)++{- | Query an AABB for overlapping proxies. The callback is called for each proxy that overlaps the+supplied AABB.++Returns: performance data++Binds @b2DynamicTree_OverlapAABB@.+-}+overlapAABB+  :: Ptr DynamicTree+  -> AABB+  -> Word64+  -- ^ @maskBits@+  -> FunPtr TreeQueryCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+overlapAABB a0 a1 a2 a3 a4 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2DynamicTree_OverlapAABB a0 p1 a2 a3 a4 pOut >> peek pOut++{- | Query an AABB for overlapping proxies. The callback is called for each proxy that overlaps the+supplied AABB. No filtering is performed.++Returns: performance data++Binds @b2DynamicTree_OverlapAABBAll@.+-}+overlapAABBAll+  :: Ptr DynamicTree+  -> AABB+  -> FunPtr TreeQueryCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+overlapAABBAll a0 a1 a2 a3 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2DynamicTree_OverlapAABBAll a0 p1 a2 a3 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 improve+performance in some scenarios. However, this filtering may be approximate, so the user should+still apply filtering to results.++Returns: performance data++Binds @b2DynamicTree_CastRay@.+-}+castRay+  :: Ptr DynamicTree+  -- ^ @tree@: the dynamic tree to ray cast+  -> RayCastInput+  -- ^ @input@: the ray cast input data.+  -> Word64+  -- ^ @maskBits@: mask bits used to filter proxies.+  -> FunPtr TreeRayCastCallbackFcn+  -- ^ @callback@: a callback that is called for each proxy that is hit by the ray+  -> Ptr ()+  -- ^ @context@: user context that is passed to the callback+  -> IO TreeStats+castRay a0 a1 a2 a3 a4 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2DynamicTree_CastRay a0 p1 a2 a3 a4 pOut >> peek pOut++{- | Cast a swept AABB through the tree. 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.++Returns: performance data++Binds @b2DynamicTree_CastBox@.+-}+castBox+  :: Ptr DynamicTree+  -- ^ @tree@: the dynamic tree to cast through+  -> Ptr BoxCastInput+  -- ^ @input@: the AABB cast input. The box sweeps from its origin to origin + maxFraction * translation.+  -> Word64+  -- ^ @maskBits@: filter bits: \`bool accept = (maskBits & proxy-\>categoryBits) != 0;\`+  -> FunPtr TreeBoxCastCallbackFcn+  -- ^ @callback@: a callback that is called for each proxy the swept box may hit+  -> Ptr ()+  -- ^ @context@: user context that is passed to the callback+  -> IO TreeStats+castBox a0 a1 a2 a3 a4 =+  alloca $ \pOut -> c_b2DynamicTree_CastBox a0 a1 a2 a3 a4 pOut >> peek pOut++-- | Like 'castBox', but the result is written into a caller-supplied buffer.+castBoxInto+  :: Ptr DynamicTree+  -- ^ @tree@: the dynamic tree to cast through+  -> Ptr BoxCastInput+  -- ^ @input@: the AABB cast input. The box sweeps from its origin to origin + maxFraction * translation.+  -> Word64+  -- ^ @maskBits@: filter bits: \`bool accept = (maskBits & proxy-\>categoryBits) != 0;\`+  -> FunPtr TreeBoxCastCallbackFcn+  -- ^ @callback@: a callback that is called for each proxy the swept box may hit+  -> Ptr ()+  -- ^ @context@: user context that is passed to the callback+  -> Ptr TreeStats+  -- ^ Result buffer.+  -> IO ()+castBoxInto a0 a1 a2 a3 a4 out =+  c_b2DynamicTree_CastBox a0 a1 a2 a3 a4 out++{- | Get the height of the binary tree.++Binds @b2DynamicTree_GetHeight@.+-}+getHeight+  :: Ptr DynamicTree+  -> IO Int+getHeight a0 =+  fromIntegral <$> (c_b2DynamicTree_GetHeight a0)++{- | Get the ratio of the sum of the internal node areas to the root area.++Binds @b2DynamicTree_GetAreaRatio@.+-}+getAreaRatio+  :: Ptr DynamicTree+  -> IO Float+getAreaRatio a0 =+  c_b2DynamicTree_GetAreaRatio a0++{- | Get the bounding box that contains the entire tree++Binds @b2DynamicTree_GetRootBounds@.+-}+getRootBounds+  :: Ptr DynamicTree+  -> IO AABB+getRootBounds a0 =+  alloca $ \pOut -> c_b2DynamicTree_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_b2DynamicTree_GetRootBounds a0 out++{- | Get the number of proxies created++Binds @b2DynamicTree_GetProxyCount@.+-}+getProxyCount+  :: Ptr DynamicTree+  -> IO Int+getProxyCount a0 =+  fromIntegral <$> (c_b2DynamicTree_GetProxyCount a0)++{- | Get the number of bytes used by this tree++Binds @b2DynamicTree_GetByteCount@.+-}+getByteCount+  :: Ptr DynamicTree+  -> IO CSize+getByteCount a0 =+  c_b2DynamicTree_GetByteCount a0++{- | Get proxy user data++Binds @b2DynamicTree_GetUserData@.+-}+getUserData+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> IO Word64+getUserData a0 a1 =+  c_b2DynamicTree_GetUserData a0 (fromIntegral a1)++{- | Get the AABB of a proxy++Binds @b2DynamicTree_GetProxyBounds@.+-}+getProxyBounds+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> IO AABB+getProxyBounds a0 a1 =+  alloca $ \pOut -> c_b2DynamicTree_GetProxyBounds a0 (fromIntegral a1) pOut >> peek pOut++-- | Like 'getProxyBounds', but the result is written into a caller-supplied buffer.+getProxyBoundsInto+  :: Ptr DynamicTree+  -> Int+  -- ^ @proxyId@+  -> Ptr AABB+  -- ^ Result buffer.+  -> IO ()+getProxyBoundsInto a0 a1 out =+  c_b2DynamicTree_GetProxyBounds a0 (fromIntegral a1) out++{- | Validate the tree.++Binds @b2DynamicTree_Validate@.+-}+validate+  :: Ptr DynamicTree+  -> IO ()+validate a0 =+  c_b2DynamicTree_Validate a0
+ src/Box2D/Events.hs view
@@ -0,0 +1,92 @@+{-| Reading the per-step event buffers as ordinary Haskell values.++Box2D exposes events as C-owned arrays that live only between one+'Box2D.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 'Box2D.Internal.unsafeViewVector'. Call it after stepping.++This is the hand-written counterpart to the generated bindings: the array+marshalling (roadmap phase 5) is not something the generator emits.+-}+module Box2D.Events+  ( bodyMoveEvents+  , sensorBeginTouchEvents+  , sensorEndTouchEvents+  , contactBeginTouchEvents+  , contactEndTouchEvents+  , contactHitEvents+  , jointEvents+  ) where++import Data.Vector.Storable (Vector)++import Box2D.Id (WorldId)+import Box2D.Internal (copyVector)+import Box2D.World+  ( BodyEvents (..)+  , BodyMoveEvent+  , ContactBeginTouchEvent+  , ContactEndTouchEvent+  , ContactEvents (..)+  , ContactHitEvent+  , JointEvent+  , JointEvents (..)+  , SensorBeginTouchEvent+  , SensorEndTouchEvent+  , SensorEvents (..)+  )+import Box2D.World qualified as World++{- | The bodies that moved during the last 'Box2D.World.step', copied out of the+world-owned buffer. Valid to call any time, but only reflects the most recent+step.+-}+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 @Box2D.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/Box2D/Extra.hs view
@@ -0,0 +1,14 @@+-- | Hand-written type helpers that complement the generated modules.+module Box2D.Extra+  ( withShapeProxy+  ) where++import Data.Vector.Storable qualified as VS+import Foreign++import Box2D.Geometry (ShapeProxy (..))+import Box2D.MathTypes (Vec2)++-- | Marshal a point cloud + radius as a 'ShapeProxy' for the duration of an action.+withShapeProxy :: VS.Vector Vec2 -> Float -> (Ptr ShapeProxy -> IO a) -> IO a+withShapeProxy points radius = with (ShapeProxy points radius)
+ src/Box2D/FilterJoint.hsc view
@@ -0,0 +1,65 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.FilterJoint+  ( FilterJointDef (..)+  , defaultFilterJointDef+  , create+  )+  where++import Foreign+import Foreign.C.Types (CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (JointId (..), WorldId (..))+import Box2D.Joint (JointDef)++#include "box2d/box2d.h"+++-- | A filter joint is used to disable collision between two specific bodies.+data FilterJointDef = FilterJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable FilterJointDef where+  sizeOf _ = #{size b2FilterJointDef}+  alignment _ = #{alignment b2FilterJointDef}+  peek p =+    FilterJointDef+      <$> #{peek b2FilterJointDef, base} p+      <*> #{peek b2FilterJointDef, internalValue} p+  poke p (FilterJointDef f1 f2) = do+    #{poke b2FilterJointDef, base} p f1+    #{poke b2FilterJointDef, internalValue} p f2++foreign import ccall unsafe "hs_b2DefaultFilterJointDef"+  c_b2DefaultFilterJointDef :: Ptr FilterJointDef -> IO ()++defaultFilterJointDef :: IO FilterJointDef+defaultFilterJointDef = allocaOut c_b2DefaultFilterJointDef+++foreign import ccall unsafe "b2CreateFilterJoint"+  c_b2CreateFilterJoint :: WorldId -> Ptr FilterJointDef -> IO JointId++{- | Create a filter joint.++See also @b2FilterJointDef@ for details.++Binds @b2CreateFilterJoint@.+-}+create+  :: WorldId+  -> FilterJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b2CreateFilterJoint a0 p1
+ src/Box2D/Geometry.hsc view
@@ -0,0 +1,360 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.Geometry+  ( Hull (..)+  , ChainSegment (..)+  , Segment (..)+  , Polygon (..)+  , Capsule (..)+  , Circle (..)+  , MassData (..)+  , WorldCastOutput (..)+  , CastOutput (..)+  , ShapeCastInput (..)+  , ShapeProxy (..)+  , RayCastInput (..)+  , maxPolygonVertices+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Data.Vector.Storable qualified as VS+import Box2D.Internal (copyBounded, pokeBounded)+import Box2D.MathTypes (Pos, Vec2)++#include "box2d/box2d.h"+++{- | A convex hull. Used to create convex polygons.++__Warning:__ Do not modify these values directly, instead use @b2ComputeHull()@+-}+data Hull = Hull+  { points :: VS.Vector Vec2+  -- ^ The final points of the hull+  }+  deriving (Eq, Show)++instance Storable Hull where+  sizeOf _ = #{size b2Hull}+  alignment _ = #{alignment b2Hull}+  peek p = do+    nCount <- #{peek b2Hull, count} p :: IO CInt+    Hull+      <$> copyBounded maxPolygonVertices nCount (#{ptr b2Hull, points} p)+  poke p x = do+    pokeBounded maxPolygonVertices (#{ptr b2Hull, points} p) x.points+    #{poke b2Hull, count} p (fromIntegral (minimum [maxPolygonVertices, VS.length x.points]) :: CInt)++{- | A line segment with one-sided collision. Only collides on the right side. Several of these are+generated for a chain shape.+ghost1 -\> point1 -\> point2 -\> ghost2+-}+data ChainSegment = ChainSegment+  { ghost1 :: Vec2+  -- ^ The tail ghost vertex+  , segment :: Segment+  -- ^ The line segment+  , ghost2 :: Vec2+  -- ^ The head ghost vertex+  , chainId :: CInt+  -- ^ The owning chain shape index (internal usage only)+  }+  deriving (Eq, Show)++instance Storable ChainSegment where+  sizeOf _ = #{size b2ChainSegment}+  alignment _ = #{alignment b2ChainSegment}+  peek p =+    ChainSegment+      <$> #{peek b2ChainSegment, ghost1} p+      <*> #{peek b2ChainSegment, segment} p+      <*> #{peek b2ChainSegment, ghost2} p+      <*> #{peek b2ChainSegment, chainId} p+  poke p (ChainSegment f1 f2 f3 f4) = do+    #{poke b2ChainSegment, ghost1} p f1+    #{poke b2ChainSegment, segment} p f2+    #{poke b2ChainSegment, ghost2} p f3+    #{poke b2ChainSegment, chainId} p f4++-- | A line segment with two-sided collision.+data Segment = Segment+  { point1 :: Vec2+  -- ^ The first point+  , point2 :: Vec2+  -- ^ The second point+  }+  deriving (Eq, Show)++instance Storable Segment where+  sizeOf _ = #{size b2Segment}+  alignment _ = #{alignment b2Segment}+  peek p =+    Segment+      <$> #{peek b2Segment, point1} p+      <*> #{peek b2Segment, point2} p+  poke p (Segment f1 f2) = do+    #{poke b2Segment, point1} p f1+    #{poke b2Segment, point2} p f2++{- | A solid convex polygon. It is assumed that the interior of the polygon is to the left of each+edge. Polygons have a maximum number of vertices equal to @B2_MAX_POLYGON_VERTICES@. In most cases+you should not need many vertices for a convex polygon.++__Warning:__ DO NOT fill this out manually, instead use a helper function like @b2MakePolygon@ or @b2MakeBox@.+-}+data Polygon = Polygon+  { vertices :: VS.Vector Vec2+  -- ^ The polygon vertices+  , normals :: VS.Vector Vec2+  -- ^ The outward normal vectors of the polygon sides+  , centroid :: Vec2+  -- ^ The centroid of the polygon+  , radius :: Float+  -- ^ The external radius for rounded polygons+  }+  deriving (Eq, Show)++instance Storable Polygon where+  sizeOf _ = #{size b2Polygon}+  alignment _ = #{alignment b2Polygon}+  peek p = do+    nCount <- #{peek b2Polygon, count} p :: IO CInt+    Polygon+      <$> copyBounded maxPolygonVertices nCount (#{ptr b2Polygon, vertices} p)+      <*> copyBounded maxPolygonVertices nCount (#{ptr b2Polygon, normals} p)+      <*> #{peek b2Polygon, centroid} p+      <*> #{peek b2Polygon, radius} p+  poke p x = do+    pokeBounded maxPolygonVertices (#{ptr b2Polygon, vertices} p) x.vertices+    pokeBounded maxPolygonVertices (#{ptr b2Polygon, normals} p) x.normals+    #{poke b2Polygon, centroid} p x.centroid+    #{poke b2Polygon, radius} p x.radius+    #{poke b2Polygon, count} p (fromIntegral (minimum [maxPolygonVertices, VS.length x.vertices, VS.length x.normals]) :: CInt)++-- | A solid capsule can be viewed as two semicircles connected by a rectangle.+data Capsule = Capsule+  { center1 :: Vec2+  -- ^ Local center of the first semicircle+  , center2 :: Vec2+  -- ^ Local center of the second semicircle+  , radius :: Float+  -- ^ The radius of the semicircles+  }+  deriving (Eq, Show)++instance Storable Capsule where+  sizeOf _ = #{size b2Capsule}+  alignment _ = #{alignment b2Capsule}+  peek p =+    Capsule+      <$> #{peek b2Capsule, center1} p+      <*> #{peek b2Capsule, center2} p+      <*> #{peek b2Capsule, radius} p+  poke p (Capsule f1 f2 f3) = do+    #{poke b2Capsule, center1} p f1+    #{poke b2Capsule, center2} p f2+    #{poke b2Capsule, radius} p f3++-- | A solid circle+data Circle = Circle+  { center :: Vec2+  -- ^ The local center+  , radius :: Float+  -- ^ The radius+  }+  deriving (Eq, Show)++instance Storable Circle where+  sizeOf _ = #{size b2Circle}+  alignment _ = #{alignment b2Circle}+  peek p =+    Circle+      <$> #{peek b2Circle, center} p+      <*> #{peek b2Circle, radius} p+  poke p (Circle f1 f2) = do+    #{poke b2Circle, center} p f1+    #{poke b2Circle, radius} p f2++-- | This holds the mass data computed for a shape.+data MassData = MassData+  { mass :: Float+  -- ^ The mass of the shape, usually in kilograms.+  , center :: Vec2+  -- ^ The position of the shape\'s centroid relative to the shape\'s origin.+  , rotationalInertia :: Float+  -- ^ The rotational inertia of the shape about the shape center.+  }+  deriving (Eq, Show)++instance Storable MassData where+  sizeOf _ = #{size b2MassData}+  alignment _ = #{alignment b2MassData}+  peek p =+    MassData+      <$> #{peek b2MassData, mass} p+      <*> #{peek b2MassData, center} p+      <*> #{peek b2MassData, rotationalInertia} p+  poke p (MassData f1 f2 f3) = do+    #{poke b2MassData, mass} p f1+    #{poke b2MassData, center} p f2+    #{poke b2MassData, rotationalInertia} p f3++-- | World-space cast output. The hit point is a world position. The normal stays a float direction.+data WorldCastOutput = WorldCastOutput+  { normal :: Vec2+  -- ^ 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+  , hit :: CBool+  -- ^ Did the cast hit?+  }+  deriving (Eq, Show)++instance Storable WorldCastOutput where+  sizeOf _ = #{size b2WorldCastOutput}+  alignment _ = #{alignment b2WorldCastOutput}+  peek p =+    WorldCastOutput+      <$> #{peek b2WorldCastOutput, normal} p+      <*> #{peek b2WorldCastOutput, point} p+      <*> #{peek b2WorldCastOutput, fraction} p+      <*> #{peek b2WorldCastOutput, iterations} p+      <*> #{peek b2WorldCastOutput, hit} p+  poke p (WorldCastOutput f1 f2 f3 f4 f5) = do+    #{poke b2WorldCastOutput, normal} p f1+    #{poke b2WorldCastOutput, point} p f2+    #{poke b2WorldCastOutput, fraction} p f3+    #{poke b2WorldCastOutput, iterations} p f4+    #{poke b2WorldCastOutput, hit} p f5++{- | Low level ray cast or shape cast output data. The hit point is in the local or relative frame of+the input. Returns a zero fraction and normal in the case of initial overlap.+-}+data CastOutput = CastOutput+  { normal :: Vec2+  -- ^ The surface normal at the hit point+  , point :: Vec2+  -- ^ The surface hit point+  , fraction :: Float+  -- ^ The fraction of the input translation at collision+  , iterations :: CInt+  -- ^ The number of iterations used+  , hit :: CBool+  -- ^ Did the cast hit?+  }+  deriving (Eq, Show)++instance Storable CastOutput where+  sizeOf _ = #{size b2CastOutput}+  alignment _ = #{alignment b2CastOutput}+  peek p =+    CastOutput+      <$> #{peek b2CastOutput, normal} p+      <*> #{peek b2CastOutput, point} p+      <*> #{peek b2CastOutput, fraction} p+      <*> #{peek b2CastOutput, iterations} p+      <*> #{peek b2CastOutput, hit} p+  poke p (CastOutput f1 f2 f3 f4 f5) = do+    #{poke b2CastOutput, normal} p f1+    #{poke b2CastOutput, point} p f2+    #{poke b2CastOutput, fraction} p f3+    #{poke b2CastOutput, iterations} p f4+    #{poke b2CastOutput, hit} p f5++{- | Low level shape cast input in generic form. This allows casting an arbitrary point cloud wrapped+with a radius. For example, a circle 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 shape+  , translation :: Vec2+  -- ^ 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 b2ShapeCastInput}+  alignment _ = #{alignment b2ShapeCastInput}+  peek p =+    ShapeCastInput+      <$> #{peek b2ShapeCastInput, proxy} p+      <*> #{peek b2ShapeCastInput, translation} p+      <*> #{peek b2ShapeCastInput, maxFraction} p+      <*> #{peek b2ShapeCastInput, canEncroach} p+  poke p (ShapeCastInput f1 f2 f3 f4) = do+    #{poke b2ShapeCastInput, proxy} p f1+    #{poke b2ShapeCastInput, translation} p f2+    #{poke b2ShapeCastInput, maxFraction} p f3+    #{poke b2ShapeCastInput, canEncroach} p f4++{- | A distance proxy is used by the GJK algorithm. It encapsulates any shape. You can provide+between 1 and @B2_MAX_POLYGON_VERTICES@ points and a radius.+-}+data ShapeProxy = ShapeProxy+  { points :: VS.Vector Vec2+  -- ^ The point cloud+  , radius :: Float+  -- ^ The external radius of the point cloud. May be zero.+  }+  deriving (Eq, Show)++instance Storable ShapeProxy where+  sizeOf _ = #{size b2ShapeProxy}+  alignment _ = #{alignment b2ShapeProxy}+  peek p = do+    nCount <- #{peek b2ShapeProxy, count} p :: IO CInt+    ShapeProxy+      <$> copyBounded maxPolygonVertices nCount (#{ptr b2ShapeProxy, points} p)+      <*> #{peek b2ShapeProxy, radius} p+  poke p x = do+    pokeBounded maxPolygonVertices (#{ptr b2ShapeProxy, points} p) x.points+    #{poke b2ShapeProxy, radius} p x.radius+    #{poke b2ShapeProxy, count} p (fromIntegral (minimum [maxPolygonVertices, VS.length x.points]) :: CInt)++-- | Low level ray cast input data+data RayCastInput = RayCastInput+  { origin :: Vec2+  -- ^ Start point of the ray cast+  , translation :: Vec2+  -- ^ Translation of the ray cast+  , maxFraction :: Float+  -- ^ The maximum fraction of the translation to consider, typically 1+  }+  deriving (Eq, Show)++instance Storable RayCastInput where+  sizeOf _ = #{size b2RayCastInput}+  alignment _ = #{alignment b2RayCastInput}+  peek p =+    RayCastInput+      <$> #{peek b2RayCastInput, origin} p+      <*> #{peek b2RayCastInput, translation} p+      <*> #{peek b2RayCastInput, maxFraction} p+  poke p (RayCastInput f1 f2 f3) = do+    #{poke b2RayCastInput, origin} p f1+    #{poke b2RayCastInput, translation} p f2+    #{poke b2RayCastInput, maxFraction} p f3++-- | @B2_MAX_POLYGON_VERTICES@+maxPolygonVertices :: Int+maxPolygonVertices = #{const B2_MAX_POLYGON_VERTICES}++++
+ src/Box2D/Id.hsc view
@@ -0,0 +1,122 @@+{- | Low-level bindings to @box2d/id.h@: opaque handles to Box2D 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/box2d_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 Box2D.Id+  ( WorldId (..)+  , BodyId (..)+  , ShapeId (..)+  , ChainId (..)+  , JointId (..)+  , ContactId (..)+  , nullWorldId+  , nullBodyId+  , nullShapeId+  , nullChainId+  , nullJointId+  , nullContactId+  ) where++import Foreign++#include "box2d/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 chain instance.+newtype ChainId = ChainId 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 b2WorldId}+  alignment _ = alignment (0 :: Word32)+  peek p = WorldId <$> peek (castPtr p)+  poke p (WorldId w) = poke (castPtr p) w++instance Storable BodyId where+  sizeOf _ = #{size b2BodyId}+  alignment _ = alignment (0 :: Word64)+  peek p = BodyId <$> peek (castPtr p)+  poke p (BodyId w) = poke (castPtr p) w++instance Storable ShapeId where+  sizeOf _ = #{size b2ShapeId}+  alignment _ = alignment (0 :: Word64)+  peek p = ShapeId <$> peek (castPtr p)+  poke p (ShapeId w) = poke (castPtr p) w++instance Storable ChainId where+  sizeOf _ = #{size b2ChainId}+  alignment _ = alignment (0 :: Word64)+  peek p = ChainId <$> peek (castPtr p)+  poke p (ChainId w) = poke (castPtr p) w++instance Storable JointId where+  sizeOf _ = #{size b2JointId}+  alignment _ = alignment (0 :: Word64)+  peek p = JointId <$> peek (castPtr p)+  poke p (JointId w) = poke (castPtr p) w++instance Storable ContactId where+  sizeOf _ = #{size b2ContactId}+  alignment _ = #{alignment b2ContactId}+  peek p =+    ContactId+      <$> #{peek b2ContactId, index1} p+      <*> #{peek b2ContactId, world0} p+      <*> #{peek b2ContactId, generation} p+  poke p (ContactId i w g) = do+    #{poke b2ContactId, index1} p i+    #{poke b2ContactId, world0} p w+    #{poke b2ContactId, padding} p (0 :: Int16)+    #{poke b2ContactId, generation} p g++nullWorldId :: WorldId+nullWorldId = WorldId 0++nullBodyId :: BodyId+nullBodyId = BodyId 0++nullShapeId :: ShapeId+nullShapeId = ShapeId 0++nullChainId :: ChainId+nullChainId = ChainId 0++nullJointId :: JointId+nullJointId = JointId 0++nullContactId :: ContactId+nullContactId = ContactId 0 0 0
+ src/Box2D/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 Box2D.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/Box2D/Internal/DrawClosure.hsc view
@@ -0,0 +1,63 @@+{-| Field-by-field access to the shim's @hs_b2DrawClosure@.++Split out of "Box2D.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 Box2D.Internal.DrawClosure+  ( size+  , polygon+  , solidPolygon+  , circle+  , solidCircle+  , solidCapsule+  , line+  , transform+  , point+  , string+  , bounds+  , context+  ) where++import Foreign++#include "box2d_shim.h"++-- | Bytes to allocate for one closure.+size :: Int+size = #{size hs_b2DrawClosure}++polygon :: Ptr () -> FunPtr () -> IO ()+polygon = #{poke hs_b2DrawClosure, polygon}++solidPolygon :: Ptr () -> FunPtr () -> IO ()+solidPolygon = #{poke hs_b2DrawClosure, solidPolygon}++circle :: Ptr () -> FunPtr () -> IO ()+circle = #{poke hs_b2DrawClosure, circle}++solidCircle :: Ptr () -> FunPtr () -> IO ()+solidCircle = #{poke hs_b2DrawClosure, solidCircle}++solidCapsule :: Ptr () -> FunPtr () -> IO ()+solidCapsule = #{poke hs_b2DrawClosure, solidCapsule}++line :: Ptr () -> FunPtr () -> IO ()+line = #{poke hs_b2DrawClosure, line}++transform :: Ptr () -> FunPtr () -> IO ()+transform = #{poke hs_b2DrawClosure, transform}++point :: Ptr () -> FunPtr () -> IO ()+point = #{poke hs_b2DrawClosure, point}++string :: Ptr () -> FunPtr () -> IO ()+string = #{poke hs_b2DrawClosure, string}++bounds :: Ptr () -> FunPtr () -> IO ()+bounds = #{poke hs_b2DrawClosure, bounds}++-- | The user context the trampolines pass through to each callback.+context :: Ptr () -> Ptr () -> IO ()+context = #{poke hs_b2DrawClosure, context}
+ src/Box2D/Joint.hsc view
@@ -0,0 +1,532 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.Joint+  ( JointDef (..)+  , JointType (..)+  , pattern DistanceJoint+  , pattern FilterJoint+  , pattern MotorJoint+  , pattern MoverJoint+  , pattern PogoJoint+  , pattern PrismaticJoint+  , pattern RevoluteJoint+  , pattern WeldJoint+  , pattern WheelJoint+  , destroy+  , isValid+  , getType+  , getBodyA+  , getBodyB+  , getWorld+  , setLocalFrameA+  , getLocalFrameA+  , getLocalFrameAInto+  , setLocalFrameB+  , getLocalFrameB+  , getLocalFrameBInto+  , setCollideConnected+  , getCollideConnected+  , setUserData+  , getUserData+  , wakeBodies+  , getConstraintForce+  , getConstraintForceInto+  , getConstraintTorque+  , getLinearSeparation+  , getAngularSeparation+  , setConstraintTuning+  , getConstraintTuning+  , setForceThreshold+  , getForceThreshold+  , setTorqueThreshold+  , getTorqueThreshold+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Data.Coerce (coerce)+import Box2D.Id (BodyId (..), JointId (..), WorldId (..))+import Box2D.MathTypes (Transform, Vec2)++#include "box2d/box2d.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+  }+  deriving (Eq, Show)++instance Storable JointDef where+  sizeOf _ = #{size b2JointDef}+  alignment _ = #{alignment b2JointDef}+  peek p =+    JointDef+      <$> #{peek b2JointDef, userData} p+      <*> #{peek b2JointDef, bodyIdA} p+      <*> #{peek b2JointDef, bodyIdB} p+      <*> #{peek b2JointDef, localFrameA} p+      <*> #{peek b2JointDef, localFrameB} p+      <*> #{peek b2JointDef, forceThreshold} p+      <*> #{peek b2JointDef, torqueThreshold} p+      <*> #{peek b2JointDef, constraintHertz} p+      <*> #{peek b2JointDef, constraintDampingRatio} p+      <*> #{peek b2JointDef, drawScale} p+      <*> #{peek b2JointDef, collideConnected} p+  poke p (JointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11) = do+    #{poke b2JointDef, userData} p f1+    #{poke b2JointDef, bodyIdA} p f2+    #{poke b2JointDef, bodyIdB} p f3+    #{poke b2JointDef, localFrameA} p f4+    #{poke b2JointDef, localFrameB} p f5+    #{poke b2JointDef, forceThreshold} p f6+    #{poke b2JointDef, torqueThreshold} p f7+    #{poke b2JointDef, constraintHertz} p f8+    #{poke b2JointDef, constraintDampingRatio} p f9+    #{poke b2JointDef, drawScale} p f10+    #{poke b2JointDef, collideConnected} p f11++{- | Joint type enumeration++This is useful because all joint types use @b2JointId@ and sometimes you want to get the type of a+joint.+-}+newtype JointType = JointType CInt+  deriving (Eq, Show, Storable)++-- | constrain the distance between two points+pattern DistanceJoint :: JointType+-- | filter collision between two bodies+pattern FilterJoint :: JointType+-- | fully control the movement of a body+pattern MotorJoint :: JointType+-- | control the movement of the dynamic character mover+pattern MoverJoint :: JointType+-- | ground interaction for the dynamic character mover+pattern PogoJoint :: JointType+-- | slide along an axis+pattern PrismaticJoint :: JointType+-- | rotate around a pivot+pattern RevoluteJoint :: JointType+-- | bring two bodies together with some flex+pattern WeldJoint :: JointType+-- | wheel spins with shock absorber+pattern WheelJoint :: JointType+pattern DistanceJoint = JointType #{const b2_distanceJoint}+pattern FilterJoint = JointType #{const b2_filterJoint}+pattern MotorJoint = JointType #{const b2_motorJoint}+pattern MoverJoint = JointType #{const b2_moverJoint}+pattern PogoJoint = JointType #{const b2_pogoJoint}+pattern PrismaticJoint = JointType #{const b2_prismaticJoint}+pattern RevoluteJoint = JointType #{const b2_revoluteJoint}+pattern WeldJoint = JointType #{const b2_weldJoint}+pattern WheelJoint = JointType #{const b2_wheelJoint}+++foreign import ccall unsafe "b2DestroyJoint"+  c_b2DestroyJoint :: JointId -> IO ()++foreign import ccall unsafe "b2Joint_IsValid"+  c_b2Joint_IsValid :: JointId -> IO CBool++foreign import ccall unsafe "b2Joint_GetType"+  c_b2Joint_GetType :: JointId -> IO CInt++foreign import ccall unsafe "b2Joint_GetBodyA"+  c_b2Joint_GetBodyA :: JointId -> IO BodyId++foreign import ccall unsafe "b2Joint_GetBodyB"+  c_b2Joint_GetBodyB :: JointId -> IO BodyId++foreign import ccall unsafe "b2Joint_GetWorld"+  c_b2Joint_GetWorld :: JointId -> IO WorldId++foreign import ccall unsafe "hsg_b2Joint_SetLocalFrameA"+  c_b2Joint_SetLocalFrameA :: JointId -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b2Joint_GetLocalFrameA"+  c_b2Joint_GetLocalFrameA :: JointId -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b2Joint_SetLocalFrameB"+  c_b2Joint_SetLocalFrameB :: JointId -> Ptr Transform -> IO ()++foreign import ccall unsafe "hsg_b2Joint_GetLocalFrameB"+  c_b2Joint_GetLocalFrameB :: JointId -> Ptr Transform -> IO ()++foreign import ccall unsafe "b2Joint_SetCollideConnected"+  c_b2Joint_SetCollideConnected :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2Joint_GetCollideConnected"+  c_b2Joint_GetCollideConnected :: JointId -> IO CBool++foreign import ccall unsafe "b2Joint_SetUserData"+  c_b2Joint_SetUserData :: JointId -> Ptr () -> IO ()++foreign import ccall unsafe "b2Joint_GetUserData"+  c_b2Joint_GetUserData :: JointId -> IO (Ptr ())++foreign import ccall unsafe "b2Joint_WakeBodies"+  c_b2Joint_WakeBodies :: JointId -> IO ()++foreign import ccall unsafe "hsg_b2Joint_GetConstraintForce"+  c_b2Joint_GetConstraintForce :: JointId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "b2Joint_GetConstraintTorque"+  c_b2Joint_GetConstraintTorque :: JointId -> IO Float++foreign import ccall unsafe "b2Joint_GetLinearSeparation"+  c_b2Joint_GetLinearSeparation :: JointId -> IO Float++foreign import ccall unsafe "b2Joint_GetAngularSeparation"+  c_b2Joint_GetAngularSeparation :: JointId -> IO Float++foreign import ccall unsafe "b2Joint_SetConstraintTuning"+  c_b2Joint_SetConstraintTuning :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b2Joint_GetConstraintTuning"+  c_b2Joint_GetConstraintTuning :: JointId -> Ptr Float -> Ptr Float -> IO ()++foreign import ccall unsafe "b2Joint_SetForceThreshold"+  c_b2Joint_SetForceThreshold :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2Joint_GetForceThreshold"+  c_b2Joint_GetForceThreshold :: JointId -> IO Float++foreign import ccall unsafe "b2Joint_SetTorqueThreshold"+  c_b2Joint_SetTorqueThreshold :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2Joint_GetTorqueThreshold"+  c_b2Joint_GetTorqueThreshold :: JointId -> IO Float++{- | Destroy a joint.++Binds @b2DestroyJoint@.+-}+destroy+  :: JointId+  -> IO ()+destroy a0 =+  c_b2DestroyJoint a0++{- | Joint identifier validation. Provides validation for up to 64K allocations.++Binds @b2Joint_IsValid@.+-}+isValid+  :: JointId+  -> IO Bool+isValid a0 =+  toBool <$> (c_b2Joint_IsValid a0)++{- | Get the joint type++Binds @b2Joint_GetType@.+-}+getType+  :: JointId+  -> IO JointType+getType a0 =+  coerce <$> (c_b2Joint_GetType a0)++{- | Get body A id on a joint++Binds @b2Joint_GetBodyA@.+-}+getBodyA+  :: JointId+  -> IO BodyId+getBodyA a0 =+  c_b2Joint_GetBodyA a0++{- | Get body B id on a joint++Binds @b2Joint_GetBodyB@.+-}+getBodyB+  :: JointId+  -> IO BodyId+getBodyB a0 =+  c_b2Joint_GetBodyB a0++{- | Get the world that owns this joint++Binds @b2Joint_GetWorld@.+-}+getWorld+  :: JointId+  -> IO WorldId+getWorld a0 =+  c_b2Joint_GetWorld a0++{- | Set the local frame on bodyA++Binds @b2Joint_SetLocalFrameA@.+-}+setLocalFrameA+  :: JointId+  -> Transform+  -- ^ @localFrame@+  -> IO ()+setLocalFrameA a0 a1 =+  with a1 $ \p1 ->+  c_b2Joint_SetLocalFrameA a0 p1++{- | Get the local frame on bodyA++Binds @b2Joint_GetLocalFrameA@.+-}+getLocalFrameA+  :: JointId+  -> IO Transform+getLocalFrameA a0 =+  alloca $ \pOut -> c_b2Joint_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_b2Joint_GetLocalFrameA a0 out++{- | Set the local frame on bodyB++Binds @b2Joint_SetLocalFrameB@.+-}+setLocalFrameB+  :: JointId+  -> Transform+  -- ^ @localFrame@+  -> IO ()+setLocalFrameB a0 a1 =+  with a1 $ \p1 ->+  c_b2Joint_SetLocalFrameB a0 p1++{- | Get the local frame on bodyB++Binds @b2Joint_GetLocalFrameB@.+-}+getLocalFrameB+  :: JointId+  -> IO Transform+getLocalFrameB a0 =+  alloca $ \pOut -> c_b2Joint_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_b2Joint_GetLocalFrameB a0 out++{- | Toggle collision between connected bodies++Binds @b2Joint_SetCollideConnected@.+-}+setCollideConnected+  :: JointId+  -> Bool+  -- ^ @shouldCollide@+  -> IO ()+setCollideConnected a0 a1 =+  c_b2Joint_SetCollideConnected a0 (fromBool a1)++{- | Is collision allowed between connected bodies?++Binds @b2Joint_GetCollideConnected@.+-}+getCollideConnected+  :: JointId+  -> IO Bool+getCollideConnected a0 =+  toBool <$> (c_b2Joint_GetCollideConnected a0)++{- | Set the user data on a joint++Binds @b2Joint_SetUserData@.+-}+setUserData+  :: JointId+  -> Ptr ()+  -- ^ @userData@+  -> IO ()+setUserData a0 a1 =+  c_b2Joint_SetUserData a0 a1++{- | Get the user data on a joint++Binds @b2Joint_GetUserData@.+-}+getUserData+  :: JointId+  -> IO (Ptr ())+getUserData a0 =+  c_b2Joint_GetUserData a0++{- | Wake the bodies connected to this joint++Binds @b2Joint_WakeBodies@.+-}+wakeBodies+  :: JointId+  -> IO ()+wakeBodies a0 =+  c_b2Joint_WakeBodies a0++{- | Get the current constraint force for this joint. Usually in Newtons.++Binds @b2Joint_GetConstraintForce@.+-}+getConstraintForce+  :: JointId+  -> IO Vec2+getConstraintForce a0 =+  alloca $ \pOut -> c_b2Joint_GetConstraintForce a0 pOut >> peek pOut++-- | Like 'getConstraintForce', but the result is written into a caller-supplied buffer.+getConstraintForceInto+  :: JointId+  -> Ptr Vec2+  -- ^ Result buffer.+  -> IO ()+getConstraintForceInto a0 out =+  c_b2Joint_GetConstraintForce a0 out++{- | Get the current constraint torque for this joint. Usually in Newton * meters.++Binds @b2Joint_GetConstraintTorque@.+-}+getConstraintTorque+  :: JointId+  -> IO Float+getConstraintTorque a0 =+  c_b2Joint_GetConstraintTorque a0++{- | Get the current linear separation error for this joint. Does not consider admissible movement.+Usually in meters.++Binds @b2Joint_GetLinearSeparation@.+-}+getLinearSeparation+  :: JointId+  -> IO Float+getLinearSeparation a0 =+  c_b2Joint_GetLinearSeparation a0++{- | Get the current angular separation error for this joint. Does not consider admissible movement.+Radians.++Binds @b2Joint_GetAngularSeparation@.+-}+getAngularSeparation+  :: JointId+  -> IO Float+getAngularSeparation a0 =+  c_b2Joint_GetAngularSeparation a0++{- | Set the joint constraint tuning. Advanced feature.++Binds @b2Joint_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_b2Joint_SetConstraintTuning a0 a1 a2++{- | Get the joint constraint tuning. Advanced feature.++Binds @b2Joint_GetConstraintTuning@.+-}+getConstraintTuning+  :: JointId+  -> Ptr Float+  -- ^ @hertz@+  -> Ptr Float+  -- ^ @dampingRatio@+  -> IO ()+getConstraintTuning a0 a1 a2 =+  c_b2Joint_GetConstraintTuning a0 a1 a2++{- | Set the force threshold for joint events (Newtons)++Binds @b2Joint_SetForceThreshold@.+-}+setForceThreshold+  :: JointId+  -> Float+  -- ^ @threshold@+  -> IO ()+setForceThreshold a0 a1 =+  c_b2Joint_SetForceThreshold a0 a1++{- | Get the force threshold for joint events (Newtons)++Binds @b2Joint_GetForceThreshold@.+-}+getForceThreshold+  :: JointId+  -> IO Float+getForceThreshold a0 =+  c_b2Joint_GetForceThreshold a0++{- | Set the torque threshold for joint events (N-m)++Binds @b2Joint_SetTorqueThreshold@.+-}+setTorqueThreshold+  :: JointId+  -> Float+  -- ^ @threshold@+  -> IO ()+setTorqueThreshold a0 a1 =+  c_b2Joint_SetTorqueThreshold a0 a1++{- | Get the torque threshold for joint events (N-m)++Binds @b2Joint_GetTorqueThreshold@.+-}+getTorqueThreshold+  :: JointId+  -> IO Float+getTorqueThreshold a0 =+  c_b2Joint_GetTorqueThreshold a0
+ src/Box2D/MathFunctions.hs view
@@ -0,0 +1,102 @@+{-# LANGUAGE ForeignFunctionInterface #-}++{-| Low-level bindings to the exported (non-@inline@) functions of+@box2d/math_functions.h@.++Box2D 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 @B2_API@ are bound here: the validators (handy+as ABI oracles for the "Box2D.MathTypes" 'Foreign.Storable.Storable'+instances) and a couple of by-value constructors.+-}+module Box2D.MathFunctions+  ( isValidVec2+  , isValidRot+  , isValidTransform+  , isValidAABB+  , isValidPlane+  , computeCosSin+  , makeRot+  , rotGetAngle+  , getLengthUnitsPerMeter+  , setLengthUnitsPerMeter+  ) where++import Foreign+import Foreign.C.Types (CBool (..))++import Box2D.Internal (allocaOut, withValid)+import Box2D.MathTypes++foreign import ccall unsafe "hs_b2IsValidVec2"+  c_b2IsValidVec2 :: Ptr Vec2 -> IO CBool++foreign import ccall unsafe "hs_b2IsValidRot"+  c_b2IsValidRot :: Ptr Rot -> IO CBool++foreign import ccall unsafe "hs_b2IsValidTransform"+  c_b2IsValidTransform :: Ptr Transform -> IO CBool++foreign import ccall unsafe "hs_b2IsValidAABB"+  c_b2IsValidAABB :: Ptr AABB -> IO CBool++foreign import ccall unsafe "hs_b2IsValidPlane"+  c_b2IsValidPlane :: Ptr Plane -> IO CBool++foreign import ccall unsafe "hs_b2ComputeCosSin"+  c_b2ComputeCosSin :: Float -> Ptr CosSin -> IO ()++foreign import ccall unsafe "hs_b2MakeRot"+  c_b2MakeRot :: Float -> Ptr Rot -> IO ()++-- | Is this a valid (finite, non-NaN) vector?+isValidVec2 :: Vec2 -> IO Bool+isValidVec2 = withValid c_b2IsValidVec2++-- | Is this a valid, normalized rotation?+isValidRot :: Rot -> IO Bool+isValidRot = withValid c_b2IsValidRot++-- | Is this a valid rigid transform?+isValidTransform :: Transform -> IO Bool+isValidTransform = withValid c_b2IsValidTransform++-- | Is this a valid, bounded AABB?+isValidAABB :: AABB -> IO Bool+isValidAABB = withValid c_b2IsValidAABB++-- | Is this a valid plane (finite, unit normal)?+isValidPlane :: Plane -> IO Bool+isValidPlane = withValid c_b2IsValidPlane++-- | Compute a deterministic cosine/sine pair for the given angle in radians.+computeCosSin :: Float -> IO CosSin+computeCosSin radians = allocaOut (c_b2ComputeCosSin radians)++-- | Make a normalized rotation from an angle in radians.+makeRot :: Float -> IO Rot+makeRot radians = allocaOut (c_b2MakeRot radians)++{- | The angle of a rotation in radians, in @(-pi, pi]@; the inverse of+'makeRot'. Pure (upstream @b2Rot_GetAngle@ is an inline header function).+-}+rotGetAngle :: Rot -> Float+rotGetAngle (Rot c s) = atan2 s c++foreign import ccall unsafe "b2GetLengthUnitsPerMeter"+  c_b2GetLengthUnitsPerMeter :: IO Float++foreign import ccall unsafe "b2SetLengthUnitsPerMeter"+  c_b2SetLengthUnitsPerMeter :: Float -> IO ()++{- | The engine's current length units per meter (default 1). Tuning+constants scale with it: e.g. @B2_LINEAR_SLOP@ is 0.005 of this.+-}+getLengthUnitsPerMeter :: IO Float+getLengthUnitsPerMeter = c_b2GetLengthUnitsPerMeter++{- | Change the length units, e.g. 128 for pixels-as-units. Do this before+creating any world.+-}+setLengthUnitsPerMeter :: Float -> IO ()+setLengthUnitsPerMeter = c_b2SetLengthUnitsPerMeter
+ src/Box2D/MathTypes.hsc view
@@ -0,0 +1,309 @@+-- | Low-level bindings to the value types in @box2d/math_functions.h@.+--+-- These are the small POD math structs that the Box2D API passes and returns+-- by value. Field order matches the C layout; 'Storable' offsets come straight+-- from the header via @hsc2hs@.+--+-- 'Pos' and 'WorldTransform' follow the @double-precision@ package flag: their+-- translation is 'Coord', which is 'Float' by default and 'Double' with the flag.+module Box2D.MathTypes+  ( Vec2 (..)+  , CosSin (..)+  , Rot (..)+  , Transform (..)+  , Mat22 (..)+  , AABB (..)+  , Plane (..)+  , Coord+  , Pos (..)+  , WorldTransform (..)++    -- * Constants+  , vec2Zero+  , rotIdentity+  , transformIdentity+  , posZero+  , worldTransformIdentity++    -- * World positions+  , toPos+  , toVec2+  , subPos+  , offsetPos+  , transformWorldPoint+  , invTransformWorldPoint+  , toRelativeTransform+  , makeWorldTransform++    -- * Vector math+  , vec2Add+  , vec2Sub+  , vec2Neg+  , vec2Scale+  , vec2MulAdd+  , vec2MulSub+  , vec2Dot+  , vec2Cross+  , vec2LengthSquared+  , vec2Length+  , vec2Normalize+  , vec2Lerp++    -- * Rotation and plane math+  , rotMul+  , rotateVector+  , invRotateVector+  , planeSeparation+  ) where++import Foreign+#if defined(BOX2D_DOUBLE_PRECISION)+import GHC.Float (double2Float, float2Double)+#endif++#include "box2d/box2d.h"++-- | A 2D vector.+data Vec2 = Vec2 Float Float+  deriving (Eq, Show)++-- | Cosine and sine pair (@cosine@, @sine@).+data CosSin = CosSin Float Float+  deriving (Eq, Show)++-- | A 2D rotation stored as a cosine/sine pair (@c@, @s@).+data Rot = Rot Float Float+  deriving (Eq, Show)++-- | A 2D rigid transform: position @p@ and rotation @q@.+data Transform = Transform Vec2 Rot+  deriving (Eq, Show)++-- | A 2x2 column-major matrix (columns @cx@, @cy@).+data Mat22 = Mat22 Vec2 Vec2+  deriving (Eq, Show)++-- | Axis-aligned bounding box (@lowerBound@, @upperBound@).+data AABB = AABB Vec2 Vec2+  deriving (Eq, Show)++-- | A plane: @normal@ and @offset@ where @separation = dot(normal, point) - offset@.+data Plane = Plane Vec2 Float+  deriving (Eq, Show)++#if defined(BOX2D_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+  deriving (Eq, Show)++-- | World transform: position @p@ and rotation @q@.+data WorldTransform = WorldTransform Pos Rot+  deriving (Eq, Show)++instance Storable Vec2 where+  sizeOf _ = #{size b2Vec2}+  alignment _ = #{alignment b2Vec2}+  peek p = Vec2 <$> #{peek b2Vec2, x} p <*> #{peek b2Vec2, y} p+  poke p (Vec2 x y) = do+    #{poke b2Vec2, x} p x+    #{poke b2Vec2, y} p y++instance Storable CosSin where+  sizeOf _ = #{size b2CosSin}+  alignment _ = #{alignment b2CosSin}+  peek p = CosSin <$> #{peek b2CosSin, cosine} p <*> #{peek b2CosSin, sine} p+  poke p (CosSin c s) = do+    #{poke b2CosSin, cosine} p c+    #{poke b2CosSin, sine} p s++instance Storable Rot where+  sizeOf _ = #{size b2Rot}+  alignment _ = #{alignment b2Rot}+  peek p = Rot <$> #{peek b2Rot, c} p <*> #{peek b2Rot, s} p+  poke p (Rot c s) = do+    #{poke b2Rot, c} p c+    #{poke b2Rot, s} p s++instance Storable Transform where+  sizeOf _ = #{size b2Transform}+  alignment _ = #{alignment b2Transform}+  peek p = Transform <$> #{peek b2Transform, p} p <*> #{peek b2Transform, q} p+  poke ptr (Transform p q) = do+    #{poke b2Transform, p} ptr p+    #{poke b2Transform, q} ptr q++instance Storable Pos where+  sizeOf _ = #{size b2Pos}+  alignment _ = #{alignment b2Pos}+  peek p = Pos <$> #{peek b2Pos, x} p <*> #{peek b2Pos, y} p+  poke p (Pos x y) = do+    #{poke b2Pos, x} p x+    #{poke b2Pos, y} p y++instance Storable WorldTransform where+  sizeOf _ = #{size b2WorldTransform}+  alignment _ = #{alignment b2WorldTransform}+  peek p = WorldTransform <$> #{peek b2WorldTransform, p} p <*> #{peek b2WorldTransform, q} p+  poke ptr (WorldTransform p q) = do+    #{poke b2WorldTransform, p} ptr p+    #{poke b2WorldTransform, q} ptr q++instance Storable Mat22 where+  sizeOf _ = #{size b2Mat22}+  alignment _ = #{alignment b2Mat22}+  peek p = Mat22 <$> #{peek b2Mat22, cx} p <*> #{peek b2Mat22, cy} p+  poke p (Mat22 cx cy) = do+    #{poke b2Mat22, cx} p cx+    #{poke b2Mat22, cy} p cy++instance Storable AABB where+  sizeOf _ = #{size b2AABB}+  alignment _ = #{alignment b2AABB}+  peek p = AABB <$> #{peek b2AABB, lowerBound} p <*> #{peek b2AABB, upperBound} p+  poke p (AABB lo hi) = do+    #{poke b2AABB, lowerBound} p lo+    #{poke b2AABB, upperBound} p hi++instance Storable Plane where+  sizeOf _ = #{size b2Plane}+  alignment _ = #{alignment b2Plane}+  peek p = Plane <$> #{peek b2Plane, normal} p <*> #{peek b2Plane, offset} p+  poke p (Plane n o) = do+    #{poke b2Plane, normal} p n+    #{poke b2Plane, offset} p o++vec2Zero :: Vec2+vec2Zero = Vec2 0 0++-- | The identity rotation (cosine 1, sine 0).+rotIdentity :: Rot+rotIdentity = Rot 1 0++transformIdentity :: Transform+transformIdentity = Transform vec2Zero rotIdentity++posZero :: Pos+posZero = Pos 0 0++worldTransformIdentity :: WorldTransform+worldTransformIdentity = WorldTransform posZero rotIdentity++toPos :: Vec2 -> Pos+toPos (Vec2 x y) = Pos (widen x) (widen y)++toVec2 :: Pos -> Vec2+toVec2 (Pos x y) = Vec2 (narrow x) (narrow y)++subPos :: Pos -> Pos -> Vec2+subPos (Pos ax ay) (Pos bx by) = Vec2 (narrow (ax - bx)) (narrow (ay - by))++offsetPos :: Pos -> Vec2 -> Pos+offsetPos (Pos x y) (Vec2 dx dy) = Pos (x + widen dx) (y + widen dy)++transformWorldPoint :: WorldTransform -> Vec2 -> Pos+transformWorldPoint (WorldTransform p q) v = offsetPos p (rotateVector q v)++invTransformWorldPoint :: WorldTransform -> Pos -> Vec2+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++-- Vector math ---------------------------------------------------------------+--+-- Pure ports of the @static inline@ vector helpers of+-- @box2d/math_functions.h@, which have no linkable symbols and cannot be+-- bound by FFI. Each names its upstream original.++-- | @a + b@ (upstream @b2Add@).+vec2Add :: Vec2 -> Vec2 -> Vec2+vec2Add (Vec2 ax ay) (Vec2 bx by) = Vec2 (ax + bx) (ay + by)++-- | @a - b@ (upstream @b2Sub@).+vec2Sub :: Vec2 -> Vec2 -> Vec2+vec2Sub (Vec2 ax ay) (Vec2 bx by) = Vec2 (ax - bx) (ay - by)++-- | @-a@ (upstream @b2Neg@).+vec2Neg :: Vec2 -> Vec2+vec2Neg (Vec2 x y) = Vec2 (negate x) (negate y)++-- | @s * v@ (upstream @b2MulSV@).+vec2Scale :: Float -> Vec2 -> Vec2+vec2Scale s (Vec2 x y) = Vec2 (s * x) (s * y)++-- | @a + s * b@ (upstream @b2MulAdd@).+vec2MulAdd :: Vec2 -> Float -> Vec2 -> Vec2+vec2MulAdd a s b = vec2Add a (vec2Scale s b)++-- | @a - s * b@ (upstream @b2MulSub@).+vec2MulSub :: Vec2 -> Float -> Vec2 -> Vec2+vec2MulSub a s b = vec2Sub a (vec2Scale s b)++-- | Dot product (upstream @b2Dot@).+vec2Dot :: Vec2 -> Vec2 -> Float+vec2Dot (Vec2 ax ay) (Vec2 bx by) = ax * bx + ay * by++-- | 2D cross product, the signed area of the parallelogram (upstream+-- @b2Cross@).+vec2Cross :: Vec2 -> Vec2 -> Float+vec2Cross (Vec2 ax ay) (Vec2 bx by) = ax * by - ay * bx++-- | Squared length (upstream @b2LengthSquared@).+vec2LengthSquared :: Vec2 -> Float+vec2LengthSquared v = vec2Dot v v++-- | Length (upstream @b2Length@).+vec2Length :: Vec2 -> Float+vec2Length = sqrt . vec2LengthSquared++-- | A unit vector in the direction of @v@, or 'vec2Zero' when @v@ is too+-- short to normalize — below @FLT_EPSILON@, like upstream @b2Normalize@.+vec2Normalize :: Vec2 -> Vec2+vec2Normalize v+  | len < 1.1920929e-7 = vec2Zero+  | otherwise = vec2Scale (1 / len) v+  where+    len = vec2Length v++-- | Linear interpolation from @a@ at 0 to @b@ at 1 (upstream @b2Lerp@).+vec2Lerp :: Vec2 -> Vec2 -> Float -> Vec2+vec2Lerp (Vec2 ax ay) (Vec2 bx by) t =+  Vec2 (ax * (1 - t) + bx * t) (ay * (1 - t) + by * t)++-- | Compose two rotations: @q * r@ rotates by @r@ then @q@ (upstream+-- @b2MulRot@).+rotMul :: Rot -> Rot -> Rot+rotMul (Rot qc qs) (Rot rc rs) = Rot (qc * rc - qs * rs) (qs * rc + qc * rs)++-- | Rotate a vector (upstream @b2RotateVector@).+rotateVector :: Rot -> Vec2 -> Vec2+rotateVector (Rot c s) (Vec2 x y) = Vec2 (c * x - s * y) (s * x + c * y)++-- | Inverse-rotate a vector (upstream @b2InvRotateVector@).+invRotateVector :: Rot -> Vec2 -> Vec2+invRotateVector (Rot c s) (Vec2 x y) = Vec2 (c * x + s * y) (c * y - s * x)++-- | Signed distance of a point from a plane, using the engine's+-- @dot(normal, point) - offset@ convention (upstream @b2PlaneSeparation@).+planeSeparation :: Plane -> Vec2 -> Float+planeSeparation (Plane n o) p = vec2Dot n p - o
+ src/Box2D/MotorJoint.hsc view
@@ -0,0 +1,419 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.MotorJoint+  ( MotorJointDef (..)+  , defaultMotorJointDef+  , create+  , setLinearVelocity+  , getLinearVelocity+  , getLinearVelocityInto+  , setAngularVelocity+  , getAngularVelocity+  , setMaxVelocityForce+  , getMaxVelocityForce+  , setMaxVelocityTorque+  , getMaxVelocityTorque+  , setLinearHertz+  , getLinearHertz+  , setLinearDampingRatio+  , getLinearDampingRatio+  , setAngularHertz+  , getAngularHertz+  , setAngularDampingRatio+  , getAngularDampingRatio+  , setMaxSpringForce+  , getMaxSpringForce+  , setMaxSpringTorque+  , getMaxSpringTorque+  )+  where++import Foreign+import Foreign.C.Types (CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (JointId (..), WorldId (..))+import Box2D.Joint (JointDef)+import Box2D.MathTypes (Vec2)++#include "box2d/box2d.h"+++{- | A motor joint is used to control the relative velocity and\/or transform between two bodies. With+a velocity of zero this acts like top-down friction.+-}+data MotorJointDef = MotorJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , linearVelocity :: Vec2+  -- ^ The desired linear velocity+  , maxVelocityForce :: Float+  -- ^ The maximum motor force in newtons+  , angularVelocity :: Float+  -- ^ 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+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable MotorJointDef where+  sizeOf _ = #{size b2MotorJointDef}+  alignment _ = #{alignment b2MotorJointDef}+  peek p =+    MotorJointDef+      <$> #{peek b2MotorJointDef, base} p+      <*> #{peek b2MotorJointDef, linearVelocity} p+      <*> #{peek b2MotorJointDef, maxVelocityForce} p+      <*> #{peek b2MotorJointDef, angularVelocity} p+      <*> #{peek b2MotorJointDef, maxVelocityTorque} p+      <*> #{peek b2MotorJointDef, linearHertz} p+      <*> #{peek b2MotorJointDef, linearDampingRatio} p+      <*> #{peek b2MotorJointDef, maxSpringForce} p+      <*> #{peek b2MotorJointDef, angularHertz} p+      <*> #{peek b2MotorJointDef, angularDampingRatio} p+      <*> #{peek b2MotorJointDef, maxSpringTorque} p+      <*> #{peek b2MotorJointDef, internalValue} p+  poke p (MotorJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12) = do+    #{poke b2MotorJointDef, base} p f1+    #{poke b2MotorJointDef, linearVelocity} p f2+    #{poke b2MotorJointDef, maxVelocityForce} p f3+    #{poke b2MotorJointDef, angularVelocity} p f4+    #{poke b2MotorJointDef, maxVelocityTorque} p f5+    #{poke b2MotorJointDef, linearHertz} p f6+    #{poke b2MotorJointDef, linearDampingRatio} p f7+    #{poke b2MotorJointDef, maxSpringForce} p f8+    #{poke b2MotorJointDef, angularHertz} p f9+    #{poke b2MotorJointDef, angularDampingRatio} p f10+    #{poke b2MotorJointDef, maxSpringTorque} p f11+    #{poke b2MotorJointDef, internalValue} p f12++foreign import ccall unsafe "hs_b2DefaultMotorJointDef"+  c_b2DefaultMotorJointDef :: Ptr MotorJointDef -> IO ()++defaultMotorJointDef :: IO MotorJointDef+defaultMotorJointDef = allocaOut c_b2DefaultMotorJointDef+++foreign import ccall unsafe "b2CreateMotorJoint"+  c_b2CreateMotorJoint :: WorldId -> Ptr MotorJointDef -> IO JointId++foreign import ccall unsafe "hsg_b2MotorJoint_SetLinearVelocity"+  c_b2MotorJoint_SetLinearVelocity :: JointId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2MotorJoint_GetLinearVelocity"+  c_b2MotorJoint_GetLinearVelocity :: JointId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "b2MotorJoint_SetAngularVelocity"+  c_b2MotorJoint_SetAngularVelocity :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2MotorJoint_GetAngularVelocity"+  c_b2MotorJoint_GetAngularVelocity :: JointId -> IO Float++foreign import ccall unsafe "b2MotorJoint_SetMaxVelocityForce"+  c_b2MotorJoint_SetMaxVelocityForce :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2MotorJoint_GetMaxVelocityForce"+  c_b2MotorJoint_GetMaxVelocityForce :: JointId -> IO Float++foreign import ccall unsafe "b2MotorJoint_SetMaxVelocityTorque"+  c_b2MotorJoint_SetMaxVelocityTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2MotorJoint_GetMaxVelocityTorque"+  c_b2MotorJoint_GetMaxVelocityTorque :: JointId -> IO Float++foreign import ccall unsafe "b2MotorJoint_SetLinearHertz"+  c_b2MotorJoint_SetLinearHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2MotorJoint_GetLinearHertz"+  c_b2MotorJoint_GetLinearHertz :: JointId -> IO Float++foreign import ccall unsafe "b2MotorJoint_SetLinearDampingRatio"+  c_b2MotorJoint_SetLinearDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2MotorJoint_GetLinearDampingRatio"+  c_b2MotorJoint_GetLinearDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b2MotorJoint_SetAngularHertz"+  c_b2MotorJoint_SetAngularHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2MotorJoint_GetAngularHertz"+  c_b2MotorJoint_GetAngularHertz :: JointId -> IO Float++foreign import ccall unsafe "b2MotorJoint_SetAngularDampingRatio"+  c_b2MotorJoint_SetAngularDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2MotorJoint_GetAngularDampingRatio"+  c_b2MotorJoint_GetAngularDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b2MotorJoint_SetMaxSpringForce"+  c_b2MotorJoint_SetMaxSpringForce :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2MotorJoint_GetMaxSpringForce"+  c_b2MotorJoint_GetMaxSpringForce :: JointId -> IO Float++foreign import ccall unsafe "b2MotorJoint_SetMaxSpringTorque"+  c_b2MotorJoint_SetMaxSpringTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2MotorJoint_GetMaxSpringTorque"+  c_b2MotorJoint_GetMaxSpringTorque :: JointId -> IO Float++{- | Create a motor joint++See also @b2MotorJointDef@ for details.++Binds @b2CreateMotorJoint@.+-}+create+  :: WorldId+  -> MotorJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b2CreateMotorJoint a0 p1++{- | Set the desired relative linear velocity in meters per second++Binds @b2MotorJoint_SetLinearVelocity@.+-}+setLinearVelocity+  :: JointId+  -> Vec2+  -- ^ @velocity@+  -> IO ()+setLinearVelocity a0 a1 =+  with a1 $ \p1 ->+  c_b2MotorJoint_SetLinearVelocity a0 p1++{- | Get the desired relative linear velocity in meters per second++Binds @b2MotorJoint_GetLinearVelocity@.+-}+getLinearVelocity+  :: JointId+  -> IO Vec2+getLinearVelocity a0 =+  alloca $ \pOut -> c_b2MotorJoint_GetLinearVelocity a0 pOut >> peek pOut++-- | Like 'getLinearVelocity', but the result is written into a caller-supplied buffer.+getLinearVelocityInto+  :: JointId+  -> Ptr Vec2+  -- ^ Result buffer.+  -> IO ()+getLinearVelocityInto a0 out =+  c_b2MotorJoint_GetLinearVelocity a0 out++{- | Set the desired relative angular velocity in radians per second++Binds @b2MotorJoint_SetAngularVelocity@.+-}+setAngularVelocity+  :: JointId+  -> Float+  -- ^ @velocity@+  -> IO ()+setAngularVelocity a0 a1 =+  c_b2MotorJoint_SetAngularVelocity a0 a1++{- | Get the desired relative angular velocity in radians per second++Binds @b2MotorJoint_GetAngularVelocity@.+-}+getAngularVelocity+  :: JointId+  -> IO Float+getAngularVelocity a0 =+  c_b2MotorJoint_GetAngularVelocity a0++{- | Set the motor joint maximum force, usually in newtons++Binds @b2MotorJoint_SetMaxVelocityForce@.+-}+setMaxVelocityForce+  :: JointId+  -> Float+  -- ^ @maxForce@+  -> IO ()+setMaxVelocityForce a0 a1 =+  c_b2MotorJoint_SetMaxVelocityForce a0 a1++{- | Get the motor joint maximum force, usually in newtons++Binds @b2MotorJoint_GetMaxVelocityForce@.+-}+getMaxVelocityForce+  :: JointId+  -> IO Float+getMaxVelocityForce a0 =+  c_b2MotorJoint_GetMaxVelocityForce a0++{- | Set the motor joint maximum torque, usually in newton-meters++Binds @b2MotorJoint_SetMaxVelocityTorque@.+-}+setMaxVelocityTorque+  :: JointId+  -> Float+  -- ^ @maxTorque@+  -> IO ()+setMaxVelocityTorque a0 a1 =+  c_b2MotorJoint_SetMaxVelocityTorque a0 a1++{- | Get the motor joint maximum torque, usually in newton-meters++Binds @b2MotorJoint_GetMaxVelocityTorque@.+-}+getMaxVelocityTorque+  :: JointId+  -> IO Float+getMaxVelocityTorque a0 =+  c_b2MotorJoint_GetMaxVelocityTorque a0++{- | Set the spring linear hertz stiffness++Binds @b2MotorJoint_SetLinearHertz@.+-}+setLinearHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setLinearHertz a0 a1 =+  c_b2MotorJoint_SetLinearHertz a0 a1++{- | Get the spring linear hertz stiffness++Binds @b2MotorJoint_GetLinearHertz@.+-}+getLinearHertz+  :: JointId+  -> IO Float+getLinearHertz a0 =+  c_b2MotorJoint_GetLinearHertz a0++{- | Set the spring linear damping ratio. Use 1.0 for critical damping.++Binds @b2MotorJoint_SetLinearDampingRatio@.+-}+setLinearDampingRatio+  :: JointId+  -> Float+  -- ^ @damping@+  -> IO ()+setLinearDampingRatio a0 a1 =+  c_b2MotorJoint_SetLinearDampingRatio a0 a1++{- | Get the spring linear damping ratio.++Binds @b2MotorJoint_GetLinearDampingRatio@.+-}+getLinearDampingRatio+  :: JointId+  -> IO Float+getLinearDampingRatio a0 =+  c_b2MotorJoint_GetLinearDampingRatio a0++{- | Set the spring angular hertz stiffness++Binds @b2MotorJoint_SetAngularHertz@.+-}+setAngularHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setAngularHertz a0 a1 =+  c_b2MotorJoint_SetAngularHertz a0 a1++{- | Get the spring angular hertz stiffness++Binds @b2MotorJoint_GetAngularHertz@.+-}+getAngularHertz+  :: JointId+  -> IO Float+getAngularHertz a0 =+  c_b2MotorJoint_GetAngularHertz a0++{- | Set the spring angular damping ratio. Use 1.0 for critical damping.++Binds @b2MotorJoint_SetAngularDampingRatio@.+-}+setAngularDampingRatio+  :: JointId+  -> Float+  -- ^ @damping@+  -> IO ()+setAngularDampingRatio a0 a1 =+  c_b2MotorJoint_SetAngularDampingRatio a0 a1++{- | Get the spring angular damping ratio.++Binds @b2MotorJoint_GetAngularDampingRatio@.+-}+getAngularDampingRatio+  :: JointId+  -> IO Float+getAngularDampingRatio a0 =+  c_b2MotorJoint_GetAngularDampingRatio a0++{- | Set the maximum spring force in newtons.++Binds @b2MotorJoint_SetMaxSpringForce@.+-}+setMaxSpringForce+  :: JointId+  -> Float+  -- ^ @maxForce@+  -> IO ()+setMaxSpringForce a0 a1 =+  c_b2MotorJoint_SetMaxSpringForce a0 a1++{- | Get the maximum spring force in newtons.++Binds @b2MotorJoint_GetMaxSpringForce@.+-}+getMaxSpringForce+  :: JointId+  -> IO Float+getMaxSpringForce a0 =+  c_b2MotorJoint_GetMaxSpringForce a0++{- | Set the maximum spring torque in newtons * meters++Binds @b2MotorJoint_SetMaxSpringTorque@.+-}+setMaxSpringTorque+  :: JointId+  -> Float+  -- ^ @maxTorque@+  -> IO ()+setMaxSpringTorque a0 a1 =+  c_b2MotorJoint_SetMaxSpringTorque a0 a1++{- | Get the maximum spring torque in newtons * meters++Binds @b2MotorJoint_GetMaxSpringTorque@.+-}+getMaxSpringTorque+  :: JointId+  -> IO Float+getMaxSpringTorque a0 =+  c_b2MotorJoint_GetMaxSpringTorque a0
+ src/Box2D/Mover.hs view
@@ -0,0 +1,53 @@+{-| Friendly surface over the engine's mover plane solver.++The raw bindings ("Box2D.Collision".@solvePlanes@ / @clipVector@) work on a+mutable @b2CollisionPlane@ array: @b2SolvePlanes@ accumulates each plane's+@push@ in place and @b2ClipVector@ 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 'Box2D.Collision.PlaneResult's gathered+by 'Box2D.World.collideMover'.+-}+module Box2D.Mover+  ( solvePlanes+  , clipVector+  ) where++import Data.Vector.Storable qualified as VS+import Data.Vector.Storable.Mutable qualified as VSM++import Box2D.Collision (CollisionPlane, PlaneSolverResult (..))+import Box2D.Collision qualified as Collision+import Box2D.MathTypes (Vec2)++{- | 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 :: Vec2 -> VS.Vector CollisionPlane -> IO (Vec2, 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 :: Vec2 -> VS.Vector CollisionPlane -> IO Vec2+clipVector vector planes =+  VS.unsafeWith planes $ \p ->+    Collision.clipVector vector p (VS.length planes)
+ src/Box2D/MoverJoint.hsc view
@@ -0,0 +1,157 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.MoverJoint+  ( MoverJointDef (..)+  , defaultMoverJointDef+  , create+  , setLinearVelocity+  , getLinearVelocity+  , getLinearVelocityInto+  , setMaxVelocityForce+  , getMaxVelocityForce+  , getMaxVelocityForceInto+  )+  where++import Foreign+import Foreign.C.Types (CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (JointId (..), WorldId (..))+import Box2D.Joint (JointDef)+import Box2D.MathTypes (Vec2)++#include "box2d/box2d.h"+++{- | A mover joint is used to move a dynamic character mover through velocity commands. The x and y+directions are handled separately. Does not affect rotation.+-}+data MoverJointDef = MoverJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , linearVelocity :: Vec2+  -- ^ The desired linear velocity+  , maxVelocityForce :: Vec2+  -- ^ The maximum motor force in newtons.+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable MoverJointDef where+  sizeOf _ = #{size b2MoverJointDef}+  alignment _ = #{alignment b2MoverJointDef}+  peek p =+    MoverJointDef+      <$> #{peek b2MoverJointDef, base} p+      <*> #{peek b2MoverJointDef, linearVelocity} p+      <*> #{peek b2MoverJointDef, maxVelocityForce} p+      <*> #{peek b2MoverJointDef, internalValue} p+  poke p (MoverJointDef f1 f2 f3 f4) = do+    #{poke b2MoverJointDef, base} p f1+    #{poke b2MoverJointDef, linearVelocity} p f2+    #{poke b2MoverJointDef, maxVelocityForce} p f3+    #{poke b2MoverJointDef, internalValue} p f4++foreign import ccall unsafe "hs_b2DefaultMoverJointDef"+  c_b2DefaultMoverJointDef :: Ptr MoverJointDef -> IO ()++defaultMoverJointDef :: IO MoverJointDef+defaultMoverJointDef = allocaOut c_b2DefaultMoverJointDef+++foreign import ccall unsafe "b2CreateMoverJoint"+  c_b2CreateMoverJoint :: WorldId -> Ptr MoverJointDef -> IO JointId++foreign import ccall unsafe "hsg_b2MoverJoint_SetLinearVelocity"+  c_b2MoverJoint_SetLinearVelocity :: JointId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2MoverJoint_GetLinearVelocity"+  c_b2MoverJoint_GetLinearVelocity :: JointId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2MoverJoint_SetMaxVelocityForce"+  c_b2MoverJoint_SetMaxVelocityForce :: JointId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2MoverJoint_GetMaxVelocityForce"+  c_b2MoverJoint_GetMaxVelocityForce :: JointId -> Ptr Vec2 -> IO ()++{- | Create a mover joint++See also @b2MoverJointDef@ for details.++Binds @b2CreateMoverJoint@.+-}+create+  :: WorldId+  -> Ptr MoverJointDef+  -> IO JointId+create a0 a1 =+  c_b2CreateMoverJoint a0 a1++{- | Set the desired relative linear velocity in meters per second++Binds @b2MoverJoint_SetLinearVelocity@.+-}+setLinearVelocity+  :: JointId+  -> Vec2+  -- ^ @velocity@+  -> IO ()+setLinearVelocity a0 a1 =+  with a1 $ \p1 ->+  c_b2MoverJoint_SetLinearVelocity a0 p1++{- | Get the desired relative linear velocity in meters per second++Binds @b2MoverJoint_GetLinearVelocity@.+-}+getLinearVelocity+  :: JointId+  -> IO Vec2+getLinearVelocity a0 =+  alloca $ \pOut -> c_b2MoverJoint_GetLinearVelocity a0 pOut >> peek pOut++-- | Like 'getLinearVelocity', but the result is written into a caller-supplied buffer.+getLinearVelocityInto+  :: JointId+  -> Ptr Vec2+  -- ^ Result buffer.+  -> IO ()+getLinearVelocityInto a0 out =+  c_b2MoverJoint_GetLinearVelocity a0 out++{- | Set the joint maximum force, usually in newtons++Binds @b2MoverJoint_SetMaxVelocityForce@.+-}+setMaxVelocityForce+  :: JointId+  -> Vec2+  -- ^ @maxForce@+  -> IO ()+setMaxVelocityForce a0 a1 =+  with a1 $ \p1 ->+  c_b2MoverJoint_SetMaxVelocityForce a0 p1++{- | Get the joint maximum force, usually in newtons++Binds @b2MoverJoint_GetMaxVelocityForce@.+-}+getMaxVelocityForce+  :: JointId+  -> IO Vec2+getMaxVelocityForce a0 =+  alloca $ \pOut -> c_b2MoverJoint_GetMaxVelocityForce a0 pOut >> peek pOut++-- | Like 'getMaxVelocityForce', but the result is written into a caller-supplied buffer.+getMaxVelocityForceInto+  :: JointId+  -> Ptr Vec2+  -- ^ Result buffer.+  -> IO ()+getMaxVelocityForceInto a0 out =+  c_b2MoverJoint_GetMaxVelocityForce a0 out
+ src/Box2D/PogoJoint.hsc view
@@ -0,0 +1,232 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.PogoJoint+  ( PogoJointDef (..)+  , defaultPogoJointDef+  , create+  , setRestLength+  , getRestLength+  , setSpringHertz+  , getSpringHertz+  , setSpringDampingRatio+  , getSpringDampingRatio+  , getCurrentLength+  , getCurrentSpeed+  , getImpulse+  )+  where++import Foreign+import Foreign.C.Types (CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (JointId (..), WorldId (..))+import Box2D.Joint (JointDef)+import Box2D.MathTypes (Vec2)++#include "box2d/box2d.h"+++-- | Pogo joint definition+data PogoJointDef = PogoJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , normal :: Vec2+  -- ^ Normal vector at the contact+  , hertz :: Float+  -- ^ The spring stiffness Hertz, cycles per second+  , dampingRatio :: Float+  -- ^ The spring damping ratio, non-dimensional+  , restLength :: Float+  -- ^ Spring length.+  , maxTensionForce :: Float+  -- ^ Maximum tension force. Controls how sticky the mover is to ground.+  , maxCompressionForce :: Float+  -- ^ Maximum compression force. How hard the pogo pushes the mover up from the ground.+  , impulse :: Float+  -- ^ The initial pogo internal impulse. The pogo is typically recreated every frame. This gives a+  -- way to move the internal state forward for smoother spring behavior.+  , velocity :: Float+  -- ^ The initial pogo internal velocity. The pogo is typically recreated every frame. This gives+  -- a way to move the internal state forward for smoother spring behavior.+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable PogoJointDef where+  sizeOf _ = #{size b2PogoJointDef}+  alignment _ = #{alignment b2PogoJointDef}+  peek p =+    PogoJointDef+      <$> #{peek b2PogoJointDef, base} p+      <*> #{peek b2PogoJointDef, normal} p+      <*> #{peek b2PogoJointDef, hertz} p+      <*> #{peek b2PogoJointDef, dampingRatio} p+      <*> #{peek b2PogoJointDef, restLength} p+      <*> #{peek b2PogoJointDef, maxTensionForce} p+      <*> #{peek b2PogoJointDef, maxCompressionForce} p+      <*> #{peek b2PogoJointDef, impulse} p+      <*> #{peek b2PogoJointDef, velocity} p+      <*> #{peek b2PogoJointDef, internalValue} p+  poke p (PogoJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10) = do+    #{poke b2PogoJointDef, base} p f1+    #{poke b2PogoJointDef, normal} p f2+    #{poke b2PogoJointDef, hertz} p f3+    #{poke b2PogoJointDef, dampingRatio} p f4+    #{poke b2PogoJointDef, restLength} p f5+    #{poke b2PogoJointDef, maxTensionForce} p f6+    #{poke b2PogoJointDef, maxCompressionForce} p f7+    #{poke b2PogoJointDef, impulse} p f8+    #{poke b2PogoJointDef, velocity} p f9+    #{poke b2PogoJointDef, internalValue} p f10++foreign import ccall unsafe "hs_b2DefaultPogoJointDef"+  c_b2DefaultPogoJointDef :: Ptr PogoJointDef -> IO ()++defaultPogoJointDef :: IO PogoJointDef+defaultPogoJointDef = allocaOut c_b2DefaultPogoJointDef+++foreign import ccall unsafe "b2CreatePogoJoint"+  c_b2CreatePogoJoint :: WorldId -> Ptr PogoJointDef -> IO JointId++foreign import ccall unsafe "b2PogoJoint_SetRestLength"+  c_b2PogoJoint_SetRestLength :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2PogoJoint_GetRestLength"+  c_b2PogoJoint_GetRestLength :: JointId -> IO Float++foreign import ccall unsafe "b2PogoJoint_SetSpringHertz"+  c_b2PogoJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2PogoJoint_GetSpringHertz"+  c_b2PogoJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b2PogoJoint_SetSpringDampingRatio"+  c_b2PogoJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2PogoJoint_GetSpringDampingRatio"+  c_b2PogoJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b2PogoJoint_GetCurrentLength"+  c_b2PogoJoint_GetCurrentLength :: JointId -> IO Float++foreign import ccall unsafe "b2PogoJoint_GetCurrentSpeed"+  c_b2PogoJoint_GetCurrentSpeed :: JointId -> IO Float++foreign import ccall unsafe "b2PogoJoint_GetImpulse"+  c_b2PogoJoint_GetImpulse :: JointId -> IO Float++{- | Create a pogo joint++See also @b2PogoJointDef@ for details.++Binds @b2CreatePogoJoint@.+-}+create+  :: WorldId+  -> Ptr PogoJointDef+  -> IO JointId+create a0 a1 =+  c_b2CreatePogoJoint a0 a1++{- | Set the rest length of a pogo joint++Binds @b2PogoJoint_SetRestLength@.+-}+setRestLength+  :: JointId+  -- ^ @jointId@: The id for a pogo joint+  -> Float+  -- ^ @length@: The new pogo joint length+  -> IO ()+setRestLength a0 a1 =+  c_b2PogoJoint_SetRestLength a0 a1++{- | Get the rest length of a pogo joint++Binds @b2PogoJoint_GetRestLength@.+-}+getRestLength+  :: JointId+  -> IO Float+getRestLength a0 =+  c_b2PogoJoint_GetRestLength a0++{- | Set the spring stiffness in Hertz++Binds @b2PogoJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b2PogoJoint_SetSpringHertz a0 a1++{- | Get the spring Hertz++Binds @b2PogoJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b2PogoJoint_GetSpringHertz a0++{- | Set the spring damping ratio, non-dimensional++Binds @b2PogoJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b2PogoJoint_SetSpringDampingRatio a0 a1++{- | Get the spring damping ratio++Binds @b2PogoJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b2PogoJoint_GetSpringDampingRatio a0++{- | Get the current length of the pogo.++Binds @b2PogoJoint_GetCurrentLength@.+-}+getCurrentLength+  :: JointId+  -> IO Float+getCurrentLength a0 =+  c_b2PogoJoint_GetCurrentLength a0++{- | Get the internal pogo velocity.++Binds @b2PogoJoint_GetCurrentSpeed@.+-}+getCurrentSpeed+  :: JointId+  -> IO Float+getCurrentSpeed a0 =+  c_b2PogoJoint_GetCurrentSpeed a0++{- | Get the internal pogo impulse.++Binds @b2PogoJoint_GetImpulse@.+-}+getImpulse+  :: JointId+  -> IO Float+getImpulse a0 =+  c_b2PogoJoint_GetImpulse a0
+ src/Box2D/PrismaticJoint.hsc view
@@ -0,0 +1,439 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.PrismaticJoint+  ( PrismaticJointDef (..)+  , defaultPrismaticJointDef+  , create+  , enableSpring+  , isSpringEnabled+  , setSpringHertz+  , getSpringHertz+  , setSpringDampingRatio+  , getSpringDampingRatio+  , setTargetTranslation+  , getTargetTranslation+  , enableLimit+  , isLimitEnabled+  , getLowerLimit+  , getUpperLimit+  , setLimits+  , enableMotor+  , isMotorEnabled+  , setTargetMotorSpeed+  , getTargetMotorSpeed+  , setMaxMotorForce+  , getMaxMotorForce+  , getCurrentTranslation+  , getCurrentSpeed+  , getCurrentMotorForce+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (JointId (..), WorldId (..))+import Box2D.Joint (JointDef)++#include "box2d/box2d.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+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable PrismaticJointDef where+  sizeOf _ = #{size b2PrismaticJointDef}+  alignment _ = #{alignment b2PrismaticJointDef}+  peek p =+    PrismaticJointDef+      <$> #{peek b2PrismaticJointDef, base} p+      <*> #{peek b2PrismaticJointDef, enableSpring} p+      <*> #{peek b2PrismaticJointDef, hertz} p+      <*> #{peek b2PrismaticJointDef, dampingRatio} p+      <*> #{peek b2PrismaticJointDef, targetTranslation} p+      <*> #{peek b2PrismaticJointDef, enableLimit} p+      <*> #{peek b2PrismaticJointDef, lowerTranslation} p+      <*> #{peek b2PrismaticJointDef, upperTranslation} p+      <*> #{peek b2PrismaticJointDef, enableMotor} p+      <*> #{peek b2PrismaticJointDef, maxMotorForce} p+      <*> #{peek b2PrismaticJointDef, motorSpeed} p+      <*> #{peek b2PrismaticJointDef, internalValue} p+  poke p (PrismaticJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12) = do+    #{poke b2PrismaticJointDef, base} p f1+    #{poke b2PrismaticJointDef, enableSpring} p f2+    #{poke b2PrismaticJointDef, hertz} p f3+    #{poke b2PrismaticJointDef, dampingRatio} p f4+    #{poke b2PrismaticJointDef, targetTranslation} p f5+    #{poke b2PrismaticJointDef, enableLimit} p f6+    #{poke b2PrismaticJointDef, lowerTranslation} p f7+    #{poke b2PrismaticJointDef, upperTranslation} p f8+    #{poke b2PrismaticJointDef, enableMotor} p f9+    #{poke b2PrismaticJointDef, maxMotorForce} p f10+    #{poke b2PrismaticJointDef, motorSpeed} p f11+    #{poke b2PrismaticJointDef, internalValue} p f12++foreign import ccall unsafe "hs_b2DefaultPrismaticJointDef"+  c_b2DefaultPrismaticJointDef :: Ptr PrismaticJointDef -> IO ()++defaultPrismaticJointDef :: IO PrismaticJointDef+defaultPrismaticJointDef = allocaOut c_b2DefaultPrismaticJointDef+++foreign import ccall unsafe "b2CreatePrismaticJoint"+  c_b2CreatePrismaticJoint :: WorldId -> Ptr PrismaticJointDef -> IO JointId++foreign import ccall unsafe "b2PrismaticJoint_EnableSpring"+  c_b2PrismaticJoint_EnableSpring :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2PrismaticJoint_IsSpringEnabled"+  c_b2PrismaticJoint_IsSpringEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2PrismaticJoint_SetSpringHertz"+  c_b2PrismaticJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2PrismaticJoint_GetSpringHertz"+  c_b2PrismaticJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b2PrismaticJoint_SetSpringDampingRatio"+  c_b2PrismaticJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2PrismaticJoint_GetSpringDampingRatio"+  c_b2PrismaticJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b2PrismaticJoint_SetTargetTranslation"+  c_b2PrismaticJoint_SetTargetTranslation :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2PrismaticJoint_GetTargetTranslation"+  c_b2PrismaticJoint_GetTargetTranslation :: JointId -> IO Float++foreign import ccall unsafe "b2PrismaticJoint_EnableLimit"+  c_b2PrismaticJoint_EnableLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2PrismaticJoint_IsLimitEnabled"+  c_b2PrismaticJoint_IsLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2PrismaticJoint_GetLowerLimit"+  c_b2PrismaticJoint_GetLowerLimit :: JointId -> IO Float++foreign import ccall unsafe "b2PrismaticJoint_GetUpperLimit"+  c_b2PrismaticJoint_GetUpperLimit :: JointId -> IO Float++foreign import ccall unsafe "b2PrismaticJoint_SetLimits"+  c_b2PrismaticJoint_SetLimits :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b2PrismaticJoint_EnableMotor"+  c_b2PrismaticJoint_EnableMotor :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2PrismaticJoint_IsMotorEnabled"+  c_b2PrismaticJoint_IsMotorEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2PrismaticJoint_SetTargetMotorSpeed"+  c_b2PrismaticJoint_SetTargetMotorSpeed :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2PrismaticJoint_GetTargetMotorSpeed"+  c_b2PrismaticJoint_GetTargetMotorSpeed :: JointId -> IO Float++foreign import ccall unsafe "b2PrismaticJoint_SetMaxMotorForce"+  c_b2PrismaticJoint_SetMaxMotorForce :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2PrismaticJoint_GetMaxMotorForce"+  c_b2PrismaticJoint_GetMaxMotorForce :: JointId -> IO Float++foreign import ccall unsafe "b2PrismaticJoint_GetCurrentTranslation"+  c_b2PrismaticJoint_GetCurrentTranslation :: JointId -> IO Float++foreign import ccall unsafe "b2PrismaticJoint_GetCurrentSpeed"+  c_b2PrismaticJoint_GetCurrentSpeed :: JointId -> IO Float++foreign import ccall unsafe "b2PrismaticJoint_GetCurrentMotorForce"+  c_b2PrismaticJoint_GetCurrentMotorForce :: JointId -> IO Float++{- | Create a prismatic (slider) joint.++See also @b2PrismaticJointDef@ for details.++Binds @b2CreatePrismaticJoint@.+-}+create+  :: WorldId+  -> PrismaticJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b2CreatePrismaticJoint a0 p1++{- | Enable\/disable the joint spring.++Binds @b2PrismaticJoint_EnableSpring@.+-}+enableSpring+  :: JointId+  -> Bool+  -- ^ @enableSpring@+  -> IO ()+enableSpring a0 a1 =+  c_b2PrismaticJoint_EnableSpring a0 (fromBool a1)++{- | Is the prismatic joint spring enabled or not?++Binds @b2PrismaticJoint_IsSpringEnabled@.+-}+isSpringEnabled+  :: JointId+  -> IO Bool+isSpringEnabled a0 =+  toBool <$> (c_b2PrismaticJoint_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 @b2PrismaticJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b2PrismaticJoint_SetSpringHertz a0 a1++{- | Get the prismatic joint stiffness in Hertz++Binds @b2PrismaticJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b2PrismaticJoint_GetSpringHertz a0++{- | Set the prismatic joint damping ratio (non-dimensional)++Binds @b2PrismaticJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b2PrismaticJoint_SetSpringDampingRatio a0 a1++{- | Get the prismatic spring damping ratio (non-dimensional)++Binds @b2PrismaticJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b2PrismaticJoint_GetSpringDampingRatio a0++{- | Set the prismatic joint spring target translation, usually in meters++Binds @b2PrismaticJoint_SetTargetTranslation@.+-}+setTargetTranslation+  :: JointId+  -> Float+  -- ^ @translation@+  -> IO ()+setTargetTranslation a0 a1 =+  c_b2PrismaticJoint_SetTargetTranslation a0 a1++{- | Get the prismatic joint spring target translation, usually in meters++Binds @b2PrismaticJoint_GetTargetTranslation@.+-}+getTargetTranslation+  :: JointId+  -> IO Float+getTargetTranslation a0 =+  c_b2PrismaticJoint_GetTargetTranslation a0++{- | Enable\/disable a prismatic joint limit++Binds @b2PrismaticJoint_EnableLimit@.+-}+enableLimit+  :: JointId+  -> Bool+  -- ^ @enableLimit@+  -> IO ()+enableLimit a0 a1 =+  c_b2PrismaticJoint_EnableLimit a0 (fromBool a1)++{- | Is the prismatic joint limit enabled?++Binds @b2PrismaticJoint_IsLimitEnabled@.+-}+isLimitEnabled+  :: JointId+  -> IO Bool+isLimitEnabled a0 =+  toBool <$> (c_b2PrismaticJoint_IsLimitEnabled a0)++{- | Get the prismatic joint lower limit++Binds @b2PrismaticJoint_GetLowerLimit@.+-}+getLowerLimit+  :: JointId+  -> IO Float+getLowerLimit a0 =+  c_b2PrismaticJoint_GetLowerLimit a0++{- | Get the prismatic joint upper limit++Binds @b2PrismaticJoint_GetUpperLimit@.+-}+getUpperLimit+  :: JointId+  -> IO Float+getUpperLimit a0 =+  c_b2PrismaticJoint_GetUpperLimit a0++{- | Set the prismatic joint limits++Binds @b2PrismaticJoint_SetLimits@.+-}+setLimits+  :: JointId+  -> Float+  -- ^ @lower@+  -> Float+  -- ^ @upper@+  -> IO ()+setLimits a0 a1 a2 =+  c_b2PrismaticJoint_SetLimits a0 a1 a2++{- | Enable\/disable a prismatic joint motor++Binds @b2PrismaticJoint_EnableMotor@.+-}+enableMotor+  :: JointId+  -> Bool+  -- ^ @enableMotor@+  -> IO ()+enableMotor a0 a1 =+  c_b2PrismaticJoint_EnableMotor a0 (fromBool a1)++{- | Is the prismatic joint motor enabled?++Binds @b2PrismaticJoint_IsMotorEnabled@.+-}+isMotorEnabled+  :: JointId+  -> IO Bool+isMotorEnabled a0 =+  toBool <$> (c_b2PrismaticJoint_IsMotorEnabled a0)++{- | Set the prismatic joint motor speed, usually in meters per second++Binds @b2PrismaticJoint_SetTargetMotorSpeed@.+-}+setTargetMotorSpeed+  :: JointId+  -> Float+  -- ^ @motorSpeed@+  -> IO ()+setTargetMotorSpeed a0 a1 =+  c_b2PrismaticJoint_SetTargetMotorSpeed a0 a1++{- | Get the prismatic joint motor speed, usually in meters per second++Binds @b2PrismaticJoint_GetTargetMotorSpeed@.+-}+getTargetMotorSpeed+  :: JointId+  -> IO Float+getTargetMotorSpeed a0 =+  c_b2PrismaticJoint_GetTargetMotorSpeed a0++{- | Set the prismatic joint maximum motor force, usually in newtons++Binds @b2PrismaticJoint_SetMaxMotorForce@.+-}+setMaxMotorForce+  :: JointId+  -> Float+  -- ^ @force@+  -> IO ()+setMaxMotorForce a0 a1 =+  c_b2PrismaticJoint_SetMaxMotorForce a0 a1++{- | Get the prismatic joint maximum motor force, usually in newtons++Binds @b2PrismaticJoint_GetMaxMotorForce@.+-}+getMaxMotorForce+  :: JointId+  -> IO Float+getMaxMotorForce a0 =+  c_b2PrismaticJoint_GetMaxMotorForce a0++{- | Get the current joint translation, usually in meters.++Binds @b2PrismaticJoint_GetCurrentTranslation@.+-}+getCurrentTranslation+  :: JointId+  -> IO Float+getCurrentTranslation a0 =+  c_b2PrismaticJoint_GetCurrentTranslation a0++{- | Get the current joint translation velocity, usually in meters per second.++Binds @b2PrismaticJoint_GetCurrentSpeed@.+-}+getCurrentSpeed+  :: JointId+  -> IO Float+getCurrentSpeed a0 =+  c_b2PrismaticJoint_GetCurrentSpeed a0++{- | Get the prismatic joint current motor force, usually in newtons++Binds @b2PrismaticJoint_GetCurrentMotorForce@.+-}+getCurrentMotorForce+  :: JointId+  -> IO Float+getCurrentMotorForce a0 =+  c_b2PrismaticJoint_GetCurrentMotorForce a0
+ src/Box2D/Recording.hs view
@@ -0,0 +1,107 @@+-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.Recording+  ( create+  , destroy+  , getData+  , getSize+  , saveToFile+  , createFromFile+  )+where++import Box2D.Tags (Recording)+import Foreign+import Foreign.C.String (CString)+import Foreign.C.Types (CBool (..), CInt (..))++foreign import ccall unsafe "b2CreateRecording"+  c_b2CreateRecording :: CInt -> IO (Ptr Recording)++foreign import ccall unsafe "b2DestroyRecording"+  c_b2DestroyRecording :: Ptr Recording -> IO ()++foreign import ccall unsafe "b2Recording_GetData"+  c_b2Recording_GetData :: Ptr Recording -> IO (Ptr Word8)++foreign import ccall unsafe "b2Recording_GetSize"+  c_b2Recording_GetSize :: Ptr Recording -> IO CInt++foreign import ccall unsafe "b2Recording_SaveToFile"+  c_b2Recording_SaveToFile :: Ptr Recording -> CString -> IO CBool++foreign import ccall unsafe "b2CreateRecordingFromFile"+  c_b2CreateRecordingFromFile :: CString -> IO (Ptr Recording)++{- | Create a recording buffer. The buffer grows on demand. \@p byteCapacity pre-sizes it to avoid+reallocations during a known-length session. Pass 0 for a small default.++Returns: A recording handle, freed with @b2DestroyRecording@++Binds @b2CreateRecording@.+-}+create+  :: Int+  -- ^ @byteCapacity@+  -> IO (Ptr Recording)+create a0 =+  c_b2CreateRecording (fromIntegral a0)++{- | Destroy a recording buffer and free its memory.++Binds @b2DestroyRecording@.+-}+destroy+  :: Ptr Recording+  -> IO ()+destroy a0 =+  c_b2DestroyRecording a0++{- | Get a pointer to the recorded bytes, for saving to a file or transmitting. Valid until the next+recording call or @b2DestroyRecording@.++Binds @b2Recording_GetData@.+-}+getData+  :: Ptr Recording+  -> IO (Ptr Word8)+getData a0 =+  c_b2Recording_GetData a0++{- | Get the number of recorded bytes.++Binds @b2Recording_GetSize@.+-}+getSize+  :: Ptr Recording+  -> IO Int+getSize a0 =+  fromIntegral <$> (c_b2Recording_GetSize a0)++{- | Save a recording buffer to a file. Convenience wrapper over your own file I\/O.++Returns: false if the file could not be written++Binds @b2Recording_SaveToFile@.+-}+saveToFile+  :: Ptr Recording+  -> CString+  -- ^ @path@+  -> IO Bool+saveToFile a0 a1 =+  toBool <$> (c_b2Recording_SaveToFile a0 a1)++{- | Load a recording from a file into a new recording buffer. Convenience wrapper over your own file+I\/O. Destroy the result with @b2DestroyRecording@.++Returns: NULL if the file is missing or unreadable++Binds @b2CreateRecordingFromFile@.+-}+createFromFile+  :: CString+  -- ^ @path@+  -> IO (Ptr Recording)+createFromFile a0 =+  c_b2CreateRecordingFromFile a0
+ src/Box2D/Replay.hsc view
@@ -0,0 +1,482 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.Replay+  ( ReplayQueryHit (..)+  , ReplayQueryType (..)+  , pattern ReplayQueryOverlapAABB+  , pattern ReplayQueryOverlapShape+  , pattern ReplayQueryCastRay+  , pattern ReplayQueryCastShape+  , pattern ReplayQueryCollideMover+  , pattern ReplayQueryCastRayClosest+  , pattern ReplayQueryCastMover+  , ReplayInfo (..)+  , create+  , destroy+  , stepFrame+  , getWorldId+  , restart+  , seekFrame+  , getFrame+  , getInfo+  , isAtEnd+  , hasDiverged+  , getDivergeFrame+  , setKeyframePolicy+  , getKeyframeBudget+  , getKeyframeMinInterval+  , getKeyframeInterval+  , getKeyframeBytes+  , drawFrameQueries+  , getFrameQueryCount+  , getFrameQuery+  , getFrameQueryHit+  , getBodyCount+  , getBodyId+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..), CSize (..))+import Box2D.DebugDraw (DebugDraw)+import Box2D.Id (BodyId (..), ShapeId, WorldId (..))+import Box2D.MathTypes (AABB, Pos, Vec2)+import Box2D.Tags (Replay, ReplayQueryInfo)++#include "box2d/box2d.h"+++-- | One result of a recorded spatial query.+data ReplayQueryHit = ReplayQueryHit+  { shapeId :: ShapeId+  -- ^ the shape hit+  , point :: Pos+  -- ^ the hit point on the shape surface+  , normal :: Vec2+  -- ^ the unit normal vector at the hit point+  , fraction :: Float+  -- ^ the query translation fraction+  }+  deriving (Eq, Show)++instance Storable ReplayQueryHit where+  sizeOf _ = #{size b2ReplayQueryHit}+  alignment _ = #{alignment b2ReplayQueryHit}+  peek p =+    ReplayQueryHit+      <$> #{peek b2ReplayQueryHit, shapeId} p+      <*> #{peek b2ReplayQueryHit, point} p+      <*> #{peek b2ReplayQueryHit, normal} p+      <*> #{peek b2ReplayQueryHit, fraction} p+  poke p (ReplayQueryHit f1 f2 f3 f4) = do+    #{poke b2ReplayQueryHit, shapeId} p f1+    #{poke b2ReplayQueryHit, point} p f2+    #{poke b2ReplayQueryHit, normal} p f3+    #{poke b2ReplayQueryHit, fraction} p f4++-- | The kind of a recorded spatial query, matching the public query and cast functions.+newtype ReplayQueryType = ReplayQueryType CInt+  deriving (Eq, Show, Storable)++pattern ReplayQueryOverlapAABB :: ReplayQueryType+pattern ReplayQueryOverlapShape :: ReplayQueryType+pattern ReplayQueryCastRay :: ReplayQueryType+pattern ReplayQueryCastShape :: ReplayQueryType+pattern ReplayQueryCollideMover :: ReplayQueryType+pattern ReplayQueryCastRayClosest :: ReplayQueryType+pattern ReplayQueryCastMover :: ReplayQueryType+pattern ReplayQueryOverlapAABB = ReplayQueryType #{const b2_replayQueryOverlapAABB}+pattern ReplayQueryOverlapShape = ReplayQueryType #{const b2_replayQueryOverlapShape}+pattern ReplayQueryCastRay = ReplayQueryType #{const b2_replayQueryCastRay}+pattern ReplayQueryCastShape = ReplayQueryType #{const b2_replayQueryCastShape}+pattern ReplayQueryCollideMover = ReplayQueryType #{const b2_replayQueryCollideMover}+pattern ReplayQueryCastRayClosest = ReplayQueryType #{const b2_replayQueryCastRayClosest}+pattern ReplayQueryCastMover = ReplayQueryType #{const b2_replayQueryCastMover}++-- | Static metadata describing a recording, resolved once when the player opens the file.+data ReplayInfo = ReplayInfo+  { frameCount :: CInt+  -- ^ total recorded steps+  , workerCount :: CInt+  -- ^ worker count used for the replay world+  , timeStep :: Float+  -- ^ dt of the recorded steps+  , subStepCount :: CInt+  -- ^ recorded sub-steps+  , lengthScale :: Float+  -- ^ length units per meter in effect when recorded+  , bounds :: AABB+  -- ^ accumulated world bounds over the recording, zero-extent if unavailable+  }+  deriving (Eq, Show)++instance Storable ReplayInfo where+  sizeOf _ = #{size b2ReplayInfo}+  alignment _ = #{alignment b2ReplayInfo}+  peek p =+    ReplayInfo+      <$> #{peek b2ReplayInfo, frameCount} p+      <*> #{peek b2ReplayInfo, workerCount} p+      <*> #{peek b2ReplayInfo, timeStep} p+      <*> #{peek b2ReplayInfo, subStepCount} p+      <*> #{peek b2ReplayInfo, lengthScale} p+      <*> #{peek b2ReplayInfo, bounds} p+  poke p (ReplayInfo f1 f2 f3 f4 f5 f6) = do+    #{poke b2ReplayInfo, frameCount} p f1+    #{poke b2ReplayInfo, workerCount} p f2+    #{poke b2ReplayInfo, timeStep} p f3+    #{poke b2ReplayInfo, subStepCount} p f4+    #{poke b2ReplayInfo, lengthScale} p f5+    #{poke b2ReplayInfo, bounds} p f6+++foreign import ccall unsafe "b2CreateReplay"+  c_b2CreateReplay :: Ptr () -> CInt -> CInt -> IO (Ptr Replay)++foreign import ccall unsafe "b2DestroyReplay"+  c_b2DestroyReplay :: Ptr Replay -> IO ()++foreign import ccall safe "b2Replay_StepFrame"+  c_b2Replay_StepFrame :: Ptr Replay -> IO CBool++foreign import ccall unsafe "b2Replay_GetWorldId"+  c_b2Replay_GetWorldId :: Ptr Replay -> IO WorldId++foreign import ccall unsafe "b2Replay_Restart"+  c_b2Replay_Restart :: Ptr Replay -> IO ()++foreign import ccall safe "b2Replay_SeekFrame"+  c_b2Replay_SeekFrame :: Ptr Replay -> CInt -> IO ()++foreign import ccall unsafe "b2Replay_GetFrame"+  c_b2Replay_GetFrame :: Ptr Replay -> IO CInt++foreign import ccall unsafe "hsg_b2Replay_GetInfo"+  c_b2Replay_GetInfo :: Ptr Replay -> Ptr ReplayInfo -> IO ()++foreign import ccall unsafe "b2Replay_IsAtEnd"+  c_b2Replay_IsAtEnd :: Ptr Replay -> IO CBool++foreign import ccall unsafe "b2Replay_HasDiverged"+  c_b2Replay_HasDiverged :: Ptr Replay -> IO CBool++foreign import ccall unsafe "b2Replay_GetDivergeFrame"+  c_b2Replay_GetDivergeFrame :: Ptr Replay -> IO CInt++foreign import ccall unsafe "b2Replay_SetKeyframePolicy"+  c_b2Replay_SetKeyframePolicy :: Ptr Replay -> CSize -> CInt -> IO ()++foreign import ccall unsafe "b2Replay_GetKeyframeBudget"+  c_b2Replay_GetKeyframeBudget :: Ptr Replay -> IO CSize++foreign import ccall unsafe "b2Replay_GetKeyframeMinInterval"+  c_b2Replay_GetKeyframeMinInterval :: Ptr Replay -> IO CInt++foreign import ccall unsafe "b2Replay_GetKeyframeInterval"+  c_b2Replay_GetKeyframeInterval :: Ptr Replay -> IO CInt++foreign import ccall unsafe "b2Replay_GetKeyframeBytes"+  c_b2Replay_GetKeyframeBytes :: Ptr Replay -> IO CSize++foreign import ccall safe "b2Replay_DrawFrameQueries"+  c_b2Replay_DrawFrameQueries :: Ptr Replay -> Ptr DebugDraw -> CInt -> IO ()++foreign import ccall unsafe "b2Replay_GetFrameQueryCount"+  c_b2Replay_GetFrameQueryCount :: Ptr Replay -> IO CInt++foreign import ccall unsafe "hsg_b2Replay_GetFrameQuery"+  c_b2Replay_GetFrameQuery :: Ptr Replay -> CInt -> Ptr ReplayQueryInfo -> IO ()++foreign import ccall unsafe "hsg_b2Replay_GetFrameQueryHit"+  c_b2Replay_GetFrameQueryHit :: Ptr Replay -> CInt -> CInt -> Ptr ReplayQueryHit -> IO ()++foreign import ccall unsafe "b2Replay_GetBodyCount"+  c_b2Replay_GetBodyCount :: Ptr Replay -> IO CInt++foreign import ccall unsafe "b2Replay_GetBodyId"+  c_b2Replay_GetBodyId :: Ptr Replay -> CInt -> IO BodyId++{- | Open a recording for incremental playback and replay up to the first step. The player copies the+bytes, so you may free or destroy the source buffer immediately after this call.++Returns: A player handle, or NULL if the recording is malformed++Binds @b2CreateReplay@.+-}+create+  :: Ptr ()+  -- ^ @data@: Recorded bytes+  -> Int+  -- ^ @size@: Number of recorded bytes+  -> Int+  -- ^ @workerCount@: Worker count for the replay world. 0 uses the serial single-worker fallback.+  -> IO (Ptr Replay)+create a0 a1 a2 =+  c_b2CreateReplay a0 (fromIntegral a1) (fromIntegral a2)++{- | Close a player and free its replay world and file buffer.++Binds @b2DestroyReplay@.+-}+destroy+  :: Ptr Replay+  -- ^ @player@+  -> IO ()+destroy a0 =+  c_b2DestroyReplay a0++{- | Advance the replay by one recorded step.++Returns: true if a step executed, false once the end of the recording is reached++Binds @b2Replay_StepFrame@.+-}+stepFrame+  :: Ptr Replay+  -- ^ @player@+  -> IO Bool+stepFrame a0 =+  toBool <$> (c_b2Replay_StepFrame a0)++{- | Get the id of the replayed world.++Binds @b2Replay_GetWorldId@.+-}+getWorldId+  :: Ptr Replay+  -- ^ @player@+  -> IO WorldId+getWorldId a0 =+  c_b2Replay_GetWorldId a0++{- | Rewind the player to the first step, recreating the replay world from the file.++Binds @b2Replay_Restart@.+-}+restart+  :: Ptr Replay+  -- ^ @player@+  -> IO ()+restart a0 =+  c_b2Replay_Restart a0++{- | Seek to a recorded step. Seeking backward rewinds and re-runs from the nearest keyframe. Clamps+to the recording bounds.++Binds @b2Replay_SeekFrame@.+-}+seekFrame+  :: Ptr Replay+  -- ^ @player@+  -> Int+  -- ^ @targetFrame@+  -> IO ()+seekFrame a0 a1 =+  c_b2Replay_SeekFrame a0 (fromIntegral a1)++{- | Get the number of steps replayed so far.++Binds @b2Replay_GetFrame@.+-}+getFrame+  :: Ptr Replay+  -- ^ @player@+  -> IO Int+getFrame a0 =+  fromIntegral <$> (c_b2Replay_GetFrame a0)++{- | Get static metadata for the recording (frame count, recorded tuning, time).++Binds @b2Replay_GetInfo@.+-}+getInfo+  :: Ptr Replay+  -- ^ @player@+  -> Ptr ReplayInfo+  -- ^ Result buffer.+  -> IO ()+getInfo a0 out =+  c_b2Replay_GetInfo a0 out++{- | Returns true once the end of the recording has been reached.++Binds @b2Replay_IsAtEnd@.+-}+isAtEnd+  :: Ptr Replay+  -- ^ @player@+  -> IO Bool+isAtEnd a0 =+  toBool <$> (c_b2Replay_IsAtEnd a0)++{- | Returns true if a recorded state hash failed to reproduce, meaning replay diverged.++Binds @b2Replay_HasDiverged@.+-}+hasDiverged+  :: Ptr Replay+  -- ^ @player@+  -> IO Bool+hasDiverged a0 =+  toBool <$> (c_b2Replay_HasDiverged a0)++{- | Get the first step at which replay diverged, or -1 if it has not diverged.++Binds @b2Replay_GetDivergeFrame@.+-}+getDivergeFrame+  :: Ptr Replay+  -- ^ @player@+  -> IO Int+getDivergeFrame a0 =+  fromIntegral <$> (c_b2Replay_GetDivergeFrame a0)++{- | 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 @b2Replay_SetKeyframePolicy@.+-}+setKeyframePolicy+  :: Ptr Replay+  -- ^ @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.+  -> IO ()+setKeyframePolicy a0 a1 a2 =+  c_b2Replay_SetKeyframePolicy a0 a1 (fromIntegral a2)++{- | Get the keyframe memory budget in bytes.++Binds @b2Replay_GetKeyframeBudget@.+-}+getKeyframeBudget+  :: Ptr Replay+  -- ^ @player@+  -> IO CSize+getKeyframeBudget a0 =+  c_b2Replay_GetKeyframeBudget a0++{- | Get the finest keyframe spacing in frames.++Binds @b2Replay_GetKeyframeMinInterval@.+-}+getKeyframeMinInterval+  :: Ptr Replay+  -- ^ @player@+  -> IO Int+getKeyframeMinInterval a0 =+  fromIntegral <$> (c_b2Replay_GetKeyframeMinInterval a0)++{- | Get 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 right now.++Binds @b2Replay_GetKeyframeInterval@.+-}+getKeyframeInterval+  :: Ptr Replay+  -- ^ @player@+  -> IO Int+getKeyframeInterval a0 =+  fromIntegral <$> (c_b2Replay_GetKeyframeInterval a0)++{- | Get the memory currently held by keyframe snapshots, in bytes.++Binds @b2Replay_GetKeyframeBytes@.+-}+getKeyframeBytes+  :: Ptr Replay+  -- ^ @player@+  -> IO CSize+getKeyframeBytes a0 =+  c_b2Replay_GetKeyframeBytes a0++{- | Draw spatial queries recorded during the most recently replayed frame. Call after @b2World_Draw@+so queries are layered on top of the world.++Binds @b2Replay_DrawFrameQueries@.+-}+drawFrameQueries+  :: Ptr Replay+  -- ^ @player@: A valid player handle+  -> Ptr DebugDraw+  -- ^ @draw@: Debug draw callbacks. NULL draw function pointers are skipped.+  -> Int+  -- ^ @queryIndex@: Index into the frame\'s queries to draw, or -1 to draw all of them.+  -> IO ()+drawFrameQueries a0 a1 a2 =+  c_b2Replay_DrawFrameQueries a0 a1 (fromIntegral a2)++{- | Get the number of spatial queries recorded for the most recently replayed frame.++Binds @b2Replay_GetFrameQueryCount@.+-}+getFrameQueryCount+  :: Ptr Replay+  -- ^ @player@+  -> IO Int+getFrameQueryCount a0 =+  fromIntegral <$> (c_b2Replay_GetFrameQueryCount a0)++{- | Get a recorded query from the most recently replayed frame by index.++Binds @b2Replay_GetFrameQuery@.+-}+getFrameQuery+  :: Ptr Replay+  -- ^ @player@+  -> Int+  -- ^ @index@+  -> Ptr ReplayQueryInfo+  -- ^ Result buffer.+  -> IO ()+getFrameQuery a0 a1 out =+  c_b2Replay_GetFrameQuery a0 (fromIntegral a1) out++{- | Get one result of a recorded query from the most recently replayed frame.++Binds @b2Replay_GetFrameQueryHit@.+-}+getFrameQueryHit+  :: Ptr Replay+  -- ^ @player@+  -> Int+  -- ^ @queryIndex@+  -> Int+  -- ^ @hitIndex@+  -> Ptr ReplayQueryHit+  -- ^ Result buffer.+  -> IO ()+getFrameQueryHit a0 a1 a2 out =+  c_b2Replay_GetFrameQueryHit a0 (fromIntegral a1) (fromIntegral a2) out++{- | Get the number of body slots tracked for the outliner. This is the creation-order span and+includes holes for destroyed bodies, so it only grows as the replay advances.++Binds @b2Replay_GetBodyCount@.+-}+getBodyCount+  :: Ptr Replay+  -- ^ @player@+  -> IO Int+getBodyCount a0 =+  fromIntegral <$> (c_b2Replay_GetBodyCount a0)++{- | Get a tracked body by creation ordinal. Returns @b2_nullBodyId@ for a destroyed slot, an ordinal+not yet reached at the current frame, or an out-of-range index. Validate with @b2Body_IsValid@.++Binds @b2Replay_GetBodyId@.+-}+getBodyId+  :: Ptr Replay+  -- ^ @player@+  -> Int+  -- ^ @index@+  -> IO BodyId+getBodyId a0 a1 =+  c_b2Replay_GetBodyId a0 (fromIntegral a1)
+ src/Box2D/RevoluteJoint.hsc view
@@ -0,0 +1,437 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.RevoluteJoint+  ( RevoluteJointDef (..)+  , defaultRevoluteJointDef+  , create+  , enableSpring+  , isSpringEnabled+  , setSpringHertz+  , getSpringHertz+  , setSpringDampingRatio+  , getSpringDampingRatio+  , setTargetAngle+  , getTargetAngle+  , enableLimit+  , isLimitEnabled+  , getLowerLimit+  , getUpperLimit+  , setLimits+  , enableMotor+  , isMotorEnabled+  , setTargetMotorSpeed+  , getTargetMotorSpeed+  , setMaxMotorTorque+  , getMaxMotorTorque+  , getCurrentAngle+  , getCurrentSpeed+  , getCurrentMotorTorque+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (JointId (..), WorldId (..))+import Box2D.Joint (JointDef)++#include "box2d/box2d.h"+++{- | Revolute joint definition A point on body B is fixed to a point on body A. Allows relative+rotation.+-}+data RevoluteJointDef = RevoluteJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , targetAngle :: Float+  -- ^ The target angle for the joint in radians. The spring-damper will drive to this angle.+  , 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+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable RevoluteJointDef where+  sizeOf _ = #{size b2RevoluteJointDef}+  alignment _ = #{alignment b2RevoluteJointDef}+  peek p =+    RevoluteJointDef+      <$> #{peek b2RevoluteJointDef, base} p+      <*> #{peek b2RevoluteJointDef, targetAngle} p+      <*> #{peek b2RevoluteJointDef, enableSpring} p+      <*> #{peek b2RevoluteJointDef, hertz} p+      <*> #{peek b2RevoluteJointDef, dampingRatio} p+      <*> #{peek b2RevoluteJointDef, enableLimit} p+      <*> #{peek b2RevoluteJointDef, lowerAngle} p+      <*> #{peek b2RevoluteJointDef, upperAngle} p+      <*> #{peek b2RevoluteJointDef, enableMotor} p+      <*> #{peek b2RevoluteJointDef, maxMotorTorque} p+      <*> #{peek b2RevoluteJointDef, motorSpeed} p+      <*> #{peek b2RevoluteJointDef, internalValue} p+  poke p (RevoluteJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12) = do+    #{poke b2RevoluteJointDef, base} p f1+    #{poke b2RevoluteJointDef, targetAngle} p f2+    #{poke b2RevoluteJointDef, enableSpring} p f3+    #{poke b2RevoluteJointDef, hertz} p f4+    #{poke b2RevoluteJointDef, dampingRatio} p f5+    #{poke b2RevoluteJointDef, enableLimit} p f6+    #{poke b2RevoluteJointDef, lowerAngle} p f7+    #{poke b2RevoluteJointDef, upperAngle} p f8+    #{poke b2RevoluteJointDef, enableMotor} p f9+    #{poke b2RevoluteJointDef, maxMotorTorque} p f10+    #{poke b2RevoluteJointDef, motorSpeed} p f11+    #{poke b2RevoluteJointDef, internalValue} p f12++foreign import ccall unsafe "hs_b2DefaultRevoluteJointDef"+  c_b2DefaultRevoluteJointDef :: Ptr RevoluteJointDef -> IO ()++defaultRevoluteJointDef :: IO RevoluteJointDef+defaultRevoluteJointDef = allocaOut c_b2DefaultRevoluteJointDef+++foreign import ccall unsafe "b2CreateRevoluteJoint"+  c_b2CreateRevoluteJoint :: WorldId -> Ptr RevoluteJointDef -> IO JointId++foreign import ccall unsafe "b2RevoluteJoint_EnableSpring"+  c_b2RevoluteJoint_EnableSpring :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2RevoluteJoint_IsSpringEnabled"+  c_b2RevoluteJoint_IsSpringEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2RevoluteJoint_SetSpringHertz"+  c_b2RevoluteJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2RevoluteJoint_GetSpringHertz"+  c_b2RevoluteJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b2RevoluteJoint_SetSpringDampingRatio"+  c_b2RevoluteJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2RevoluteJoint_GetSpringDampingRatio"+  c_b2RevoluteJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b2RevoluteJoint_SetTargetAngle"+  c_b2RevoluteJoint_SetTargetAngle :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2RevoluteJoint_GetTargetAngle"+  c_b2RevoluteJoint_GetTargetAngle :: JointId -> IO Float++foreign import ccall unsafe "b2RevoluteJoint_EnableLimit"+  c_b2RevoluteJoint_EnableLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2RevoluteJoint_IsLimitEnabled"+  c_b2RevoluteJoint_IsLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2RevoluteJoint_GetLowerLimit"+  c_b2RevoluteJoint_GetLowerLimit :: JointId -> IO Float++foreign import ccall unsafe "b2RevoluteJoint_GetUpperLimit"+  c_b2RevoluteJoint_GetUpperLimit :: JointId -> IO Float++foreign import ccall unsafe "b2RevoluteJoint_SetLimits"+  c_b2RevoluteJoint_SetLimits :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b2RevoluteJoint_EnableMotor"+  c_b2RevoluteJoint_EnableMotor :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2RevoluteJoint_IsMotorEnabled"+  c_b2RevoluteJoint_IsMotorEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2RevoluteJoint_SetTargetMotorSpeed"+  c_b2RevoluteJoint_SetTargetMotorSpeed :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2RevoluteJoint_GetTargetMotorSpeed"+  c_b2RevoluteJoint_GetTargetMotorSpeed :: JointId -> IO Float++foreign import ccall unsafe "b2RevoluteJoint_SetMaxMotorTorque"+  c_b2RevoluteJoint_SetMaxMotorTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2RevoluteJoint_GetMaxMotorTorque"+  c_b2RevoluteJoint_GetMaxMotorTorque :: JointId -> IO Float++foreign import ccall unsafe "b2RevoluteJoint_GetCurrentAngle"+  c_b2RevoluteJoint_GetCurrentAngle :: JointId -> IO Float++foreign import ccall unsafe "b2RevoluteJoint_GetCurrentSpeed"+  c_b2RevoluteJoint_GetCurrentSpeed :: JointId -> IO Float++foreign import ccall unsafe "b2RevoluteJoint_GetCurrentMotorTorque"+  c_b2RevoluteJoint_GetCurrentMotorTorque :: JointId -> IO Float++{- | Create a revolute joint++See also @b2RevoluteJointDef@ for details.++Binds @b2CreateRevoluteJoint@.+-}+create+  :: WorldId+  -> RevoluteJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b2CreateRevoluteJoint a0 p1++{- | Enable\/disable the revolute joint spring++Binds @b2RevoluteJoint_EnableSpring@.+-}+enableSpring+  :: JointId+  -> Bool+  -- ^ @enableSpring@+  -> IO ()+enableSpring a0 a1 =+  c_b2RevoluteJoint_EnableSpring a0 (fromBool a1)++{- | Is the revolute angular spring enabled?++Binds @b2RevoluteJoint_IsSpringEnabled@.+-}+isSpringEnabled+  :: JointId+  -> IO Bool+isSpringEnabled a0 =+  toBool <$> (c_b2RevoluteJoint_IsSpringEnabled a0)++{- | Set the revolute joint spring stiffness in Hertz++Binds @b2RevoluteJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b2RevoluteJoint_SetSpringHertz a0 a1++{- | Get the revolute joint spring stiffness in Hertz++Binds @b2RevoluteJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b2RevoluteJoint_GetSpringHertz a0++{- | Set the revolute joint spring damping ratio, non-dimensional++Binds @b2RevoluteJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b2RevoluteJoint_SetSpringDampingRatio a0 a1++{- | Get the revolute joint spring damping ratio, non-dimensional++Binds @b2RevoluteJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b2RevoluteJoint_GetSpringDampingRatio a0++{- | Set the revolute joint spring target angle, radians++Binds @b2RevoluteJoint_SetTargetAngle@.+-}+setTargetAngle+  :: JointId+  -> Float+  -- ^ @angle@+  -> IO ()+setTargetAngle a0 a1 =+  c_b2RevoluteJoint_SetTargetAngle a0 a1++{- | Get the revolute joint spring target angle, radians++Binds @b2RevoluteJoint_GetTargetAngle@.+-}+getTargetAngle+  :: JointId+  -> IO Float+getTargetAngle a0 =+  c_b2RevoluteJoint_GetTargetAngle a0++{- | Enable\/disable the revolute joint limit++Binds @b2RevoluteJoint_EnableLimit@.+-}+enableLimit+  :: JointId+  -> Bool+  -- ^ @enableLimit@+  -> IO ()+enableLimit a0 a1 =+  c_b2RevoluteJoint_EnableLimit a0 (fromBool a1)++{- | Is the revolute joint limit enabled?++Binds @b2RevoluteJoint_IsLimitEnabled@.+-}+isLimitEnabled+  :: JointId+  -> IO Bool+isLimitEnabled a0 =+  toBool <$> (c_b2RevoluteJoint_IsLimitEnabled a0)++{- | Get the revolute joint lower limit in radians++Binds @b2RevoluteJoint_GetLowerLimit@.+-}+getLowerLimit+  :: JointId+  -> IO Float+getLowerLimit a0 =+  c_b2RevoluteJoint_GetLowerLimit a0++{- | Get the revolute joint upper limit in radians++Binds @b2RevoluteJoint_GetUpperLimit@.+-}+getUpperLimit+  :: JointId+  -> IO Float+getUpperLimit a0 =+  c_b2RevoluteJoint_GetUpperLimit a0++{- | Set the revolute joint limits in radians. It is expected that lower \<= upper and that -0.99 *+@B2_PI@ \<= lower && upper \<= 0.99 * @B2_PI@.++Binds @b2RevoluteJoint_SetLimits@.+-}+setLimits+  :: JointId+  -> Float+  -- ^ @lower@+  -> Float+  -- ^ @upper@+  -> IO ()+setLimits a0 a1 a2 =+  c_b2RevoluteJoint_SetLimits a0 a1 a2++{- | Enable\/disable a revolute joint motor++Binds @b2RevoluteJoint_EnableMotor@.+-}+enableMotor+  :: JointId+  -> Bool+  -- ^ @enableMotor@+  -> IO ()+enableMotor a0 a1 =+  c_b2RevoluteJoint_EnableMotor a0 (fromBool a1)++{- | Is the revolute joint motor enabled?++Binds @b2RevoluteJoint_IsMotorEnabled@.+-}+isMotorEnabled+  :: JointId+  -> IO Bool+isMotorEnabled a0 =+  toBool <$> (c_b2RevoluteJoint_IsMotorEnabled a0)++{- | Set the revolute joint motor speed in radians per second++Binds @b2RevoluteJoint_SetTargetMotorSpeed@.+-}+setTargetMotorSpeed+  :: JointId+  -> Float+  -- ^ @motorSpeed@+  -> IO ()+setTargetMotorSpeed a0 a1 =+  c_b2RevoluteJoint_SetTargetMotorSpeed a0 a1++{- | Get the revolute joint motor speed in radians per second++Binds @b2RevoluteJoint_GetTargetMotorSpeed@.+-}+getTargetMotorSpeed+  :: JointId+  -> IO Float+getTargetMotorSpeed a0 =+  c_b2RevoluteJoint_GetTargetMotorSpeed a0++{- | Set the revolute joint maximum motor torque, usually in newton-meters++Binds @b2RevoluteJoint_SetMaxMotorTorque@.+-}+setMaxMotorTorque+  :: JointId+  -> Float+  -- ^ @torque@+  -> IO ()+setMaxMotorTorque a0 a1 =+  c_b2RevoluteJoint_SetMaxMotorTorque a0 a1++{- | Get the revolute joint maximum motor torque, usually in newton-meters++Binds @b2RevoluteJoint_GetMaxMotorTorque@.+-}+getMaxMotorTorque+  :: JointId+  -> IO Float+getMaxMotorTorque a0 =+  c_b2RevoluteJoint_GetMaxMotorTorque a0++{- | Get the revolute joint current angle in radians. This is the relative rotation of the joint+frames.++Binds @b2RevoluteJoint_GetCurrentAngle@.+-}+getCurrentAngle+  :: JointId+  -> IO Float+getCurrentAngle a0 =+  c_b2RevoluteJoint_GetCurrentAngle a0++{- | Get the current revolute joint motor speed in radians per second++Binds @b2RevoluteJoint_GetCurrentSpeed@.+-}+getCurrentSpeed+  :: JointId+  -> IO Float+getCurrentSpeed a0 =+  c_b2RevoluteJoint_GetCurrentSpeed a0++{- | Get the revolute joint current motor torque, usually in newton-meters++Binds @b2RevoluteJoint_GetCurrentMotorTorque@.+-}+getCurrentMotorTorque+  :: JointId+  -> IO Float+getCurrentMotorTorque a0 =+  c_b2RevoluteJoint_GetCurrentMotorTorque a0
+ src/Box2D/Shape.hsc view
@@ -0,0 +1,1266 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.Shape+  ( ShapeDef (..)+  , SurfaceMaterial (..)+  , ShapeType (..)+  , pattern CircleShape+  , pattern CapsuleShape+  , pattern SegmentShape+  , pattern PolygonShape+  , pattern ChainSegmentShape+  , QueryFilter (..)+  , Filter (..)+  , defaultFilter+  , defaultQueryFilter+  , defaultSurfaceMaterial+  , defaultShapeDef+  , createCircle+  , createSegment+  , createChainSegment+  , createCapsule+  , createPolygon+  , destroy+  , isValid+  , getType+  , getBody+  , getWorld+  , isSensor+  , setUserData+  , getUserData+  , setDensity+  , getDensity+  , setFriction+  , getFriction+  , setRestitution+  , getRestitution+  , setUserMaterial+  , getUserMaterial+  , setSurfaceMaterial+  , getSurfaceMaterial+  , getSurfaceMaterialInto+  , getFilter+  , getFilterInto+  , setFilter+  , enableSensorEvents+  , areSensorEventsEnabled+  , enableContactEvents+  , areContactEventsEnabled+  , enablePreSolveEvents+  , arePreSolveEventsEnabled+  , enableHitEvents+  , areHitEventsEnabled+  , testPoint+  , castRay+  , getCircle+  , getCircleInto+  , getSegment+  , getSegmentInto+  , getChainSegment+  , getCapsule+  , getCapsuleInto+  , getPolygon+  , getPolygonInto+  , setCircle+  , setCapsule+  , setSegment+  , setPolygon+  , setChainSegment+  , getParentChain+  , getContactCapacity+  , getContactData+  , getSensorCapacity+  , getSensorData+  , getBounds+  , getBoundsInto+  , computeMassData+  , computeMassDataInto+  , getClosestPoint+  , applyWind+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Data.Coerce (coerce)+import Box2D.Internal (allocaOut)+import Box2D.Contact (ContactData)+import Box2D.Geometry (Capsule, ChainSegment, Circle, MassData, Polygon, Segment, WorldCastOutput)+import Box2D.Id (BodyId (..), ChainId (..), ShapeId (..), WorldId (..))+import Box2D.MathTypes (AABB, Pos, Vec2)++#include "box2d/box2d.h"+++{- | Used to create a shape. This is a temporary object used to bundle shape creation parameters. You+may use the same shape definition to create multiple shapes. Must be initialized using+@b2DefaultShapeDef()@.+-}+data ShapeDef = ShapeDef+  { userData :: Ptr ()+  -- ^ Use this to store application specific shape data.+  , material :: SurfaceMaterial+  -- ^ The surface material for this shape.+  , density :: Float+  -- ^ The density, usually in kg\/m^2. This is not part of the surface material because this is for+  -- the interior, which may have other considerations, such as being hollow. For example a wood+  -- barrel may be hollow or full of water.+  , filter :: Filter+  -- ^ Collision filtering data.+  , enableCustomFiltering :: CBool+  -- ^ Enable custom filtering. Only one of the two shapes needs to enable custom filtering. See+  -- @b2World_SetCustomFilterCallback@.+  , isSensor :: CBool+  -- ^ A sensor shape generates overlap events but never generates a collision response. Fast+  -- moving sensors may miss overlaps. 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. Both shapes+  -- involved must have this flag set to true. False by default, even for sensors.+  , enableContactEvents :: CBool+  -- ^ Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Only one+  -- shape involved needs this flag set to true. Ignored for sensors. False by default.+  , enableHitEvents :: CBool+  -- ^ Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Only one+  -- shape involved needs this flag set to true. 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 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 @b2Body_UpdateMassFromShapes@ or @b2Body_SetMassData@+  -- before simulating the world.+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable ShapeDef where+  sizeOf _ = #{size b2ShapeDef}+  alignment _ = #{alignment b2ShapeDef}+  peek p =+    ShapeDef+      <$> #{peek b2ShapeDef, userData} p+      <*> #{peek b2ShapeDef, material} p+      <*> #{peek b2ShapeDef, density} p+      <*> #{peek b2ShapeDef, filter} p+      <*> #{peek b2ShapeDef, enableCustomFiltering} p+      <*> #{peek b2ShapeDef, isSensor} p+      <*> #{peek b2ShapeDef, enableSensorEvents} p+      <*> #{peek b2ShapeDef, enableContactEvents} p+      <*> #{peek b2ShapeDef, enableHitEvents} p+      <*> #{peek b2ShapeDef, enablePreSolveEvents} p+      <*> #{peek b2ShapeDef, invokeContactCreation} p+      <*> #{peek b2ShapeDef, updateBodyMass} p+      <*> #{peek b2ShapeDef, internalValue} p+  poke p (ShapeDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11 f12 f13) = do+    #{poke b2ShapeDef, userData} p f1+    #{poke b2ShapeDef, material} p f2+    #{poke b2ShapeDef, density} p f3+    #{poke b2ShapeDef, filter} p f4+    #{poke b2ShapeDef, enableCustomFiltering} p f5+    #{poke b2ShapeDef, isSensor} p f6+    #{poke b2ShapeDef, enableSensorEvents} p f7+    #{poke b2ShapeDef, enableContactEvents} p f8+    #{poke b2ShapeDef, enableHitEvents} p f9+    #{poke b2ShapeDef, enablePreSolveEvents} p f10+    #{poke b2ShapeDef, invokeContactCreation} p f11+    #{poke b2ShapeDef, updateBodyMass} p f12+    #{poke b2ShapeDef, internalValue} p f13++{- | The surface material groups together parameters related to the shape surface, which is the 2D+boundary. Surface materials allow chain shapes to have per segment properties.+-}+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\].+  , tangentSpeed :: Float+  -- ^ The tangent speed for conveyor belts+  , 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.+  }+  deriving (Eq, Show)++instance Storable SurfaceMaterial where+  sizeOf _ = #{size b2SurfaceMaterial}+  alignment _ = #{alignment b2SurfaceMaterial}+  peek p =+    SurfaceMaterial+      <$> #{peek b2SurfaceMaterial, friction} p+      <*> #{peek b2SurfaceMaterial, restitution} p+      <*> #{peek b2SurfaceMaterial, rollingResistance} p+      <*> #{peek b2SurfaceMaterial, tangentSpeed} p+      <*> #{peek b2SurfaceMaterial, userMaterialId} p+      <*> #{peek b2SurfaceMaterial, customColor} p+  poke p (SurfaceMaterial f1 f2 f3 f4 f5 f6) = do+    #{poke b2SurfaceMaterial, friction} p f1+    #{poke b2SurfaceMaterial, restitution} p f2+    #{poke b2SurfaceMaterial, rollingResistance} p f3+    #{poke b2SurfaceMaterial, tangentSpeed} p f4+    #{poke b2SurfaceMaterial, userMaterialId} p f5+    #{poke b2SurfaceMaterial, customColor} p f6++-- | Shape type+newtype ShapeType = ShapeType CInt+  deriving (Eq, Show, Storable)++-- | A circle with an offset+pattern CircleShape :: ShapeType+-- | A capsule is an extruded circle+pattern CapsuleShape :: ShapeType+-- | A line segment+pattern SegmentShape :: ShapeType+-- | A convex polygon+pattern PolygonShape :: ShapeType+-- | A line segment owned by a chain shape+pattern ChainSegmentShape :: ShapeType+pattern CircleShape = ShapeType #{const b2_circleShape}+pattern CapsuleShape = ShapeType #{const b2_capsuleShape}+pattern SegmentShape = ShapeType #{const b2_segmentShape}+pattern PolygonShape = ShapeType #{const b2_polygonShape}+pattern ChainSegmentShape = ShapeType #{const b2_chainSegmentShape}++{- | 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.+  }+  deriving (Eq, Show)++instance Storable QueryFilter where+  sizeOf _ = #{size b2QueryFilter}+  alignment _ = #{alignment b2QueryFilter}+  peek p =+    QueryFilter+      <$> #{peek b2QueryFilter, categoryBits} p+      <*> #{peek b2QueryFilter, maskBits} p+  poke p (QueryFilter f1 f2) = do+    #{poke b2QueryFilter, categoryBits} p f1+    #{poke b2QueryFilter, maskBits} p f2++{- | This is used to filter collision on shapes. It affects shape-vs-shape collision and+shape-versus-query collision (such as @b2World_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 b2Filter}+  alignment _ = #{alignment b2Filter}+  peek p =+    Filter+      <$> #{peek b2Filter, categoryBits} p+      <*> #{peek b2Filter, maskBits} p+      <*> #{peek b2Filter, groupIndex} p+  poke p (Filter f1 f2 f3) = do+    #{poke b2Filter, categoryBits} p f1+    #{poke b2Filter, maskBits} p f2+    #{poke b2Filter, groupIndex} p f3++foreign import ccall unsafe "hs_b2DefaultFilter"+  c_b2DefaultFilter :: Ptr Filter -> IO ()++defaultFilter :: IO Filter+defaultFilter = allocaOut c_b2DefaultFilter++foreign import ccall unsafe "hs_b2DefaultQueryFilter"+  c_b2DefaultQueryFilter :: Ptr QueryFilter -> IO ()++defaultQueryFilter :: IO QueryFilter+defaultQueryFilter = allocaOut c_b2DefaultQueryFilter++foreign import ccall unsafe "hs_b2DefaultSurfaceMaterial"+  c_b2DefaultSurfaceMaterial :: Ptr SurfaceMaterial -> IO ()++defaultSurfaceMaterial :: IO SurfaceMaterial+defaultSurfaceMaterial = allocaOut c_b2DefaultSurfaceMaterial++foreign import ccall unsafe "hs_b2DefaultShapeDef"+  c_b2DefaultShapeDef :: Ptr ShapeDef -> IO ()++defaultShapeDef :: IO ShapeDef+defaultShapeDef = allocaOut c_b2DefaultShapeDef+++foreign import ccall unsafe "b2CreateCircleShape"+  c_b2CreateCircleShape :: BodyId -> Ptr ShapeDef -> Ptr Circle -> IO ShapeId++foreign import ccall unsafe "b2CreateSegmentShape"+  c_b2CreateSegmentShape :: BodyId -> Ptr ShapeDef -> Ptr Segment -> IO ShapeId++foreign import ccall unsafe "b2CreateChainSegmentShape"+  c_b2CreateChainSegmentShape :: BodyId -> Ptr ShapeDef -> Ptr ChainSegment -> IO ShapeId++foreign import ccall unsafe "b2CreateCapsuleShape"+  c_b2CreateCapsuleShape :: BodyId -> Ptr ShapeDef -> Ptr Capsule -> IO ShapeId++foreign import ccall unsafe "b2CreatePolygonShape"+  c_b2CreatePolygonShape :: BodyId -> Ptr ShapeDef -> Ptr Polygon -> IO ShapeId++foreign import ccall unsafe "b2DestroyShape"+  c_b2DestroyShape :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b2Shape_IsValid"+  c_b2Shape_IsValid :: ShapeId -> IO CBool++foreign import ccall unsafe "b2Shape_GetType"+  c_b2Shape_GetType :: ShapeId -> IO CInt++foreign import ccall unsafe "b2Shape_GetBody"+  c_b2Shape_GetBody :: ShapeId -> IO BodyId++foreign import ccall unsafe "b2Shape_GetWorld"+  c_b2Shape_GetWorld :: ShapeId -> IO WorldId++foreign import ccall unsafe "b2Shape_IsSensor"+  c_b2Shape_IsSensor :: ShapeId -> IO CBool++foreign import ccall unsafe "b2Shape_SetUserData"+  c_b2Shape_SetUserData :: ShapeId -> Ptr () -> IO ()++foreign import ccall unsafe "b2Shape_GetUserData"+  c_b2Shape_GetUserData :: ShapeId -> IO (Ptr ())++foreign import ccall unsafe "b2Shape_SetDensity"+  c_b2Shape_SetDensity :: ShapeId -> Float -> CBool -> IO ()++foreign import ccall unsafe "b2Shape_GetDensity"+  c_b2Shape_GetDensity :: ShapeId -> IO Float++foreign import ccall unsafe "b2Shape_SetFriction"+  c_b2Shape_SetFriction :: ShapeId -> Float -> IO ()++foreign import ccall unsafe "b2Shape_GetFriction"+  c_b2Shape_GetFriction :: ShapeId -> IO Float++foreign import ccall unsafe "b2Shape_SetRestitution"+  c_b2Shape_SetRestitution :: ShapeId -> Float -> IO ()++foreign import ccall unsafe "b2Shape_GetRestitution"+  c_b2Shape_GetRestitution :: ShapeId -> IO Float++foreign import ccall unsafe "b2Shape_SetUserMaterial"+  c_b2Shape_SetUserMaterial :: ShapeId -> Word64 -> IO ()++foreign import ccall unsafe "b2Shape_GetUserMaterial"+  c_b2Shape_GetUserMaterial :: ShapeId -> IO Word64++foreign import ccall unsafe "b2Shape_SetSurfaceMaterial"+  c_b2Shape_SetSurfaceMaterial :: ShapeId -> Ptr SurfaceMaterial -> IO ()++foreign import ccall unsafe "hsg_b2Shape_GetSurfaceMaterial"+  c_b2Shape_GetSurfaceMaterial :: ShapeId -> Ptr SurfaceMaterial -> IO ()++foreign import ccall unsafe "hsg_b2Shape_GetFilter"+  c_b2Shape_GetFilter :: ShapeId -> Ptr Filter -> IO ()++foreign import ccall unsafe "hsg_b2Shape_SetFilter"+  c_b2Shape_SetFilter :: ShapeId -> Ptr Filter -> IO ()++foreign import ccall unsafe "b2Shape_EnableSensorEvents"+  c_b2Shape_EnableSensorEvents :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b2Shape_AreSensorEventsEnabled"+  c_b2Shape_AreSensorEventsEnabled :: ShapeId -> IO CBool++foreign import ccall unsafe "b2Shape_EnableContactEvents"+  c_b2Shape_EnableContactEvents :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b2Shape_AreContactEventsEnabled"+  c_b2Shape_AreContactEventsEnabled :: ShapeId -> IO CBool++foreign import ccall unsafe "b2Shape_EnablePreSolveEvents"+  c_b2Shape_EnablePreSolveEvents :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b2Shape_ArePreSolveEventsEnabled"+  c_b2Shape_ArePreSolveEventsEnabled :: ShapeId -> IO CBool++foreign import ccall unsafe "b2Shape_EnableHitEvents"+  c_b2Shape_EnableHitEvents :: ShapeId -> CBool -> IO ()++foreign import ccall unsafe "b2Shape_AreHitEventsEnabled"+  c_b2Shape_AreHitEventsEnabled :: ShapeId -> IO CBool++foreign import ccall unsafe "hsg_b2Shape_TestPoint"+  c_b2Shape_TestPoint :: ShapeId -> Ptr Pos -> IO CBool++foreign import ccall unsafe "hsg_b2Shape_CastRay"+  c_b2Shape_CastRay :: ShapeId -> Ptr Pos -> Ptr Vec2 -> Ptr WorldCastOutput -> IO ()++foreign import ccall unsafe "hsg_b2Shape_GetCircle"+  c_b2Shape_GetCircle :: ShapeId -> Ptr Circle -> IO ()++foreign import ccall unsafe "hsg_b2Shape_GetSegment"+  c_b2Shape_GetSegment :: ShapeId -> Ptr Segment -> IO ()++foreign import ccall unsafe "hsg_b2Shape_GetChainSegment"+  c_b2Shape_GetChainSegment :: ShapeId -> Ptr ChainSegment -> IO ()++foreign import ccall unsafe "hsg_b2Shape_GetCapsule"+  c_b2Shape_GetCapsule :: ShapeId -> Ptr Capsule -> IO ()++foreign import ccall unsafe "hsg_b2Shape_GetPolygon"+  c_b2Shape_GetPolygon :: ShapeId -> Ptr Polygon -> IO ()++foreign import ccall unsafe "b2Shape_SetCircle"+  c_b2Shape_SetCircle :: ShapeId -> Ptr Circle -> IO ()++foreign import ccall unsafe "b2Shape_SetCapsule"+  c_b2Shape_SetCapsule :: ShapeId -> Ptr Capsule -> IO ()++foreign import ccall unsafe "b2Shape_SetSegment"+  c_b2Shape_SetSegment :: ShapeId -> Ptr Segment -> IO ()++foreign import ccall unsafe "b2Shape_SetPolygon"+  c_b2Shape_SetPolygon :: ShapeId -> Ptr Polygon -> IO ()++foreign import ccall unsafe "b2Shape_SetChainSegment"+  c_b2Shape_SetChainSegment :: ShapeId -> Ptr ChainSegment -> IO ()++foreign import ccall unsafe "b2Shape_GetParentChain"+  c_b2Shape_GetParentChain :: ShapeId -> IO ChainId++foreign import ccall unsafe "b2Shape_GetContactCapacity"+  c_b2Shape_GetContactCapacity :: ShapeId -> IO CInt++foreign import ccall unsafe "b2Shape_GetContactData"+  c_b2Shape_GetContactData :: ShapeId -> Ptr ContactData -> CInt -> IO CInt++foreign import ccall unsafe "b2Shape_GetSensorCapacity"+  c_b2Shape_GetSensorCapacity :: ShapeId -> IO CInt++foreign import ccall unsafe "b2Shape_GetSensorData"+  c_b2Shape_GetSensorData :: ShapeId -> Ptr ShapeId -> CInt -> IO CInt++foreign import ccall unsafe "hsg_b2Shape_GetBounds"+  c_b2Shape_GetBounds :: ShapeId -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b2Shape_ComputeMassData"+  c_b2Shape_ComputeMassData :: ShapeId -> Ptr MassData -> IO ()++foreign import ccall unsafe "hsg_b2Shape_GetClosestPoint"+  c_b2Shape_GetClosestPoint :: ShapeId -> Ptr Pos -> Ptr Pos -> IO ()++foreign import ccall unsafe "hsg_b2Shape_ApplyWind"+  c_b2Shape_ApplyWind :: ShapeId -> Ptr Vec2 -> Float -> Float -> CBool -> IO ()++{- | 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 @b2CreateCircleShape@.+-}+createCircle+  :: BodyId+  -> ShapeDef+  -> Circle+  -> IO ShapeId+createCircle a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CreateCircleShape a0 p1 p2++{- | Create a line segment 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 or @b2_nullShapeId@ if the segment is too short.++Binds @b2CreateSegmentShape@.+-}+createSegment+  :: BodyId+  -> ShapeDef+  -> Segment+  -> IO ShapeId+createSegment a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CreateSegmentShape a0 p1 p2++{- | Create a chain segment shape and attach it to a body. The shape definition and geometry are+fully cloned. The segment is not owned by any chain shape (@b2Shape_GetParentChain@ returns+@b2_nullChainId@). Contacts are not created until the next time step. The chainId on the segment+is ignored. This uses more memory than using a chain shape, so I don\'t recommend using this.++Returns: the shape id, or @b2_nullShapeId@ if the segment is too short.++Binds @b2CreateChainSegmentShape@.+-}+createChainSegment+  :: BodyId+  -> ShapeDef+  -> Ptr ChainSegment+  -> IO ShapeId+createChainSegment a0 a1 a2 =+  with a1 $ \p1 ->+  c_b2CreateChainSegmentShape a0 p1 a2++{- | 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, this will be @b2_nullShapeId@ if the length is too small.++Binds @b2CreateCapsuleShape@.+-}+createCapsule+  :: BodyId+  -> ShapeDef+  -> Capsule+  -> IO ShapeId+createCapsule a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CreateCapsuleShape a0 p1 p2++{- | Create a polygon 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 @b2CreatePolygonShape@.+-}+createPolygon+  :: BodyId+  -> ShapeDef+  -> Polygon+  -> IO ShapeId+createPolygon a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  c_b2CreatePolygonShape a0 p1 p2++{- | 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 @b2Body_UpdateMassFromShapes@.++Binds @b2DestroyShape@.+-}+destroy+  :: ShapeId+  -> Bool+  -- ^ @updateBodyMass@+  -> IO ()+destroy a0 a1 =+  c_b2DestroyShape a0 (fromBool a1)++{- | Shape identifier validation. Provides validation for up to 64K allocations.++Binds @b2Shape_IsValid@.+-}+isValid+  :: ShapeId+  -> IO Bool+isValid a0 =+  toBool <$> (c_b2Shape_IsValid a0)++{- | Get the type of a shape++Binds @b2Shape_GetType@.+-}+getType+  :: ShapeId+  -> IO ShapeType+getType a0 =+  coerce <$> (c_b2Shape_GetType a0)++{- | Get the id of the body that a shape is attached to++Binds @b2Shape_GetBody@.+-}+getBody+  :: ShapeId+  -> IO BodyId+getBody a0 =+  c_b2Shape_GetBody a0++{- | Get the world that owns this shape++Binds @b2Shape_GetWorld@.+-}+getWorld+  :: ShapeId+  -> IO WorldId+getWorld a0 =+  c_b2Shape_GetWorld a0++{- | Returns true if the shape is a sensor. It is not possible to change a shape from sensor to solid+dynamically because this breaks the contract for sensor events.++Binds @b2Shape_IsSensor@.+-}+isSensor+  :: ShapeId+  -> IO Bool+isSensor a0 =+  toBool <$> (c_b2Shape_IsSensor a0)++{- | Set the user data for a shape++Binds @b2Shape_SetUserData@.+-}+setUserData+  :: ShapeId+  -> Ptr ()+  -- ^ @userData@+  -> IO ()+setUserData a0 a1 =+  c_b2Shape_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 @b2Shape_GetUserData@.+-}+getUserData+  :: ShapeId+  -> IO (Ptr ())+getUserData a0 =+  c_b2Shape_GetUserData a0++{- | Set the mass density of a shape, usually in kg\/m^2. This will optionally update the mass+properties on the parent body.++See also @b2ShapeDef::density@, @b2Body_UpdateMassFromShapes@.++Binds @b2Shape_SetDensity@.+-}+setDensity+  :: ShapeId+  -> Float+  -- ^ @density@+  -> Bool+  -- ^ @updateBodyMass@+  -> IO ()+setDensity a0 a1 a2 =+  c_b2Shape_SetDensity a0 a1 (fromBool a2)++{- | Get the density of a shape, usually in kg\/m^2++Binds @b2Shape_GetDensity@.+-}+getDensity+  :: ShapeId+  -> IO Float+getDensity a0 =+  c_b2Shape_GetDensity a0++{- | Set the friction on a shape++Binds @b2Shape_SetFriction@.+-}+setFriction+  :: ShapeId+  -> Float+  -- ^ @friction@+  -> IO ()+setFriction a0 a1 =+  c_b2Shape_SetFriction a0 a1++{- | Get the friction of a shape++Binds @b2Shape_GetFriction@.+-}+getFriction+  :: ShapeId+  -> IO Float+getFriction a0 =+  c_b2Shape_GetFriction a0++{- | Set the shape restitution (bounciness)++Binds @b2Shape_SetRestitution@.+-}+setRestitution+  :: ShapeId+  -> Float+  -- ^ @restitution@+  -> IO ()+setRestitution a0 a1 =+  c_b2Shape_SetRestitution a0 a1++{- | Get the shape restitution++Binds @b2Shape_GetRestitution@.+-}+getRestitution+  :: ShapeId+  -> IO Float+getRestitution a0 =+  c_b2Shape_GetRestitution a0++{- | Set the user material identifier++Binds @b2Shape_SetUserMaterial@.+-}+setUserMaterial+  :: ShapeId+  -> Word64+  -- ^ @material@+  -> IO ()+setUserMaterial a0 a1 =+  c_b2Shape_SetUserMaterial a0 a1++{- | Get the user material identifier++Binds @b2Shape_GetUserMaterial@.+-}+getUserMaterial+  :: ShapeId+  -> IO Word64+getUserMaterial a0 =+  c_b2Shape_GetUserMaterial a0++{- | Set the shape surface material++Binds @b2Shape_SetSurfaceMaterial@.+-}+setSurfaceMaterial+  :: ShapeId+  -> SurfaceMaterial+  -> IO ()+setSurfaceMaterial a0 a1 =+  with a1 $ \p1 ->+  c_b2Shape_SetSurfaceMaterial a0 p1++{- | Get the shape surface material++Binds @b2Shape_GetSurfaceMaterial@.+-}+getSurfaceMaterial+  :: ShapeId+  -> IO SurfaceMaterial+getSurfaceMaterial a0 =+  alloca $ \pOut -> c_b2Shape_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_b2Shape_GetSurfaceMaterial a0 out++{- | Get the shape filter++Binds @b2Shape_GetFilter@.+-}+getFilter+  :: ShapeId+  -> IO Filter+getFilter a0 =+  alloca $ \pOut -> c_b2Shape_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_b2Shape_GetFilter a0 out++{- | Set the current filter. This is almost as expensive as recreating the shape. This may cause+contacts to be immediately destroyed. However contacts are not created until the next world+step. Sensor overlap state is also not updated until the next world step.++See also @b2ShapeDef::filter@.++Binds @b2Shape_SetFilter@.+-}+setFilter+  :: ShapeId+  -> Filter+  -> IO ()+setFilter a0 a1 =+  with a1 $ \p1 ->+  c_b2Shape_SetFilter a0 p1++{- | Enable sensor events for this shape.++See also @b2ShapeDef::enableSensorEvents@.++Binds @b2Shape_EnableSensorEvents@.+-}+enableSensorEvents+  :: ShapeId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSensorEvents a0 a1 =+  c_b2Shape_EnableSensorEvents a0 (fromBool a1)++{- | Returns true if sensor events are enabled.++Binds @b2Shape_AreSensorEventsEnabled@.+-}+areSensorEventsEnabled+  :: ShapeId+  -> IO Bool+areSensorEventsEnabled a0 =+  toBool <$> (c_b2Shape_AreSensorEventsEnabled a0)++{- | Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for+sensors.++See also @b2ShapeDef::enableContactEvents@.++__Warning:__ changing this at run-time may lead to lost begin\/end events++Binds @b2Shape_EnableContactEvents@.+-}+enableContactEvents+  :: ShapeId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableContactEvents a0 a1 =+  c_b2Shape_EnableContactEvents a0 (fromBool a1)++{- | Returns true if contact events are enabled++Binds @b2Shape_AreContactEventsEnabled@.+-}+areContactEventsEnabled+  :: ShapeId+  -> IO Bool+areContactEventsEnabled a0 =+  toBool <$> (c_b2Shape_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 @b2PreSolveCallbackFcn@.++Binds @b2Shape_EnablePreSolveEvents@.+-}+enablePreSolveEvents+  :: ShapeId+  -> Bool+  -- ^ @flag@+  -> IO ()+enablePreSolveEvents a0 a1 =+  c_b2Shape_EnablePreSolveEvents a0 (fromBool a1)++{- | Returns true if pre-solve events are enabled++Binds @b2Shape_ArePreSolveEventsEnabled@.+-}+arePreSolveEventsEnabled+  :: ShapeId+  -> IO Bool+arePreSolveEventsEnabled a0 =+  toBool <$> (c_b2Shape_ArePreSolveEventsEnabled a0)++{- | Enable contact hit events for this shape. Ignored for sensors.++See also @b2WorldDef::hitEventThreshold@.++Binds @b2Shape_EnableHitEvents@.+-}+enableHitEvents+  :: ShapeId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableHitEvents a0 a1 =+  c_b2Shape_EnableHitEvents a0 (fromBool a1)++{- | Returns true if hit events are enabled++Binds @b2Shape_AreHitEventsEnabled@.+-}+areHitEventsEnabled+  :: ShapeId+  -> IO Bool+areHitEventsEnabled a0 =+  toBool <$> (c_b2Shape_AreHitEventsEnabled a0)++{- | Test a world point for overlap with a shape++Binds @b2Shape_TestPoint@.+-}+testPoint+  :: ShapeId+  -> Pos+  -- ^ @point@+  -> IO Bool+testPoint a0 a1 =+  with a1 $ \p1 ->+  toBool <$> (c_b2Shape_TestPoint a0 p1)++{- | Ray cast a single shape. The ray starts at the origin and extends by the translation. The output+point is a world position.++Binds @b2Shape_CastRay@.+-}+castRay+  :: ShapeId+  -> Pos+  -- ^ @origin@+  -> Vec2+  -- ^ @translation@+  -> IO WorldCastOutput+castRay a0 a1 a2 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  alloca $ \pOut -> c_b2Shape_CastRay a0 p1 p2 pOut >> peek pOut++{- | Get a copy of the shape\'s circle. Asserts the type is correct.++Binds @b2Shape_GetCircle@.+-}+getCircle+  :: ShapeId+  -> IO Circle+getCircle a0 =+  alloca $ \pOut -> c_b2Shape_GetCircle a0 pOut >> peek pOut++-- | Like 'getCircle', but the result is written into a caller-supplied buffer.+getCircleInto+  :: ShapeId+  -> Ptr Circle+  -- ^ Result buffer.+  -> IO ()+getCircleInto a0 out =+  c_b2Shape_GetCircle a0 out++{- | Get a copy of the shape\'s line segment. Asserts the type is correct.++Binds @b2Shape_GetSegment@.+-}+getSegment+  :: ShapeId+  -> IO Segment+getSegment a0 =+  alloca $ \pOut -> c_b2Shape_GetSegment a0 pOut >> peek pOut++-- | Like 'getSegment', but the result is written into a caller-supplied buffer.+getSegmentInto+  :: ShapeId+  -> Ptr Segment+  -- ^ Result buffer.+  -> IO ()+getSegmentInto a0 out =+  c_b2Shape_GetSegment a0 out++{- | Get a copy of the shape\'s chain segment. These come from chain shapes. Asserts the type is+correct.++Binds @b2Shape_GetChainSegment@.+-}+getChainSegment+  :: ShapeId+  -> Ptr ChainSegment+  -- ^ Result buffer.+  -> IO ()+getChainSegment a0 out =+  c_b2Shape_GetChainSegment a0 out++{- | Get a copy of the shape\'s capsule. Asserts the type is correct.++Binds @b2Shape_GetCapsule@.+-}+getCapsule+  :: ShapeId+  -> IO Capsule+getCapsule a0 =+  alloca $ \pOut -> c_b2Shape_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_b2Shape_GetCapsule a0 out++{- | Get a copy of the shape\'s convex polygon. Asserts the type is correct.++Binds @b2Shape_GetPolygon@.+-}+getPolygon+  :: ShapeId+  -> IO Polygon+getPolygon a0 =+  alloca $ \pOut -> c_b2Shape_GetPolygon a0 pOut >> peek pOut++-- | Like 'getPolygon', but the result is written into a caller-supplied buffer.+getPolygonInto+  :: ShapeId+  -> Ptr Polygon+  -- ^ Result buffer.+  -> IO ()+getPolygonInto a0 out =+  c_b2Shape_GetPolygon a0 out++{- | Allows you to change a shape to be a circle or update the current circle. This does not modify+the mass properties.++See also @b2Body_UpdateMassFromShapes@.++Binds @b2Shape_SetCircle@.+-}+setCircle+  :: ShapeId+  -> Circle+  -> IO ()+setCircle a0 a1 =+  with a1 $ \p1 ->+  c_b2Shape_SetCircle 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 @b2Body_UpdateMassFromShapes@.++Binds @b2Shape_SetCapsule@.+-}+setCapsule+  :: ShapeId+  -> Capsule+  -> IO ()+setCapsule a0 a1 =+  with a1 $ \p1 ->+  c_b2Shape_SetCapsule a0 p1++{- | Allows you to change a shape to be a segment or update the current segment.++Binds @b2Shape_SetSegment@.+-}+setSegment+  :: ShapeId+  -> Segment+  -> IO ()+setSegment a0 a1 =+  with a1 $ \p1 ->+  c_b2Shape_SetSegment a0 p1++{- | Allows you to change a shape to be a polygon or update the current polygon. This does not modify+the mass properties.++See also @b2Body_UpdateMassFromShapes@.++Binds @b2Shape_SetPolygon@.+-}+setPolygon+  :: ShapeId+  -> Polygon+  -> IO ()+setPolygon a0 a1 =+  with a1 $ \p1 ->+  c_b2Shape_SetPolygon a0 p1++{- | Allows you to change a shape to be a loose chain segment or update the current chain segment.+The resulting shape does not belong to a chain shape. Asserts if the shape is already a chain+segment owned by a chain shape (chainId != @B2_NULL_INDEX@).++Binds @b2Shape_SetChainSegment@.+-}+setChainSegment+  :: ShapeId+  -> Ptr ChainSegment+  -> IO ()+setChainSegment a0 a1 =+  c_b2Shape_SetChainSegment a0 a1++{- | Get the parent chain id if the shape type is a chain segment, otherwise returns @b2_nullChainId@.++Binds @b2Shape_GetParentChain@.+-}+getParentChain+  :: ShapeId+  -> IO ChainId+getParentChain a0 =+  c_b2Shape_GetParentChain a0++{- | Get the maximum capacity required for retrieving all the touching contacts on a shape++Binds @b2Shape_GetContactCapacity@.+-}+getContactCapacity+  :: ShapeId+  -> IO Int+getContactCapacity a0 =+  fromIntegral <$> (c_b2Shape_GetContactCapacity a0)++{- | Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or+shapeIdB on the contact data.++__Note:__ Box2D 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 @b2Shape_GetContactData@.+-}+getContactData+  :: ShapeId+  -> Ptr ContactData+  -> Int+  -- ^ @capacity@+  -> IO Int+getContactData a0 a1 a2 =+  fromIntegral <$> (c_b2Shape_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 @b2Shape_GetSensorData@++Binds @b2Shape_GetSensorCapacity@.+-}+getSensorCapacity+  :: ShapeId+  -- ^ @shapeId@: the id of a sensor shape+  -> IO Int+getSensorCapacity a0 =+  fromIntegral <$> (c_b2Shape_GetSensorCapacity a0)++{- | Get the overlap data for a sensor shape computed during the previous world step.++__Warning:__ do not ignore the return value, it specifies the valid number of elements++__Warning:__ overlaps may contain destroyed shapes so use @b2Shape_IsValid@ to confirm each overlap++Returns: the number of elements filled in the provided array++Binds @b2Shape_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 visitorIds+  -> IO Int+getSensorData a0 a1 a2 =+  fromIntegral <$> (c_b2Shape_GetSensorData a0 a1 (fromIntegral a2))++{- | Get the current world AABB++Binds @b2Shape_GetBounds@.+-}+getBounds+  :: ShapeId+  -> IO AABB+getBounds a0 =+  alloca $ \pOut -> c_b2Shape_GetBounds a0 pOut >> peek pOut++-- | Like 'getBounds', but the result is written into a caller-supplied buffer.+getBoundsInto+  :: ShapeId+  -> Ptr AABB+  -- ^ Result buffer.+  -> IO ()+getBoundsInto a0 out =+  c_b2Shape_GetBounds a0 out++{- | Compute the mass data for a shape++Binds @b2Shape_ComputeMassData@.+-}+computeMassData+  :: ShapeId+  -> IO MassData+computeMassData a0 =+  alloca $ \pOut -> c_b2Shape_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_b2Shape_ComputeMassData a0 out++{- | Get the closest point on a shape to a target point. Target and result are world positions.++Binds @b2Shape_GetClosestPoint@.+-}+getClosestPoint+  :: ShapeId+  -> Pos+  -- ^ @target@+  -> IO Pos+getClosestPoint a0 a1 =+  with a1 $ \p1 ->+  alloca $ \pOut -> c_b2Shape_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 @b2Shape_ApplyWind@.+-}+applyWind+  :: ShapeId+  -- ^ @shapeId@: the shape id+  -> Vec2+  -- ^ @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+  -> Bool+  -- ^ @wake@: should this wake the body+  -> IO ()+applyWind a0 a1 a2 a3 a4 =+  with a1 $ \p1 ->+  c_b2Shape_ApplyWind a0 p1 a2 a3 (fromBool a4)
+ src/Box2D/Tags.hs view
@@ -0,0 +1,209 @@+{-| Phantom tag types for the by-reference structs and callbacks of the Box2D API.++These carry no fields and no 'Storable'; a raw FFI signature reads+@Ptr Foo@ / @FunPtr Foo@ while the caller supplies the value.+-}+module Box2D.Tags+  ( AllocCallbackFcn+  , AssertCallbackFcn+  , CastResultCallbackFcn+  , CustomFilterCallbackFcn+  , DrawBoundsFcn+  , DrawCircleFcn+  , DrawLineFcn+  , DrawPointFcn+  , DrawPolygonFcn+  , DrawSolidCapsuleFcn+  , DrawSolidCircleFcn+  , DrawSolidPolygonFcn+  , DrawStringFcn+  , DrawTransformFcn+  , DynamicTree+  , EnqueueTaskCallbackFcn+  , FinishTaskCallbackFcn+  , FreeCallbackFcn+  , FrictionCallbackFcn+  , LocalManifold+  , LogCallbackFcn+  , OverlapResultCallbackFcn+  , PlaneResultCallbackFcn+  , PreContinuousCallbackFcn+  , PreSolveCallbackFcn+  , Recording+  , Replay+  , ReplayQueryInfo+  , RestitutionCallbackFcn+  , TaskCallbackFcn+  , TreeBoxCastCallbackFcn+  , TreeQueryCallbackFcn+  , TreeRayCastCallbackFcn+  )+where++-- | This function receives proxies found in the AABB query.+data TreeQueryCallbackFcn++{- | 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++{- | 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++{- | Task interface This is the prototype for a Box2D 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 @b2EnqueueTaskCallbackFcn@.+-}+data TaskCallbackFcn++{- | These functions can be provided to Box2D to invoke a task system. Returns a pointer to the+user\'s task object. May be nullptr. A nullptr indicates to Box2D that the work was executed+serially within the callback and there is no need to call @b2FinishTaskCallbackFcn@.+-}+data EnqueueTaskCallbackFcn++-- | Finishes a user task object that wraps a Box2D task.+data FinishTaskCallbackFcn++{- | 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 Box2D state or user application state.+-}+data FrictionCallbackFcn++{- | 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 Box2D state or user application state.+-}+data RestitutionCallbackFcn++{- | 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. Notes:++- this function must be thread-safe+- this is only called if one of the two shapes has enabled custom filtering+- this may be called for awake dynamic bodies and sensors Return false if you want to disable++the collision++See also @b2ShapeDef@.++__Warning:__ Do not attempt to modify the world inside this callback+-}+data CustomFilterCallbackFcn++{- | Prototype for a pre-solve callback. This is called after a contact manifold is updated. This+allows you to inspect a manifold before it goes to the solver. If you are careful, you can+modify the manifold (e.g. modify the normal). You can set the manifold point count to zero to+disable collision. Notes:++- this function must be thread-safe+- this is only called if the shape has enabled pre-solve events+- this is called only for awake dynamic bodies+- this is not called for sensors+- the supplied manifold impulses are not reliable++__Warning:__ Do not attempt to modify the world inside this callback+-}+data PreSolveCallbackFcn++{- | Prototype for a pre-continuous callback. This is called when a time of impact event is computed+in continuous collision. This function can be used to skip the event. Unlike the pre-solve+callback, this only supplies the point and normal. At this stage of the solver the manifold may+not exist or may be stale. Notes:++- this function must be thread-safe+- this is only called if the shape has enabled pre-solve events+- this is called only for awake dynamic bodies+- this is not called for sensors Return false if you want to disable the time of impact event.++__Warning:__ Do not attempt to modify the world inside this callback+-}+data PreContinuousCallbackFcn++{- | Prototype callback for overlap queries. Called for each shape found in the query.++See also @b2World_OverlapAABB@.+-}+data OverlapResultCallbackFcn++{- | Prototype callback for ray and shape 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 A cast with initial overlap will return a zero++fraction and a zero normal.++See also @b2World_CastRay@.+-}+data CastResultCallbackFcn++-- | Used to collect collision planes for character movers. Return true to continue gathering planes.+data PlaneResultCallbackFcn++-- | Prototype for user allocation function+data AllocCallbackFcn++-- | Prototype for user free function+data FreeCallbackFcn++-- | Prototype for the user assert callback. Return 0 to skip the debugger break.+data AssertCallbackFcn++-- | Prototype for user log callback. Used to log warnings.+data LogCallbackFcn++-- | Draw a closed polygon provided in CCW order.+data DrawPolygonFcn++-- | Draw a solid closed polygon provided in CCW order.+data DrawSolidPolygonFcn++-- | Draw a circle.+data DrawCircleFcn++-- | Draw a solid circle.+data DrawSolidCircleFcn++-- | Draw a solid capsule.+data DrawSolidCapsuleFcn++-- | Draw a line segment.+data DrawLineFcn++-- | Draw a transform. Choose your own length scale.+data DrawTransformFcn++-- | Draw a point.+data DrawPointFcn++-- | Draw a string in world space+data DrawStringFcn++{- | Draw a bounding box. With double precision enabled, the single precision bounding box gets+increasing padding when moving far from the origin.+-}+data DrawBoundsFcn++data DynamicTree+data LocalManifold+data Recording+data Replay+data ReplayQueryInfo
+ src/Box2D/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 Box2D.UserData+  ( HasUserData (..)+  , setUserIndex+  , getUserIndex+  , userIndexToPtr+  , ptrToUserIndex+  ) where++import Foreign.Ptr (IntPtr (..), Ptr, intPtrToPtr, ptrToIntPtr)++import Box2D.Body qualified as Body+import Box2D.Id (BodyId, JointId, ShapeId)+import Box2D.Joint qualified as Joint+import Box2D.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/Box2D/WeldJoint.hsc view
@@ -0,0 +1,205 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.WeldJoint+  ( WeldJointDef (..)+  , defaultWeldJointDef+  , create+  , setLinearHertz+  , getLinearHertz+  , setLinearDampingRatio+  , getLinearDampingRatio+  , setAngularHertz+  , getAngularHertz+  , setAngularDampingRatio+  , getAngularDampingRatio+  )+  where++import Foreign+import Foreign.C.Types (CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (JointId (..), WorldId (..))+import Box2D.Joint (JointDef)++#include "box2d/box2d.h"+++{- | Weld joint definition Connects two bodies together rigidly. This constraint provides springs to+mimic soft-body simulation.++__Note:__ The approximate solver in Box2D 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+  -- ^ Angular damping ratio, non-dimensional. Use 1 for critical damping.+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable WeldJointDef where+  sizeOf _ = #{size b2WeldJointDef}+  alignment _ = #{alignment b2WeldJointDef}+  peek p =+    WeldJointDef+      <$> #{peek b2WeldJointDef, base} p+      <*> #{peek b2WeldJointDef, linearHertz} p+      <*> #{peek b2WeldJointDef, angularHertz} p+      <*> #{peek b2WeldJointDef, linearDampingRatio} p+      <*> #{peek b2WeldJointDef, angularDampingRatio} p+      <*> #{peek b2WeldJointDef, internalValue} p+  poke p (WeldJointDef f1 f2 f3 f4 f5 f6) = do+    #{poke b2WeldJointDef, base} p f1+    #{poke b2WeldJointDef, linearHertz} p f2+    #{poke b2WeldJointDef, angularHertz} p f3+    #{poke b2WeldJointDef, linearDampingRatio} p f4+    #{poke b2WeldJointDef, angularDampingRatio} p f5+    #{poke b2WeldJointDef, internalValue} p f6++foreign import ccall unsafe "hs_b2DefaultWeldJointDef"+  c_b2DefaultWeldJointDef :: Ptr WeldJointDef -> IO ()++defaultWeldJointDef :: IO WeldJointDef+defaultWeldJointDef = allocaOut c_b2DefaultWeldJointDef+++foreign import ccall unsafe "b2CreateWeldJoint"+  c_b2CreateWeldJoint :: WorldId -> Ptr WeldJointDef -> IO JointId++foreign import ccall unsafe "b2WeldJoint_SetLinearHertz"+  c_b2WeldJoint_SetLinearHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2WeldJoint_GetLinearHertz"+  c_b2WeldJoint_GetLinearHertz :: JointId -> IO Float++foreign import ccall unsafe "b2WeldJoint_SetLinearDampingRatio"+  c_b2WeldJoint_SetLinearDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2WeldJoint_GetLinearDampingRatio"+  c_b2WeldJoint_GetLinearDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b2WeldJoint_SetAngularHertz"+  c_b2WeldJoint_SetAngularHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2WeldJoint_GetAngularHertz"+  c_b2WeldJoint_GetAngularHertz :: JointId -> IO Float++foreign import ccall unsafe "b2WeldJoint_SetAngularDampingRatio"+  c_b2WeldJoint_SetAngularDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2WeldJoint_GetAngularDampingRatio"+  c_b2WeldJoint_GetAngularDampingRatio :: JointId -> IO Float++{- | Create a weld joint++See also @b2WeldJointDef@ for details.++Binds @b2CreateWeldJoint@.+-}+create+  :: WorldId+  -> WeldJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b2CreateWeldJoint a0 p1++{- | Set the weld joint linear stiffness in Hertz. 0 is rigid.++Binds @b2WeldJoint_SetLinearHertz@.+-}+setLinearHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setLinearHertz a0 a1 =+  c_b2WeldJoint_SetLinearHertz a0 a1++{- | Get the weld joint linear stiffness in Hertz++Binds @b2WeldJoint_GetLinearHertz@.+-}+getLinearHertz+  :: JointId+  -> IO Float+getLinearHertz a0 =+  c_b2WeldJoint_GetLinearHertz a0++{- | Set the weld joint linear damping ratio (non-dimensional)++Binds @b2WeldJoint_SetLinearDampingRatio@.+-}+setLinearDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setLinearDampingRatio a0 a1 =+  c_b2WeldJoint_SetLinearDampingRatio a0 a1++{- | Get the weld joint linear damping ratio (non-dimensional)++Binds @b2WeldJoint_GetLinearDampingRatio@.+-}+getLinearDampingRatio+  :: JointId+  -> IO Float+getLinearDampingRatio a0 =+  c_b2WeldJoint_GetLinearDampingRatio a0++{- | Set the weld joint angular stiffness in Hertz. 0 is rigid.++Binds @b2WeldJoint_SetAngularHertz@.+-}+setAngularHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setAngularHertz a0 a1 =+  c_b2WeldJoint_SetAngularHertz a0 a1++{- | Get the weld joint angular stiffness in Hertz++Binds @b2WeldJoint_GetAngularHertz@.+-}+getAngularHertz+  :: JointId+  -> IO Float+getAngularHertz a0 =+  c_b2WeldJoint_GetAngularHertz a0++{- | Set weld joint angular damping ratio, non-dimensional++Binds @b2WeldJoint_SetAngularDampingRatio@.+-}+setAngularDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setAngularDampingRatio a0 a1 =+  c_b2WeldJoint_SetAngularDampingRatio a0 a1++{- | Get the weld joint angular damping ratio, non-dimensional++Binds @b2WeldJoint_GetAngularDampingRatio@.+-}+getAngularDampingRatio+  :: JointId+  -> IO Float+getAngularDampingRatio a0 =+  c_b2WeldJoint_GetAngularDampingRatio a0
+ src/Box2D/WheelJoint.hsc view
@@ -0,0 +1,373 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.WheelJoint+  ( WheelJointDef (..)+  , defaultWheelJointDef+  , create+  , enableSpring+  , isSpringEnabled+  , setSpringHertz+  , getSpringHertz+  , setSpringDampingRatio+  , getSpringDampingRatio+  , enableLimit+  , isLimitEnabled+  , getLowerLimit+  , getUpperLimit+  , setLimits+  , enableMotor+  , isMotorEnabled+  , setTargetMotorSpeed+  , getTargetMotorSpeed+  , setMaxMotorTorque+  , getMaxMotorTorque+  , getCurrentMotorTorque+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..))+import Box2D.Internal (allocaOut)+import Box2D.Id (JointId (..), WorldId (..))+import Box2D.Joint (JointDef)++#include "box2d/box2d.h"+++{- | Wheel joint definition Body B is a wheel that may rotate freely and slide along the local x-axis+in frame A. The joint translation is zero when the local frame origins coincide in world space.+-}+data WheelJointDef = WheelJointDef+  { base :: JointDef+  -- ^ Base joint definition+  , enableSpring :: CBool+  -- ^ Enable a linear spring along the local axis+  , hertz :: Float+  -- ^ Spring stiffness in Hertz+  , dampingRatio :: Float+  -- ^ Spring damping ratio, non-dimensional+  , enableLimit :: CBool+  -- ^ Enable\/disable the joint linear limit+  , lowerTranslation :: Float+  -- ^ The lower translation limit+  , upperTranslation :: Float+  -- ^ The upper translation limit+  , enableMotor :: CBool+  -- ^ Enable\/disable the joint rotational motor+  , maxMotorTorque :: Float+  -- ^ The maximum motor torque, typically in newton-meters+  , motorSpeed :: Float+  -- ^ The desired motor speed in radians per second+  , internalValue :: CInt+  -- ^ Used internally to detect a valid definition. DO NOT SET.+  }+  deriving (Eq, Show)++instance Storable WheelJointDef where+  sizeOf _ = #{size b2WheelJointDef}+  alignment _ = #{alignment b2WheelJointDef}+  peek p =+    WheelJointDef+      <$> #{peek b2WheelJointDef, base} p+      <*> #{peek b2WheelJointDef, enableSpring} p+      <*> #{peek b2WheelJointDef, hertz} p+      <*> #{peek b2WheelJointDef, dampingRatio} p+      <*> #{peek b2WheelJointDef, enableLimit} p+      <*> #{peek b2WheelJointDef, lowerTranslation} p+      <*> #{peek b2WheelJointDef, upperTranslation} p+      <*> #{peek b2WheelJointDef, enableMotor} p+      <*> #{peek b2WheelJointDef, maxMotorTorque} p+      <*> #{peek b2WheelJointDef, motorSpeed} p+      <*> #{peek b2WheelJointDef, internalValue} p+  poke p (WheelJointDef f1 f2 f3 f4 f5 f6 f7 f8 f9 f10 f11) = do+    #{poke b2WheelJointDef, base} p f1+    #{poke b2WheelJointDef, enableSpring} p f2+    #{poke b2WheelJointDef, hertz} p f3+    #{poke b2WheelJointDef, dampingRatio} p f4+    #{poke b2WheelJointDef, enableLimit} p f5+    #{poke b2WheelJointDef, lowerTranslation} p f6+    #{poke b2WheelJointDef, upperTranslation} p f7+    #{poke b2WheelJointDef, enableMotor} p f8+    #{poke b2WheelJointDef, maxMotorTorque} p f9+    #{poke b2WheelJointDef, motorSpeed} p f10+    #{poke b2WheelJointDef, internalValue} p f11++foreign import ccall unsafe "hs_b2DefaultWheelJointDef"+  c_b2DefaultWheelJointDef :: Ptr WheelJointDef -> IO ()++defaultWheelJointDef :: IO WheelJointDef+defaultWheelJointDef = allocaOut c_b2DefaultWheelJointDef+++foreign import ccall unsafe "b2CreateWheelJoint"+  c_b2CreateWheelJoint :: WorldId -> Ptr WheelJointDef -> IO JointId++foreign import ccall unsafe "b2WheelJoint_EnableSpring"+  c_b2WheelJoint_EnableSpring :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2WheelJoint_IsSpringEnabled"+  c_b2WheelJoint_IsSpringEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2WheelJoint_SetSpringHertz"+  c_b2WheelJoint_SetSpringHertz :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2WheelJoint_GetSpringHertz"+  c_b2WheelJoint_GetSpringHertz :: JointId -> IO Float++foreign import ccall unsafe "b2WheelJoint_SetSpringDampingRatio"+  c_b2WheelJoint_SetSpringDampingRatio :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2WheelJoint_GetSpringDampingRatio"+  c_b2WheelJoint_GetSpringDampingRatio :: JointId -> IO Float++foreign import ccall unsafe "b2WheelJoint_EnableLimit"+  c_b2WheelJoint_EnableLimit :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2WheelJoint_IsLimitEnabled"+  c_b2WheelJoint_IsLimitEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2WheelJoint_GetLowerLimit"+  c_b2WheelJoint_GetLowerLimit :: JointId -> IO Float++foreign import ccall unsafe "b2WheelJoint_GetUpperLimit"+  c_b2WheelJoint_GetUpperLimit :: JointId -> IO Float++foreign import ccall unsafe "b2WheelJoint_SetLimits"+  c_b2WheelJoint_SetLimits :: JointId -> Float -> Float -> IO ()++foreign import ccall unsafe "b2WheelJoint_EnableMotor"+  c_b2WheelJoint_EnableMotor :: JointId -> CBool -> IO ()++foreign import ccall unsafe "b2WheelJoint_IsMotorEnabled"+  c_b2WheelJoint_IsMotorEnabled :: JointId -> IO CBool++foreign import ccall unsafe "b2WheelJoint_SetTargetMotorSpeed"+  c_b2WheelJoint_SetTargetMotorSpeed :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2WheelJoint_GetTargetMotorSpeed"+  c_b2WheelJoint_GetTargetMotorSpeed :: JointId -> IO Float++foreign import ccall unsafe "b2WheelJoint_SetMaxMotorTorque"+  c_b2WheelJoint_SetMaxMotorTorque :: JointId -> Float -> IO ()++foreign import ccall unsafe "b2WheelJoint_GetMaxMotorTorque"+  c_b2WheelJoint_GetMaxMotorTorque :: JointId -> IO Float++foreign import ccall unsafe "b2WheelJoint_GetCurrentMotorTorque"+  c_b2WheelJoint_GetCurrentMotorTorque :: JointId -> IO Float++{- | Create a wheel joint++See also @b2WheelJointDef@ for details.++Binds @b2CreateWheelJoint@.+-}+create+  :: WorldId+  -> WheelJointDef+  -> IO JointId+create a0 a1 =+  with a1 $ \p1 ->+  c_b2CreateWheelJoint a0 p1++{- | Enable\/disable the wheel joint spring++Binds @b2WheelJoint_EnableSpring@.+-}+enableSpring+  :: JointId+  -> Bool+  -- ^ @enableSpring@+  -> IO ()+enableSpring a0 a1 =+  c_b2WheelJoint_EnableSpring a0 (fromBool a1)++{- | Is the wheel joint spring enabled?++Binds @b2WheelJoint_IsSpringEnabled@.+-}+isSpringEnabled+  :: JointId+  -> IO Bool+isSpringEnabled a0 =+  toBool <$> (c_b2WheelJoint_IsSpringEnabled a0)++{- | Set the wheel joint stiffness in Hertz++Binds @b2WheelJoint_SetSpringHertz@.+-}+setSpringHertz+  :: JointId+  -> Float+  -- ^ @hertz@+  -> IO ()+setSpringHertz a0 a1 =+  c_b2WheelJoint_SetSpringHertz a0 a1++{- | Get the wheel joint stiffness in Hertz++Binds @b2WheelJoint_GetSpringHertz@.+-}+getSpringHertz+  :: JointId+  -> IO Float+getSpringHertz a0 =+  c_b2WheelJoint_GetSpringHertz a0++{- | Set the wheel joint damping ratio, non-dimensional++Binds @b2WheelJoint_SetSpringDampingRatio@.+-}+setSpringDampingRatio+  :: JointId+  -> Float+  -- ^ @dampingRatio@+  -> IO ()+setSpringDampingRatio a0 a1 =+  c_b2WheelJoint_SetSpringDampingRatio a0 a1++{- | Get the wheel joint damping ratio, non-dimensional++Binds @b2WheelJoint_GetSpringDampingRatio@.+-}+getSpringDampingRatio+  :: JointId+  -> IO Float+getSpringDampingRatio a0 =+  c_b2WheelJoint_GetSpringDampingRatio a0++{- | Enable\/disable the wheel joint limit++Binds @b2WheelJoint_EnableLimit@.+-}+enableLimit+  :: JointId+  -> Bool+  -- ^ @enableLimit@+  -> IO ()+enableLimit a0 a1 =+  c_b2WheelJoint_EnableLimit a0 (fromBool a1)++{- | Is the wheel joint limit enabled?++Binds @b2WheelJoint_IsLimitEnabled@.+-}+isLimitEnabled+  :: JointId+  -> IO Bool+isLimitEnabled a0 =+  toBool <$> (c_b2WheelJoint_IsLimitEnabled a0)++{- | Get the wheel joint lower limit++Binds @b2WheelJoint_GetLowerLimit@.+-}+getLowerLimit+  :: JointId+  -> IO Float+getLowerLimit a0 =+  c_b2WheelJoint_GetLowerLimit a0++{- | Get the wheel joint upper limit++Binds @b2WheelJoint_GetUpperLimit@.+-}+getUpperLimit+  :: JointId+  -> IO Float+getUpperLimit a0 =+  c_b2WheelJoint_GetUpperLimit a0++{- | Set the wheel joint limits++Binds @b2WheelJoint_SetLimits@.+-}+setLimits+  :: JointId+  -> Float+  -- ^ @lower@+  -> Float+  -- ^ @upper@+  -> IO ()+setLimits a0 a1 a2 =+  c_b2WheelJoint_SetLimits a0 a1 a2++{- | Enable\/disable the wheel joint motor++Binds @b2WheelJoint_EnableMotor@.+-}+enableMotor+  :: JointId+  -> Bool+  -- ^ @enableMotor@+  -> IO ()+enableMotor a0 a1 =+  c_b2WheelJoint_EnableMotor a0 (fromBool a1)++{- | Is the wheel joint motor enabled?++Binds @b2WheelJoint_IsMotorEnabled@.+-}+isMotorEnabled+  :: JointId+  -> IO Bool+isMotorEnabled a0 =+  toBool <$> (c_b2WheelJoint_IsMotorEnabled a0)++{- | Set the wheel joint motor speed in radians per second++Binds @b2WheelJoint_SetTargetMotorSpeed@.+-}+setTargetMotorSpeed+  :: JointId+  -> Float+  -- ^ @motorSpeed@+  -> IO ()+setTargetMotorSpeed a0 a1 =+  c_b2WheelJoint_SetTargetMotorSpeed a0 a1++{- | Get the wheel joint motor speed in radians per second++Binds @b2WheelJoint_GetTargetMotorSpeed@.+-}+getTargetMotorSpeed+  :: JointId+  -> IO Float+getTargetMotorSpeed a0 =+  c_b2WheelJoint_GetTargetMotorSpeed a0++{- | Set the wheel joint maximum motor torque, usually in newton-meters++Binds @b2WheelJoint_SetMaxMotorTorque@.+-}+setMaxMotorTorque+  :: JointId+  -> Float+  -- ^ @torque@+  -> IO ()+setMaxMotorTorque a0 a1 =+  c_b2WheelJoint_SetMaxMotorTorque a0 a1++{- | Get the wheel joint maximum motor torque, usually in newton-meters++Binds @b2WheelJoint_GetMaxMotorTorque@.+-}+getMaxMotorTorque+  :: JointId+  -> IO Float+getMaxMotorTorque a0 =+  c_b2WheelJoint_GetMaxMotorTorque a0++{- | Get the wheel joint current motor torque, usually in newton-meters++Binds @b2WheelJoint_GetCurrentMotorTorque@.+-}+getCurrentMotorTorque+  :: JointId+  -> IO Float+getCurrentMotorTorque a0 =+  c_b2WheelJoint_GetCurrentMotorTorque a0
+ src/Box2D/World.hsc view
@@ -0,0 +1,1902 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++-- Produced by boxnd-gen; edit the generator, not this file.++module Box2D.World+  ( JointEvents (..)+  , JointEvent (..)+  , BodyEvents (..)+  , BodyMoveEvent (..)+  , ContactEvents (..)+  , ContactHitEvent (..)+  , ContactEndTouchEvent (..)+  , ContactBeginTouchEvent (..)+  , SensorEvents (..)+  , SensorEndTouchEvent (..)+  , SensorBeginTouchEvent (..)+  , ExplosionDef (..)+  , Counters (..)+  , Profile (..)+  , WorldDef (..)+  , Capacity (..)+  , RayResult (..)+  , 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+  , setRestitutionIterations+  , getRestitutionIterations+  , enableRestitutionPropagation+  , isRestitutionPropagationEnabled+  , setHitEventThreshold+  , getHitEventThreshold+  , setCustomFilterCallback+  , setPreSolveCallback+  , setGravity+  , getGravity+  , getGravityInto+  , explode+  , setContactTuning+  , getContactTuning+  , setContactRecycleDistance+  , getContactRecycleDistance+  , setMaxLinearSpeed+  , getMaxLinearSpeed+  , enableWarmStarting+  , isWarmStartingEnabled+  , enableSSE2Fallback+  , getAwakeBodyCount+  , getProfile+  , getCounters+  , getMaxCapacity+  , getMaxCapacityInto+  , setUserData+  , getUserData+  , setFrictionCallback+  , setRestitutionCallback+  , setWorkerCount+  , getWorkerCount+  , dumpMemoryStats+  , rebuildStaticTree+  , getStateHash+  , startRecording+  , stopRecording+  , getSnapshot+  , restoreFromSnapshot+  , createFromSnapshot+  )+  where++import Foreign+import Foreign.C.Types (CBool (..), CInt (..), CSize (..))+import Data.Vector.Storable qualified as VS+import Box2D.Internal (allocaOut, copyVector, pokeBounded)+import Box2D.DebugDraw (DebugDraw)+import Box2D.DynamicTree (TreeStats)+import Box2D.Geometry (Capsule, ShapeProxy)+import Box2D.Id (BodyId, ContactId, JointId, ShapeId, WorldId (..))+import Box2D.MathTypes (AABB, Pos, Vec2, WorldTransform)+import Box2D.Shape (QueryFilter)+import Box2D.Tags (CastResultCallbackFcn, CustomFilterCallbackFcn, EnqueueTaskCallbackFcn, FinishTaskCallbackFcn, FrictionCallbackFcn, OverlapResultCallbackFcn, PlaneResultCallbackFcn, PreContinuousCallbackFcn, PreSolveCallbackFcn, Recording, RestitutionCallbackFcn)++#include "box2d/box2d.h"+++{- | 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 b2JointEvents}+  alignment _ = #{alignment b2JointEvents}+  peek p =+    JointEvents+      <$> #{peek b2JointEvents, jointEvents} p+      <*> #{peek b2JointEvents, count} p+  poke p (JointEvents f1 f2) = do+    #{poke b2JointEvents, jointEvents} p f1+    #{poke b2JointEvents, 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 b2JointEvent}+  alignment _ = #{alignment b2JointEvent}+  peek p =+    JointEvent+      <$> #{peek b2JointEvent, jointId} p+      <*> #{peek b2JointEvent, userData} p+  poke p (JointEvent f1 f2) = do+    #{poke b2JointEvent, jointId} p f1+    #{poke b2JointEvent, userData} p f2++{- | Body events are buffered in the Box2D 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 b2BodyEvents}+  alignment _ = #{alignment b2BodyEvents}+  peek p =+    BodyEvents+      <$> #{peek b2BodyEvents, moveEvents} p+      <*> #{peek b2BodyEvents, moveCount} p+  poke p (BodyEvents f1 f2) = do+    #{poke b2BodyEvents, moveEvents} p f1+    #{poke b2BodyEvents, 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+@b2Body_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 world transform of the body.+  , bodyId :: BodyId+  -- ^ The id of the body that moved.+  , fellAsleep :: CBool+  -- ^ Did the body fall asleep this time step?+  }+  deriving (Eq, Show)++instance Storable BodyMoveEvent where+  sizeOf _ = #{size b2BodyMoveEvent}+  alignment _ = #{alignment b2BodyMoveEvent}+  peek p =+    BodyMoveEvent+      <$> #{peek b2BodyMoveEvent, userData} p+      <*> #{peek b2BodyMoveEvent, transform} p+      <*> #{peek b2BodyMoveEvent, bodyId} p+      <*> #{peek b2BodyMoveEvent, fellAsleep} p+  poke p (BodyMoveEvent f1 f2 f3 f4) = do+    #{poke b2BodyMoveEvent, userData} p f1+    #{poke b2BodyMoveEvent, transform} p f2+    #{poke b2BodyMoveEvent, bodyId} p f3+    #{poke b2BodyMoveEvent, fellAsleep} p f4++{- | Contact events are buffered in the Box2D 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 b2ContactEvents}+  alignment _ = #{alignment b2ContactEvents}+  peek p =+    ContactEvents+      <$> #{peek b2ContactEvents, beginEvents} p+      <*> #{peek b2ContactEvents, endEvents} p+      <*> #{peek b2ContactEvents, hitEvents} p+      <*> #{peek b2ContactEvents, beginCount} p+      <*> #{peek b2ContactEvents, endCount} p+      <*> #{peek b2ContactEvents, hitCount} p+  poke p (ContactEvents f1 f2 f3 f4 f5 f6) = do+    #{poke b2ContactEvents, beginEvents} p f1+    #{poke b2ContactEvents, endEvents} p f2+    #{poke b2ContactEvents, hitEvents} p f3+    #{poke b2ContactEvents, beginCount} p f4+    #{poke b2ContactEvents, endCount} p f5+    #{poke b2ContactEvents, hitCount} p f6++{- | A hit event is generated when two shapes collide with a speed faster than the hit event+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 @b2Contact_IsValid@.+  , point :: Pos+  -- ^ Point where the shapes hit. This is a mid-point between the two surfaces. It could be a+  -- speculative point where the shapes were not touching at the beginning of the time step. The+  -- point is reconstructed after the step, so when both bodies are moving it may trail the+  -- impact by up to one step of motion.+  , normal :: Vec2+  -- ^ Normal vector pointing from shape A to shape B+  , approachSpeed :: Float+  -- ^ The speed the shapes are approaching. Always positive. Typically in meters per second.+  }+  deriving (Eq, Show)++instance Storable ContactHitEvent where+  sizeOf _ = #{size b2ContactHitEvent}+  alignment _ = #{alignment b2ContactHitEvent}+  peek p =+    ContactHitEvent+      <$> #{peek b2ContactHitEvent, shapeIdA} p+      <*> #{peek b2ContactHitEvent, shapeIdB} p+      <*> #{peek b2ContactHitEvent, contactId} p+      <*> #{peek b2ContactHitEvent, point} p+      <*> #{peek b2ContactHitEvent, normal} p+      <*> #{peek b2ContactHitEvent, approachSpeed} p+  poke p (ContactHitEvent f1 f2 f3 f4 f5 f6) = do+    #{poke b2ContactHitEvent, shapeIdA} p f1+    #{poke b2ContactHitEvent, shapeIdB} p f2+    #{poke b2ContactHitEvent, contactId} p f3+    #{poke b2ContactHitEvent, point} p f4+    #{poke b2ContactHitEvent, normal} p f5+    #{poke b2ContactHitEvent, approachSpeed} p f6++{- | 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 @b2Shape_IsValid@.+  , shapeIdB :: ShapeId+  -- ^ Id of the second shape+  --+  -- __Warning:__ this shape may have been destroyed+  --+  -- See also @b2Shape_IsValid@.+  , contactId :: ContactId+  -- ^ Id of the contact.+  --+  -- __Warning:__ this contact may have been destroyed+  --+  -- See also @b2Contact_IsValid@.+  }+  deriving (Eq, Show)++instance Storable ContactEndTouchEvent where+  sizeOf _ = #{size b2ContactEndTouchEvent}+  alignment _ = #{alignment b2ContactEndTouchEvent}+  peek p =+    ContactEndTouchEvent+      <$> #{peek b2ContactEndTouchEvent, shapeIdA} p+      <*> #{peek b2ContactEndTouchEvent, shapeIdB} p+      <*> #{peek b2ContactEndTouchEvent, contactId} p+  poke p (ContactEndTouchEvent f1 f2 f3) = do+    #{poke b2ContactEndTouchEvent, shapeIdA} p f1+    #{poke b2ContactEndTouchEvent, shapeIdB} p f2+    #{poke b2ContactEndTouchEvent, 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 @b2Contact_IsValid@ before using this id.+  }+  deriving (Eq, Show)++instance Storable ContactBeginTouchEvent where+  sizeOf _ = #{size b2ContactBeginTouchEvent}+  alignment _ = #{alignment b2ContactBeginTouchEvent}+  peek p =+    ContactBeginTouchEvent+      <$> #{peek b2ContactBeginTouchEvent, shapeIdA} p+      <*> #{peek b2ContactBeginTouchEvent, shapeIdB} p+      <*> #{peek b2ContactBeginTouchEvent, contactId} p+  poke p (ContactBeginTouchEvent f1 f2 f3) = do+    #{poke b2ContactBeginTouchEvent, shapeIdA} p f1+    #{poke b2ContactBeginTouchEvent, shapeIdB} p f2+    #{poke b2ContactBeginTouchEvent, 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 b2SensorEvents}+  alignment _ = #{alignment b2SensorEvents}+  peek p =+    SensorEvents+      <$> #{peek b2SensorEvents, beginEvents} p+      <*> #{peek b2SensorEvents, endEvents} p+      <*> #{peek b2SensorEvents, beginCount} p+      <*> #{peek b2SensorEvents, endCount} p+  poke p (SensorEvents f1 f2 f3 f4) = do+    #{poke b2SensorEvents, beginEvents} p f1+    #{poke b2SensorEvents, endEvents} p f2+    #{poke b2SensorEvents, beginCount} p f3+    #{poke b2SensorEvents, endCount} p f4++{- | An end touch event is generated when a shape stops overlapping a sensor shape. Normally this+event is generated from simulation movement. However, this event can also be generated from API+interactions: setting the body transform, destroying a body or shape, or changing a filter. You+will also get an end event if the sensor is destroyed. Therefore you should always confirm the+shape id is valid using @b2Shape_IsValid@.+-}+data SensorEndTouchEvent = SensorEndTouchEvent+  { sensorShapeId :: ShapeId+  -- ^ The id of the sensor shape+  --+  -- __Warning:__ this shape may have been destroyed+  --+  -- See also @b2Shape_IsValid@.+  , visitorShapeId :: ShapeId+  -- ^ The id of the shape that stopped touching the sensor shape+  --+  -- __Warning:__ this shape may have been destroyed+  --+  -- See also @b2Shape_IsValid@.+  }+  deriving (Eq, Show)++instance Storable SensorEndTouchEvent where+  sizeOf _ = #{size b2SensorEndTouchEvent}+  alignment _ = #{alignment b2SensorEndTouchEvent}+  peek p =+    SensorEndTouchEvent+      <$> #{peek b2SensorEndTouchEvent, sensorShapeId} p+      <*> #{peek b2SensorEndTouchEvent, visitorShapeId} p+  poke p (SensorEndTouchEvent f1 f2) = do+    #{poke b2SensorEndTouchEvent, sensorShapeId} p f1+    #{poke b2SensorEndTouchEvent, 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 b2SensorBeginTouchEvent}+  alignment _ = #{alignment b2SensorBeginTouchEvent}+  peek p =+    SensorBeginTouchEvent+      <$> #{peek b2SensorBeginTouchEvent, sensorShapeId} p+      <*> #{peek b2SensorBeginTouchEvent, visitorShapeId} p+  poke p (SensorBeginTouchEvent f1 f2) = do+    #{poke b2SensorBeginTouchEvent, sensorShapeId} p f1+    #{poke b2SensorBeginTouchEvent, 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.+  , impulsePerLength :: Float+  -- ^ Impulse per unit length. This applies an impulse according to the shape perimeter that is+  -- facing the explosion. Explosions only apply to circles, capsules, and polygons. This may be+  -- negative for implosions.+  }+  deriving (Eq, Show)++instance Storable ExplosionDef where+  sizeOf _ = #{size b2ExplosionDef}+  alignment _ = #{alignment b2ExplosionDef}+  peek p =+    ExplosionDef+      <$> #{peek b2ExplosionDef, maskBits} p+      <*> #{peek b2ExplosionDef, position} p+      <*> #{peek b2ExplosionDef, radius} p+      <*> #{peek b2ExplosionDef, falloff} p+      <*> #{peek b2ExplosionDef, impulsePerLength} p+  poke p (ExplosionDef f1 f2 f3 f4 f5) = do+    #{poke b2ExplosionDef, maskBits} p f1+    #{poke b2ExplosionDef, position} p f2+    #{poke b2ExplosionDef, radius} p f3+    #{poke b2ExplosionDef, falloff} p f4+    #{poke b2ExplosionDef, impulsePerLength} p f5++-- | Counters that give details of the simulation size.+data Counters = Counters+  { byteCount :: CSize+  , bodyCount :: CInt+  , shapeCount :: CInt+  , contactCount :: CInt+  , jointCount :: CInt+  , islandCount :: CInt+  , stackUsed :: CInt+  , staticTreeHeight :: CInt+  , treeHeight :: CInt+  , taskCount :: CInt+  , colorCounts :: 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.+  }+  deriving (Eq, Show)++instance Storable Counters where+  sizeOf _ = #{size b2Counters}+  alignment _ = #{alignment b2Counters}+  peek p =+    Counters+      <$> #{peek b2Counters, byteCount} p+      <*> #{peek b2Counters, bodyCount} p+      <*> #{peek b2Counters, shapeCount} p+      <*> #{peek b2Counters, contactCount} p+      <*> #{peek b2Counters, jointCount} p+      <*> #{peek b2Counters, islandCount} p+      <*> #{peek b2Counters, stackUsed} p+      <*> #{peek b2Counters, staticTreeHeight} p+      <*> #{peek b2Counters, treeHeight} p+      <*> #{peek b2Counters, taskCount} p+      <*> copyVector (#{const 24}) (#{ptr b2Counters, colorCounts} p)+      <*> #{peek b2Counters, awakeContactCount} p+      <*> #{peek b2Counters, recycledContactCount} p+  poke p x = do+    #{poke b2Counters, byteCount} p x.byteCount+    #{poke b2Counters, bodyCount} p x.bodyCount+    #{poke b2Counters, shapeCount} p x.shapeCount+    #{poke b2Counters, contactCount} p x.contactCount+    #{poke b2Counters, jointCount} p x.jointCount+    #{poke b2Counters, islandCount} p x.islandCount+    #{poke b2Counters, stackUsed} p x.stackUsed+    #{poke b2Counters, staticTreeHeight} p x.staticTreeHeight+    #{poke b2Counters, treeHeight} p x.treeHeight+    #{poke b2Counters, taskCount} p x.taskCount+    pokeBounded (#{const 24}) (#{ptr b2Counters, colorCounts} p) x.colorCounts+    #{poke b2Counters, awakeContactCount} p x.awakeContactCount+    #{poke b2Counters, recycledContactCount} p x.recycledContactCount++-- | 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+  , restitution :: 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 b2Profile}+  alignment _ = #{alignment b2Profile}+  peek p =+    Profile+      <$> #{peek b2Profile, step} p+      <*> #{peek b2Profile, pairs} p+      <*> #{peek b2Profile, collide} p+      <*> #{peek b2Profile, solve} p+      <*> #{peek b2Profile, solverSetup} p+      <*> #{peek b2Profile, constraints} p+      <*> #{peek b2Profile, prepareConstraints} p+      <*> #{peek b2Profile, integrateVelocities} p+      <*> #{peek b2Profile, warmStart} p+      <*> #{peek b2Profile, solveImpulses} p+      <*> #{peek b2Profile, integratePositions} p+      <*> #{peek b2Profile, relaxImpulses} p+      <*> #{peek b2Profile, restitution} p+      <*> #{peek b2Profile, storeImpulses} p+      <*> #{peek b2Profile, splitIslands} p+      <*> #{peek b2Profile, transforms} p+      <*> #{peek b2Profile, sensorHits} p+      <*> #{peek b2Profile, jointEvents} p+      <*> #{peek b2Profile, hitEvents} p+      <*> #{peek b2Profile, refit} p+      <*> #{peek b2Profile, bullets} p+      <*> #{peek b2Profile, sleepIslands} p+      <*> #{peek b2Profile, 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 b2Profile, step} p f1+    #{poke b2Profile, pairs} p f2+    #{poke b2Profile, collide} p f3+    #{poke b2Profile, solve} p f4+    #{poke b2Profile, solverSetup} p f5+    #{poke b2Profile, constraints} p f6+    #{poke b2Profile, prepareConstraints} p f7+    #{poke b2Profile, integrateVelocities} p f8+    #{poke b2Profile, warmStart} p f9+    #{poke b2Profile, solveImpulses} p f10+    #{poke b2Profile, integratePositions} p f11+    #{poke b2Profile, relaxImpulses} p f12+    #{poke b2Profile, restitution} p f13+    #{poke b2Profile, storeImpulses} p f14+    #{poke b2Profile, splitIslands} p f15+    #{poke b2Profile, transforms} p f16+    #{poke b2Profile, sensorHits} p f17+    #{poke b2Profile, jointEvents} p f18+    #{poke b2Profile, hitEvents} p f19+    #{poke b2Profile, refit} p f20+    #{poke b2Profile, bullets} p f21+    #{poke b2Profile, sleepIslands} p f22+    #{poke b2Profile, sensors} p f23++{- | World definition used to create a simulation world. Must be initialized using+@b2DefaultWorldDef()@.+-}+data WorldDef = WorldDef+  { gravity :: Vec2+  -- ^ Gravity vector. Box2D has no up-vector defined.+  , restitutionThreshold :: Float+  -- ^ Restitution speed threshold, usually in m\/s. Collisions above this speed have restitution+  -- applied (will bounce).+  , restitutionIterations :: CInt+  -- ^ Number of iterations of the restitution solver. More iterations can lead to less box+  -- spinning.+  --+  -- See also @B2_MAX_RESTITUTION_ITERATIONS@.+  , enableRestitutionPropagation :: CBool+  -- ^ Enable full contact propagation in the restitution solver. Expensive. (Experimental)+  , hitEventThreshold :: Float+  -- ^ Threshold speed for hit events. Usually meters per second.+  , 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 FrictionCallbackFcn+  -- ^ Optional mixing callback for friction. The default uses sqrt(frictionA * frictionB).+  , restitutionCallback :: FunPtr RestitutionCallbackFcn+  -- ^ 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 :: CInt+  -- ^ Number of workers for multithreading. Box2D performs best when using performance cores and+  -- accessing a single L3 cache (uniform memory). Efficiency cores and SMT provide little+  -- benefit and may even harm performance. This is clamped to the range \[1, B2_MAX_WORKERS\]. If+  -- task callbacks are provided then Box2D will use the user provided task system. If no task+  -- callbacks are installed then Box2D will run single-threaded with a worker count of 1, or use+  -- the internal scheduler with a worker count greater than 1.+  , enqueueTask :: FunPtr EnqueueTaskCallbackFcn+  -- ^ Function to spawn tasks+  , finishTask :: FunPtr FinishTaskCallbackFcn+  -- ^ Function to finish a task+  , userTaskContext :: Ptr ()+  -- ^ User context that is provided to enqueueTask and finishTask+  , userData :: Ptr ()+  -- ^ User data+  , 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 b2WorldDef}+  alignment _ = #{alignment b2WorldDef}+  peek p =+    WorldDef+      <$> #{peek b2WorldDef, gravity} p+      <*> #{peek b2WorldDef, restitutionThreshold} p+      <*> #{peek b2WorldDef, restitutionIterations} p+      <*> #{peek b2WorldDef, enableRestitutionPropagation} p+      <*> #{peek b2WorldDef, hitEventThreshold} p+      <*> #{peek b2WorldDef, contactHertz} p+      <*> #{peek b2WorldDef, contactDampingRatio} p+      <*> #{peek b2WorldDef, contactSpeed} p+      <*> #{peek b2WorldDef, maximumLinearSpeed} p+      <*> #{peek b2WorldDef, frictionCallback} p+      <*> #{peek b2WorldDef, restitutionCallback} p+      <*> #{peek b2WorldDef, enableSleep} p+      <*> #{peek b2WorldDef, enableContinuous} p+      <*> #{peek b2WorldDef, workerCount} p+      <*> #{peek b2WorldDef, enqueueTask} p+      <*> #{peek b2WorldDef, finishTask} p+      <*> #{peek b2WorldDef, userTaskContext} p+      <*> #{peek b2WorldDef, userData} p+      <*> #{peek b2WorldDef, capacity} p+      <*> #{peek b2WorldDef, 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) = do+    #{poke b2WorldDef, gravity} p f1+    #{poke b2WorldDef, restitutionThreshold} p f2+    #{poke b2WorldDef, restitutionIterations} p f3+    #{poke b2WorldDef, enableRestitutionPropagation} p f4+    #{poke b2WorldDef, hitEventThreshold} p f5+    #{poke b2WorldDef, contactHertz} p f6+    #{poke b2WorldDef, contactDampingRatio} p f7+    #{poke b2WorldDef, contactSpeed} p f8+    #{poke b2WorldDef, maximumLinearSpeed} p f9+    #{poke b2WorldDef, frictionCallback} p f10+    #{poke b2WorldDef, restitutionCallback} p f11+    #{poke b2WorldDef, enableSleep} p f12+    #{poke b2WorldDef, enableContinuous} p f13+    #{poke b2WorldDef, workerCount} p f14+    #{poke b2WorldDef, enqueueTask} p f15+    #{poke b2WorldDef, finishTask} p f16+    #{poke b2WorldDef, userTaskContext} p f17+    #{poke b2WorldDef, userData} p f18+    #{poke b2WorldDef, capacity} p f19+    #{poke b2WorldDef, internalValue} p f20++{- | Optional world capacities that can be used to avoid run-time allocations.++See also @b2World_GetMaxCapacity@.+-}+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.+  , arenaByteCount :: CInt+  -- ^ Reserve arena allocator space, used for temporary allocations.+  }+  deriving (Eq, Show)++instance Storable Capacity where+  sizeOf _ = #{size b2Capacity}+  alignment _ = #{alignment b2Capacity}+  peek p =+    Capacity+      <$> #{peek b2Capacity, staticShapeCount} p+      <*> #{peek b2Capacity, dynamicShapeCount} p+      <*> #{peek b2Capacity, staticBodyCount} p+      <*> #{peek b2Capacity, dynamicBodyCount} p+      <*> #{peek b2Capacity, contactCount} p+      <*> #{peek b2Capacity, arenaByteCount} p+  poke p (Capacity f1 f2 f3 f4 f5 f6) = do+    #{poke b2Capacity, staticShapeCount} p f1+    #{poke b2Capacity, dynamicShapeCount} p f2+    #{poke b2Capacity, staticBodyCount} p f3+    #{poke b2Capacity, dynamicBodyCount} p f4+    #{poke b2Capacity, contactCount} p f5+    #{poke b2Capacity, arenaByteCount} p f6++{- | Result from @b2World_CastRayClosest@ If there is initial overlap the fraction and normal will be+zero while the point is an arbitrary point in the overlap region.+-}+data RayResult = RayResult+  { shapeId :: ShapeId+  -- ^ The shape hit.+  , point :: Pos+  -- ^ The point of contact, on the surface of the hit shape.+  , normal :: Vec2+  -- ^ The unit normal vector at the point of contact.+  , fraction :: Float+  -- ^ The fraction of the input ray.+  , 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 b2RayResult}+  alignment _ = #{alignment b2RayResult}+  peek p =+    RayResult+      <$> #{peek b2RayResult, shapeId} p+      <*> #{peek b2RayResult, point} p+      <*> #{peek b2RayResult, normal} p+      <*> #{peek b2RayResult, fraction} p+      <*> #{peek b2RayResult, nodeVisits} p+      <*> #{peek b2RayResult, leafVisits} p+      <*> #{peek b2RayResult, hit} p+  poke p (RayResult f1 f2 f3 f4 f5 f6 f7) = do+    #{poke b2RayResult, shapeId} p f1+    #{poke b2RayResult, point} p f2+    #{poke b2RayResult, normal} p f3+    #{poke b2RayResult, fraction} p f4+    #{poke b2RayResult, nodeVisits} p f5+    #{poke b2RayResult, leafVisits} p f6+    #{poke b2RayResult, hit} p f7++foreign import ccall unsafe "hs_b2DefaultWorldDef"+  c_b2DefaultWorldDef :: Ptr WorldDef -> IO ()++defaultWorldDef :: IO WorldDef+defaultWorldDef = allocaOut c_b2DefaultWorldDef++foreign import ccall unsafe "hs_b2DefaultExplosionDef"+  c_b2DefaultExplosionDef :: Ptr ExplosionDef -> IO ()++defaultExplosionDef :: IO ExplosionDef+defaultExplosionDef = allocaOut c_b2DefaultExplosionDef+++#if defined(BOX2D_DOUBLE_PRECISION)+foreign import ccall unsafe "b2CreateWorldDoublePrecision"+#else+foreign import ccall unsafe "b2CreateWorld"+#endif+  c_b2CreateWorld :: Ptr WorldDef -> IO WorldId++foreign import ccall unsafe "b2DestroyWorld"+  c_b2DestroyWorld :: WorldId -> IO ()++foreign import ccall unsafe "b2World_IsValid"+  c_b2World_IsValid :: WorldId -> IO CBool++foreign import ccall safe "b2World_Step"+  c_b2World_Step :: WorldId -> Float -> CInt -> IO ()++foreign import ccall safe "b2World_Draw"+  c_b2World_Draw :: WorldId -> Ptr DebugDraw -> IO ()++foreign import ccall unsafe "hsg_b2World_GetBounds"+  c_b2World_GetBounds :: WorldId -> Ptr AABB -> IO ()++foreign import ccall unsafe "hsg_b2World_GetBodyEvents"+  c_b2World_GetBodyEvents :: WorldId -> Ptr BodyEvents -> IO ()++foreign import ccall unsafe "hsg_b2World_GetSensorEvents"+  c_b2World_GetSensorEvents :: WorldId -> Ptr SensorEvents -> IO ()++foreign import ccall unsafe "hsg_b2World_GetContactEvents"+  c_b2World_GetContactEvents :: WorldId -> Ptr ContactEvents -> IO ()++foreign import ccall unsafe "hsg_b2World_GetJointEvents"+  c_b2World_GetJointEvents :: WorldId -> Ptr JointEvents -> IO ()++foreign import ccall safe "hsg_b2World_OverlapAABB"+  c_b2World_OverlapAABB :: WorldId -> Ptr Pos -> Ptr AABB -> Ptr QueryFilter -> FunPtr OverlapResultCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b2World_OverlapShape"+  c_b2World_OverlapShape :: WorldId -> Ptr Pos -> Ptr ShapeProxy -> Ptr QueryFilter -> FunPtr OverlapResultCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall safe "hsg_b2World_CastRay"+  c_b2World_CastRay :: WorldId -> Ptr Pos -> Ptr Vec2 -> Ptr QueryFilter -> FunPtr CastResultCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall unsafe "hsg_b2World_CastRayClosest"+  c_b2World_CastRayClosest :: WorldId -> Ptr Pos -> Ptr Vec2 -> Ptr QueryFilter -> Ptr RayResult -> IO ()++foreign import ccall safe "hsg_b2World_CastShape"+  c_b2World_CastShape :: WorldId -> Ptr Pos -> Ptr ShapeProxy -> Ptr Vec2 -> Ptr QueryFilter -> FunPtr CastResultCallbackFcn -> Ptr () -> Ptr TreeStats -> IO ()++foreign import ccall unsafe "hsg_b2World_CastMover"+  c_b2World_CastMover :: WorldId -> Ptr Pos -> Ptr Capsule -> Ptr Vec2 -> Ptr QueryFilter -> IO Float++foreign import ccall safe "hsg_b2World_CollideMover"+  c_b2World_CollideMover :: WorldId -> Ptr Pos -> Ptr Capsule -> Ptr QueryFilter -> FunPtr PlaneResultCallbackFcn -> Ptr () -> IO ()++foreign import ccall unsafe "b2World_EnableSleeping"+  c_b2World_EnableSleeping :: WorldId -> CBool -> IO ()++foreign import ccall unsafe "b2World_IsSleepingEnabled"+  c_b2World_IsSleepingEnabled :: WorldId -> IO CBool++foreign import ccall unsafe "b2World_EnableContinuous"+  c_b2World_EnableContinuous :: WorldId -> CBool -> IO ()++foreign import ccall unsafe "b2World_IsContinuousEnabled"+  c_b2World_IsContinuousEnabled :: WorldId -> IO CBool++foreign import ccall unsafe "b2World_SetRestitutionThreshold"+  c_b2World_SetRestitutionThreshold :: WorldId -> Float -> IO ()++foreign import ccall unsafe "b2World_GetRestitutionThreshold"+  c_b2World_GetRestitutionThreshold :: WorldId -> IO Float++foreign import ccall unsafe "b2World_SetRestitutionIterations"+  c_b2World_SetRestitutionIterations :: WorldId -> CInt -> IO ()++foreign import ccall unsafe "b2World_GetRestitutionIterations"+  c_b2World_GetRestitutionIterations :: WorldId -> IO CInt++foreign import ccall unsafe "b2World_EnableRestitutionPropagation"+  c_b2World_EnableRestitutionPropagation :: WorldId -> CBool -> IO ()++foreign import ccall unsafe "b2World_IsRestitutionPropagationEnabled"+  c_b2World_IsRestitutionPropagationEnabled :: WorldId -> IO CBool++foreign import ccall unsafe "b2World_SetHitEventThreshold"+  c_b2World_SetHitEventThreshold :: WorldId -> Float -> IO ()++foreign import ccall unsafe "b2World_GetHitEventThreshold"+  c_b2World_GetHitEventThreshold :: WorldId -> IO Float++foreign import ccall safe "b2World_SetCustomFilterCallback"+  c_b2World_SetCustomFilterCallback :: WorldId -> FunPtr CustomFilterCallbackFcn -> Ptr () -> IO ()++foreign import ccall safe "b2World_SetPreSolveCallback"+  c_b2World_SetPreSolveCallback :: WorldId -> FunPtr PreSolveCallbackFcn -> FunPtr PreContinuousCallbackFcn -> Ptr () -> IO ()++foreign import ccall unsafe "hsg_b2World_SetGravity"+  c_b2World_SetGravity :: WorldId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "hsg_b2World_GetGravity"+  c_b2World_GetGravity :: WorldId -> Ptr Vec2 -> IO ()++foreign import ccall unsafe "b2World_Explode"+  c_b2World_Explode :: WorldId -> Ptr ExplosionDef -> IO ()++foreign import ccall unsafe "b2World_SetContactTuning"+  c_b2World_SetContactTuning :: WorldId -> Float -> Float -> Float -> IO ()++foreign import ccall unsafe "b2World_GetContactTuning"+  c_b2World_GetContactTuning :: WorldId -> Ptr Float -> Ptr Float -> Ptr Float -> IO ()++foreign import ccall unsafe "b2World_SetContactRecycleDistance"+  c_b2World_SetContactRecycleDistance :: WorldId -> Float -> IO ()++foreign import ccall unsafe "b2World_GetContactRecycleDistance"+  c_b2World_GetContactRecycleDistance :: WorldId -> IO Float++foreign import ccall unsafe "b2World_SetMaxLinearSpeed"+  c_b2World_SetMaxLinearSpeed :: WorldId -> Float -> IO ()++foreign import ccall unsafe "b2World_GetMaxLinearSpeed"+  c_b2World_GetMaxLinearSpeed :: WorldId -> IO Float++foreign import ccall unsafe "b2World_EnableWarmStarting"+  c_b2World_EnableWarmStarting :: WorldId -> CBool -> IO ()++foreign import ccall unsafe "b2World_IsWarmStartingEnabled"+  c_b2World_IsWarmStartingEnabled :: WorldId -> IO CBool++foreign import ccall unsafe "b2World_EnableSSE2Fallback"+  c_b2World_EnableSSE2Fallback :: WorldId -> CBool -> IO ()++foreign import ccall unsafe "b2World_GetAwakeBodyCount"+  c_b2World_GetAwakeBodyCount :: WorldId -> IO CInt++foreign import ccall unsafe "hsg_b2World_GetProfile"+  c_b2World_GetProfile :: WorldId -> Ptr Profile -> IO ()++foreign import ccall unsafe "hsg_b2World_GetCounters"+  c_b2World_GetCounters :: WorldId -> Ptr Counters -> IO ()++foreign import ccall unsafe "hsg_b2World_GetMaxCapacity"+  c_b2World_GetMaxCapacity :: WorldId -> Ptr Capacity -> IO ()++foreign import ccall unsafe "b2World_SetUserData"+  c_b2World_SetUserData :: WorldId -> Ptr () -> IO ()++foreign import ccall unsafe "b2World_GetUserData"+  c_b2World_GetUserData :: WorldId -> IO (Ptr ())++foreign import ccall safe "b2World_SetFrictionCallback"+  c_b2World_SetFrictionCallback :: WorldId -> FunPtr FrictionCallbackFcn -> IO ()++foreign import ccall safe "b2World_SetRestitutionCallback"+  c_b2World_SetRestitutionCallback :: WorldId -> FunPtr RestitutionCallbackFcn -> IO ()++foreign import ccall unsafe "b2World_SetWorkerCount"+  c_b2World_SetWorkerCount :: WorldId -> CInt -> IO ()++foreign import ccall unsafe "b2World_GetWorkerCount"+  c_b2World_GetWorkerCount :: WorldId -> IO CInt++foreign import ccall unsafe "b2World_DumpMemoryStats"+  c_b2World_DumpMemoryStats :: WorldId -> IO ()++foreign import ccall unsafe "b2World_RebuildStaticTree"+  c_b2World_RebuildStaticTree :: WorldId -> IO ()++foreign import ccall unsafe "b2World_GetStateHash"+  c_b2World_GetStateHash :: WorldId -> IO Word64++foreign import ccall unsafe "b2World_StartRecording"+  c_b2World_StartRecording :: WorldId -> Ptr Recording -> IO ()++foreign import ccall unsafe "b2World_StopRecording"+  c_b2World_StopRecording :: WorldId -> IO ()++foreign import ccall unsafe "b2World_GetSnapshot"+  c_b2World_GetSnapshot :: WorldId -> Ptr Word8 -> CInt -> IO CInt++foreign import ccall unsafe "b2World_RestoreFromSnapshot"+  c_b2World_RestoreFromSnapshot :: WorldId -> Ptr Word8 -> CInt -> IO CBool++foreign import ccall unsafe "b2CreateWorldFromSnapshot"+  c_b2CreateWorldFromSnapshot :: Ptr Word8 -> CInt -> CInt -> IO WorldId++{- | Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You+may create up to @B2_MAX_WORLDS@ worlds. Each world is completely independent and may be simulated+in parallel.++Returns: the world id.++Binds @b2CreateWorld@.+-}+create+  :: WorldDef+  -> IO WorldId+create a0 =+  with a0 $ \p0 ->+  c_b2CreateWorld p0++{- | Destroy a world++Binds @b2DestroyWorld@.+-}+destroy+  :: WorldId+  -> IO ()+destroy a0 =+  c_b2DestroyWorld a0++{- | World id validation. Provides validation for up to 64K allocations.++Binds @b2World_IsValid@.+-}+isValid+  :: WorldId+  -> IO Bool+isValid a0 =+  toBool <$> (c_b2World_IsValid a0)++{- | Simulate a world for one time step. This performs collision detection, integration, and+constraint solution.++Binds @b2World_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_b2World_Step a0 a1 (fromIntegral a2)++{- | Call this to draw shapes and other debug draw data++Binds @b2World_Draw@.+-}+draw+  :: WorldId+  -> Ptr DebugDraw+  -> IO ()+draw a0 a1 =+  c_b2World_Draw a0 a1++{- | Call this to get the world bounds based on the union of all shape bounds.++Binds @b2World_GetBounds@.+-}+getBounds+  :: WorldId+  -> IO AABB+getBounds a0 =+  alloca $ \pOut -> c_b2World_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_b2World_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 @b2World_GetBodyEvents@.+-}+getBodyEvents+  :: WorldId+  -> IO BodyEvents+getBodyEvents a0 =+  alloca $ \pOut -> c_b2World_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_b2World_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 @b2World_GetSensorEvents@.+-}+getSensorEvents+  :: WorldId+  -> IO SensorEvents+getSensorEvents a0 =+  alloca $ \pOut -> c_b2World_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_b2World_GetSensorEvents a0 out++{- | Get contact events for the current time step. The event data is transient. Do not store a+reference to this data.++Binds @b2World_GetContactEvents@.+-}+getContactEvents+  :: WorldId+  -> IO ContactEvents+getContactEvents a0 =+  alloca $ \pOut -> c_b2World_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_b2World_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 @b2World_GetJointEvents@.+-}+getJointEvents+  :: WorldId+  -> IO JointEvents+getJointEvents a0 =+  alloca $ \pOut -> c_b2World_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_b2World_GetJointEvents a0 out++{- | Overlap test for all shapes that *potentially* overlap the provided AABB. The AABB is relative+to the origin, which keeps the test precise far from the world origin in large world mode. Near+the origin pass @b2Pos_zero@ and a world AABB.++Binds @b2World_OverlapAABB@.+-}+overlapAABB+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> AABB+  -> QueryFilter+  -> FunPtr OverlapResultCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+overlapAABB a0 a1 a2 a3 a4 a5 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  alloca $ \pOut -> c_b2World_OverlapAABB a0 p1 p2 p3 a4 a5 pOut >> peek pOut++{- | Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative+to the origin. Near the origin pass @b2Pos_zero@ and world points.++Binds @b2World_OverlapShape@.+-}+overlapShape+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> Ptr ShapeProxy+  -> QueryFilter+  -> FunPtr OverlapResultCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+overlapShape a0 a1 a2 a3 a4 a5 =+  with a1 $ \p1 ->+  with a3 $ \p3 ->+  alloca $ \pOut -> c_b2World_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 @b2World_CastRay@.+-}+castRay+  :: WorldId+  -- ^ @worldId@: The world to cast the ray against+  -> Pos+  -- ^ @origin@: The start point of the ray+  -> Vec2+  -- ^ @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 CastResultCallbackFcn+  -- ^ @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_b2World_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 @b2World_CastRay()@ and does not allow for custom+filtering.++Binds @b2World_CastRayClosest@.+-}+castRayClosest+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> Vec2+  -- ^ @translation@+  -> QueryFilter+  -> IO RayResult+castRayClosest a0 a1 a2 a3 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  alloca $ \pOut -> c_b2World_CastRayClosest a0 p1 p2 p3 pOut >> peek pOut++{- | Cast a shape through the world. Similar to a ray cast except that a shape is cast instead of a+point. The proxy points are relative to the origin. Callback points are world positions.++See also @b2World_CastRay@.++Binds @b2World_CastShape@.+-}+castShape+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> Ptr ShapeProxy+  -> Vec2+  -- ^ @translation@+  -> QueryFilter+  -> FunPtr CastResultCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> IO TreeStats+castShape a0 a1 a2 a3 a4 a5 a6 =+  with a1 $ \p1 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  alloca $ \pOut -> c_b2World_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. The mover capsule is relative to the origin. Near the+origin pass @b2Pos_zero@ and a world capsule. Returns the translation fraction at the first hit.++Binds @b2World_CastMover@.+-}+castMover+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> Capsule+  -- ^ @mover@+  -> Vec2+  -- ^ @translation@+  -> QueryFilter+  -> IO Float+castMover a0 a1 a2 a3 a4 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  with a4 $ \p4 ->+  c_b2World_CastMover a0 p1 p2 p3 p4++{- | Collide a capsule mover with the world, gathering collision planes that can be fed to+@b2SolvePlanes@. Useful for kinematic character movement. The mover capsule and the resulting+planes are relative to the origin.++Binds @b2World_CollideMover@.+-}+collideMover+  :: WorldId+  -> Pos+  -- ^ @origin@+  -> Capsule+  -- ^ @mover@+  -> QueryFilter+  -> FunPtr PlaneResultCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> IO ()+collideMover a0 a1 a2 a3 a4 a5 =+  with a1 $ \p1 ->+  with a2 $ \p2 ->+  with a3 $ \p3 ->+  c_b2World_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 @b2WorldDef@.++Binds @b2World_EnableSleeping@.+-}+enableSleeping+  :: WorldId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSleeping a0 a1 =+  c_b2World_EnableSleeping a0 (fromBool a1)++{- | Is body sleeping enabled?++Binds @b2World_IsSleepingEnabled@.+-}+isSleepingEnabled+  :: WorldId+  -> IO Bool+isSleepingEnabled a0 =+  toBool <$> (c_b2World_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 @b2WorldDef@.++Binds @b2World_EnableContinuous@.+-}+enableContinuous+  :: WorldId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableContinuous a0 a1 =+  c_b2World_EnableContinuous a0 (fromBool a1)++{- | Is continuous collision enabled?++Binds @b2World_IsContinuousEnabled@.+-}+isContinuousEnabled+  :: WorldId+  -> IO Bool+isContinuousEnabled a0 =+  toBool <$> (c_b2World_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 @b2WorldDef@.++Binds @b2World_SetRestitutionThreshold@.+-}+setRestitutionThreshold+  :: WorldId+  -> Float+  -- ^ @value@+  -> IO ()+setRestitutionThreshold a0 a1 =+  c_b2World_SetRestitutionThreshold a0 a1++{- | Get the restitution speed threshold. Usually in meters per second.++Binds @b2World_GetRestitutionThreshold@.+-}+getRestitutionThreshold+  :: WorldId+  -> IO Float+getRestitutionThreshold a0 =+  c_b2World_GetRestitutionThreshold a0++{- | Set the number of iterations used by the restitution solver.++See also @B2_MAX_RESTITUTION_ITERATIONS@.++Binds @b2World_SetRestitutionIterations@.+-}+setRestitutionIterations+  :: WorldId+  -> Int+  -- ^ @iterations@+  -> IO ()+setRestitutionIterations a0 a1 =+  c_b2World_SetRestitutionIterations a0 (fromIntegral a1)++{- | Get the number of iterations used by the restitution solver.++Binds @b2World_GetRestitutionIterations@.+-}+getRestitutionIterations+  :: WorldId+  -> IO Int+getRestitutionIterations a0 =+  fromIntegral <$> (c_b2World_GetRestitutionIterations a0)++{- | Enable\/disable restitution propagation.++See also @b2WorldDef::enableRestitutionPropagation@.++Binds @b2World_EnableRestitutionPropagation@.+-}+enableRestitutionPropagation+  :: WorldId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableRestitutionPropagation a0 a1 =+  c_b2World_EnableRestitutionPropagation a0 (fromBool a1)++{- | Is restitution propagation enabled?++Binds @b2World_IsRestitutionPropagationEnabled@.+-}+isRestitutionPropagationEnabled+  :: WorldId+  -> IO Bool+isRestitutionPropagationEnabled a0 =+  toBool <$> (c_b2World_IsRestitutionPropagationEnabled a0)++{- | Adjust the hit event threshold. This controls the collision speed needed to generate a+@b2ContactHitEvent@. Usually in meters per second.++See also @b2WorldDef::hitEventThreshold@.++Binds @b2World_SetHitEventThreshold@.+-}+setHitEventThreshold+  :: WorldId+  -> Float+  -- ^ @value@+  -> IO ()+setHitEventThreshold a0 a1 =+  c_b2World_SetHitEventThreshold a0 a1++{- | Get the hit event speed threshold. Usually in meters per second.++Binds @b2World_GetHitEventThreshold@.+-}+getHitEventThreshold+  :: WorldId+  -> IO Float+getHitEventThreshold a0 =+  c_b2World_GetHitEventThreshold a0++{- | Register the custom filter callback. This is optional.++Binds @b2World_SetCustomFilterCallback@.+-}+setCustomFilterCallback+  :: WorldId+  -> FunPtr CustomFilterCallbackFcn+  -> Ptr ()+  -- ^ @context@+  -> IO ()+setCustomFilterCallback a0 a1 a2 =+  c_b2World_SetCustomFilterCallback a0 a1 a2++{- | Register the pre-solve and pre-continuous callbacks. This is optional.++Binds @b2World_SetPreSolveCallback@.+-}+setPreSolveCallback+  :: WorldId+  -> FunPtr PreSolveCallbackFcn+  -- ^ @preSolveFcn@+  -> FunPtr PreContinuousCallbackFcn+  -- ^ @preContinuousFcn@+  -> Ptr ()+  -- ^ @context@+  -> IO ()+setPreSolveCallback a0 a1 a2 a3 =+  c_b2World_SetPreSolveCallback a0 a1 a2 a3++{- | Set the gravity vector for the entire world. Box2D has no concept of an up direction and this is+left as a decision for the application. Usually in m\/s^2.++See also @b2WorldDef@.++Binds @b2World_SetGravity@.+-}+setGravity+  :: WorldId+  -> Vec2+  -- ^ @gravity@+  -> IO ()+setGravity a0 a1 =+  with a1 $ \p1 ->+  c_b2World_SetGravity a0 p1++{- | Get the gravity vector++Binds @b2World_GetGravity@.+-}+getGravity+  :: WorldId+  -> IO Vec2+getGravity a0 =+  alloca $ \pOut -> c_b2World_GetGravity a0 pOut >> peek pOut++-- | Like 'getGravity', but the result is written into a caller-supplied buffer.+getGravityInto+  :: WorldId+  -> Ptr Vec2+  -- ^ Result buffer.+  -> IO ()+getGravityInto a0 out =+  c_b2World_GetGravity a0 out++{- | Apply a radial explosion++Binds @b2World_Explode@.+-}+explode+  :: WorldId+  -- ^ @worldId@: The world id+  -> ExplosionDef+  -- ^ @explosionDef@: The explosion definition+  -> IO ()+explode a0 a1 =+  with a1 $ \p1 ->+  c_b2World_Explode a0 p1++{- | Adjust contact tuning parameters++__Note:__ Advanced feature++Binds @b2World_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+  -- ^ @pushSpeed@: The maximum contact constraint push out speed (meters per second)+  -> IO ()+setContactTuning a0 a1 a2 a3 =+  c_b2World_SetContactTuning a0 a1 a2 a3++{- | Get the contact tuning.++Binds @b2World_GetContactTuning@.+-}+getContactTuning+  :: WorldId+  -> Ptr Float+  -- ^ @hertz@+  -> Ptr Float+  -- ^ @dampingRatio@+  -> Ptr Float+  -- ^ @pushSpeed@+  -> IO ()+getContactTuning a0 a1 a2 a3 =+  c_b2World_GetContactTuning a0 a1 a2 a3++{- | Set the contact point recycling distance. Setting this to zero disables contact point recycling.+Usually in meters.++Binds @b2World_SetContactRecycleDistance@.+-}+setContactRecycleDistance+  :: WorldId+  -> Float+  -- ^ @recycleDistance@+  -> IO ()+setContactRecycleDistance a0 a1 =+  c_b2World_SetContactRecycleDistance a0 a1++{- | Get the contact point recycling distance. Usually in meters.++Binds @b2World_GetContactRecycleDistance@.+-}+getContactRecycleDistance+  :: WorldId+  -> IO Float+getContactRecycleDistance a0 =+  c_b2World_GetContactRecycleDistance a0++{- | Set the maximum linear speed. Usually in m\/s.++Binds @b2World_SetMaxLinearSpeed@.+-}+setMaxLinearSpeed+  :: WorldId+  -> Float+  -- ^ @maximumLinearSpeed@+  -> IO ()+setMaxLinearSpeed a0 a1 =+  c_b2World_SetMaxLinearSpeed a0 a1++{- | Get the maximum linear speed. Usually in m\/s.++Binds @b2World_GetMaxLinearSpeed@.+-}+getMaxLinearSpeed+  :: WorldId+  -> IO Float+getMaxLinearSpeed a0 =+  c_b2World_GetMaxLinearSpeed a0++{- | Enable\/disable constraint warm starting. Advanced feature for testing. Disabling warm starting+greatly reduces stability and provides no performance gain.++Binds @b2World_EnableWarmStarting@.+-}+enableWarmStarting+  :: WorldId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableWarmStarting a0 a1 =+  c_b2World_EnableWarmStarting a0 (fromBool a1)++{- | Is constraint warm starting enabled?++Binds @b2World_IsWarmStartingEnabled@.+-}+isWarmStartingEnabled+  :: WorldId+  -> IO Bool+isWarmStartingEnabled a0 =+  toBool <$> (c_b2World_IsWarmStartingEnabled a0)++{- | Force SSE2 to be used even when AVX2 is available. This is for testing.++Binds @b2World_EnableSSE2Fallback@.+-}+enableSSE2Fallback+  :: WorldId+  -> Bool+  -- ^ @flag@+  -> IO ()+enableSSE2Fallback a0 a1 =+  c_b2World_EnableSSE2Fallback a0 (fromBool a1)++{- | Get the number of awake bodies.++Binds @b2World_GetAwakeBodyCount@.+-}+getAwakeBodyCount+  :: WorldId+  -> IO Int+getAwakeBodyCount a0 =+  fromIntegral <$> (c_b2World_GetAwakeBodyCount a0)++{- | Get the current world performance profile++Binds @b2World_GetProfile@.+-}+getProfile+  :: WorldId+  -> Ptr Profile+  -- ^ Result buffer.+  -> IO ()+getProfile a0 out =+  c_b2World_GetProfile a0 out++{- | Get world counters and sizes++Binds @b2World_GetCounters@.+-}+getCounters+  :: WorldId+  -> Ptr Counters+  -- ^ Result buffer.+  -> IO ()+getCounters a0 out =+  c_b2World_GetCounters a0 out++{- | Get max capacity. This can be used with @b2WorldDef@ to avoid run-time allocations and copies++Binds @b2World_GetMaxCapacity@.+-}+getMaxCapacity+  :: WorldId+  -> IO Capacity+getMaxCapacity a0 =+  alloca $ \pOut -> c_b2World_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_b2World_GetMaxCapacity a0 out++{- | Set the user data pointer.++Binds @b2World_SetUserData@.+-}+setUserData+  :: WorldId+  -> Ptr ()+  -- ^ @userData@+  -> IO ()+setUserData a0 a1 =+  c_b2World_SetUserData a0 a1++{- | Get the user data pointer.++Binds @b2World_GetUserData@.+-}+getUserData+  :: WorldId+  -> IO (Ptr ())+getUserData a0 =+  c_b2World_GetUserData a0++{- | Set the friction callback. Passing NULL resets to default.++Binds @b2World_SetFrictionCallback@.+-}+setFrictionCallback+  :: WorldId+  -> FunPtr FrictionCallbackFcn+  -> IO ()+setFrictionCallback a0 a1 =+  c_b2World_SetFrictionCallback a0 a1++{- | Set the restitution callback. Passing NULL resets to default.++Binds @b2World_SetRestitutionCallback@.+-}+setRestitutionCallback+  :: WorldId+  -> FunPtr RestitutionCallbackFcn+  -> IO ()+setRestitutionCallback a0 a1 =+  c_b2World_SetRestitutionCallback a0 a1++{- | Set the worker count. Must be in the range \[1, B2_MAX_WORKERS\]++Binds @b2World_SetWorkerCount@.+-}+setWorkerCount+  :: WorldId+  -> Int+  -- ^ @count@+  -> IO ()+setWorkerCount a0 a1 =+  c_b2World_SetWorkerCount a0 (fromIntegral a1)++{- | Get the worker count.++Binds @b2World_GetWorkerCount@.+-}+getWorkerCount+  :: WorldId+  -> IO Int+getWorkerCount a0 =+  fromIntegral <$> (c_b2World_GetWorkerCount a0)++{- | Dump memory stats to box2d_memory.txt++Binds @b2World_DumpMemoryStats@.+-}+dumpMemoryStats+  :: WorldId+  -> IO ()+dumpMemoryStats a0 =+  c_b2World_DumpMemoryStats a0++{- | This is for internal testing++Binds @b2World_RebuildStaticTree@.+-}+rebuildStaticTree+  :: WorldId+  -> IO ()+rebuildStaticTree a0 =+  c_b2World_RebuildStaticTree a0++{- | Compute a hash of the simulation state. Use this to detect desyncs, for example in rollback+resimulation.++Binds @b2World_GetStateHash@.+-}+getStateHash+  :: WorldId+  -> IO Word64+getStateHash a0 =+  c_b2World_GetStateHash a0++{- | Begin recording the world into \@p recording. Serializes a snapshot of the current world as the+seed, then logs every mutating call. Call at a step boundary, before or after creating bodies.+Start before the first step to capture the whole session. No effect if already recording or if+the world is locked.++Binds @b2World_StartRecording@.+-}+startRecording+  :: WorldId+  -- ^ @worldId@: The world to record+  -> Ptr Recording+  -- ^ @recording@: A recording buffer from @b2CreateRecording@+  -> IO ()+startRecording a0 a1 =+  c_b2World_StartRecording a0 a1++{- | Stop recording. The recording buffer keeps its bytes. Save and destroy it yourself.++Binds @b2World_StopRecording@.+-}+stopRecording+  :: WorldId+  -- ^ @worldId@: The world being recorded+  -> IO ()+stopRecording a0 =+  c_b2World_StopRecording a0++{- | Write a snapshot of the world\'s simulation state into a caller-owned buffer. Call once with+image == NULL to get the required size, then again with a buffer of at least that size. Must be+called at a step boundary.++Returns: The number of bytes the snapshot needs. If it exceeds capacity nothing is written. Returns 0 if the world is mid-step.++Binds @b2World_GetSnapshot@.+-}+getSnapshot+  :: WorldId+  -- ^ @worldId@: The world to snapshot+  -> Ptr Word8+  -- ^ @image@: Destination buffer, or NULL to query the size+  -> Int+  -- ^ @capacity@: Size of image in bytes, ignored when querying+  -> IO Int+getSnapshot a0 a1 a2 =+  fromIntegral <$> (c_b2World_GetSnapshot a0 a1 (fromIntegral a2))++{- | Restore a world\'s simulation state from a snapshot image. The @b2WorldId@ and ids held from this+same world stay valid for objects that existed at the snapshot instant. Callbacks and threading+configuration are preserved. User data on shapes, bodies, and joints is nulled.++Returns: true on success. On a rejected image (bad magic, version, or layout) the world is left unchanged. A corrupt payload detected after the rebuild begins returns false and leaves the world unusable, so the caller must destroy it.++Binds @b2World_RestoreFromSnapshot@.+-}+restoreFromSnapshot+  :: WorldId+  -- ^ @worldId@: The world to restore into+  -> Ptr Word8+  -- ^ @image@: A snapshot image produced by @b2World_GetSnapshot@+  -> Int+  -- ^ @size@: Size of image in bytes+  -> IO Bool+restoreFromSnapshot a0 a1 a2 =+  toBool <$> (c_b2World_RestoreFromSnapshot a0 a1 (fromIntegral a2))++{- | Create a new world from a snapshot image. This is mainly useful for replay. Callbacks are reset+to default.++Returns: The new world id, or @b2_nullWorldId@ on failure.++Binds @b2CreateWorldFromSnapshot@.+-}+createFromSnapshot+  :: Ptr Word8+  -- ^ @image@: A snapshot image produced by @b2World_GetSnapshot@+  -> Int+  -- ^ @size@: Size of image in bytes+  -> Int+  -- ^ @workerCount@: Worker count for the new world. 0 uses the serial single-worker fallback.+  -> IO WorldId+createFromSnapshot a0 a1 a2 =+  c_b2CreateWorldFromSnapshot a0 (fromIntegral a1) (fromIntegral a2)
+ test/Spec.hs view
@@ -0,0 +1,1048 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# LANGUAGE PatternSynonyms #-}++module Main (main) where++import Control.Concurrent (getNumCapabilities, yield)+import Control.Exception (ErrorCall (..), bracket, evaluate, try)+import Control.Monad (forM_, replicateM_, unless, when)+import Data.IORef+import Data.Vector.Storable qualified as VS+import Foreign+import Foreign.C.Types (CBool (..))+import GHC.Clock (getMonotonicTime)++import Test.Tasty+import Test.Tasty.HUnit+import Test.Tasty.Runners (NumThreads (..))++import Box2D.Base qualified as Base+import Box2D.Body qualified as Body+import Box2D.Callbacks qualified as CB+import Box2D.Chain qualified as Chain+import Box2D.Collision qualified as Collision+import Box2D.Contact qualified as Contact+import Box2D.DistanceJoint qualified as DistanceJoint+import Box2D.DynamicTree qualified as DynamicTree+import Box2D.Events qualified as Events+import Box2D.Extra (withShapeProxy)+import Box2D.Id (BodyId (..), ChainId, ContactId (..), ShapeId (..), WorldId (..), nullBodyId, nullContactId, nullWorldId)+import Box2D.Joint qualified as Joint+import Box2D.MathFunctions+import Box2D.MathTypes+import Box2D.Mover qualified as Mover+import Box2D.Shape qualified as Shape+import Box2D.UserData qualified as UD+import Box2D.World qualified as World++import Box2D.Body (BodyDef (..), defaultBodyDef, pattern DynamicBody, pattern StaticBody)+import Box2D.Collision (CollisionPlane (..), Manifold (..), PlaneResult (..))+import Box2D.Contact (ContactData (..))+import Box2D.DebugDraw (DebugDraw (..), defaultDebugDraw)+import Box2D.Distance (SimplexCache (..))+import Box2D.DistanceJoint (DistanceJointDef (..), defaultDistanceJointDef)+import Box2D.FilterJoint (defaultFilterJointDef)+import Box2D.Geometry (Capsule (..), CastOutput (..), Circle (..), Hull (..), MassData (..), Polygon (..), RayCastInput (..), Segment (..), ShapeProxy (..))+import Box2D.Joint (JointDef (..), pattern DistanceJoint)+import Box2D.MotorJoint (defaultMotorJointDef)+import Box2D.PrismaticJoint (defaultPrismaticJointDef)+import Box2D.RevoluteJoint (defaultRevoluteJointDef)+import Box2D.Shape (QueryFilter (..), ShapeDef (..), defaultFilter, defaultQueryFilter, defaultShapeDef, defaultSurfaceMaterial, pattern CapsuleShape, pattern ChainSegmentShape, pattern CircleShape, pattern PolygonShape, pattern SegmentShape)+import Box2D.WeldJoint (defaultWeldJointDef)+import Box2D.WheelJoint (defaultWheelJointDef)+import Box2D.World (BodyMoveEvent (..), ContactBeginTouchEvent (..), ContactEndTouchEvent (..), ContactHitEvent (..), ExplosionDef (..), JointEvent (..), RayResult (..), SensorBeginTouchEvent (..), SensorEndTouchEvent (..), WorldDef (..), defaultExplosionDef, defaultWorldDef)++main :: IO ()+-- Box2D 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+    "Box2D"+    [ testGroup "Base" baseTests+    , testGroup "MathTypes" mathTypesTests+    , testGroup "MathFunctions" mathFunctionsTests+    , testGroup "Id" idTests+    , testGroup "Types" typesTests+    , testGroup "Simulation" simulationTests+    , testGroup "Generated breadth" generatedTests+    , testGroup "Joints" jointTests+    , testGroup "Events" eventTests+    , testGroup "Queries" queryTests+    , testGroup "Polygons and chains" polygonTests+    , testGroup "Proxies, contacts and the mover" moverTests+    , testGroup "Callbacks" callbackTests+    ]++-- | 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 3.2.0" $ do+      v <- Base.getVersion+      v.major @?= 3+      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 16 else 8+  ]++mathTypesTests :: [TestTree]+mathTypesTests =+  [ testCase "Pos and WorldTransform round-trip through Storable" $ do+      roundTrip (Pos 1.5 (-2.5)) >>= (@?= Pos 1.5 (-2.5))+      let xf = WorldTransform (Pos 3 4) (Rot 0 1)+      roundTrip xf >>= (@?= xf)+  , testCase "world position helpers cross the Pos/Vec2 boundary" $ do+      let xf = WorldTransform (Pos 10 20) (Rot 0 1)+      toVec2 (toPos (Vec2 1.5 2.5)) @?= Vec2 1.5 2.5+      subPos (Pos 5 7) (Pos 2 3) @?= Vec2 3 4+      offsetPos (Pos 1 2) (Vec2 3 4) @?= Pos 4 6+      transformWorldPoint xf (Vec2 1 0) @?= Pos 10 21+      invTransformWorldPoint xf (Pos 10 21) @?= Vec2 1 0+      toRelativeTransform xf (Pos 9 18) @?= Transform (Vec2 1 2) (Rot 0 1)+      makeWorldTransform (Transform (Vec2 1 2) (Rot 0 1)) @?= WorldTransform (Pos 1 2) (Rot 0 1)+  , testCase "type sizes match the C ABI" $ do+      sizeOf (Vec2 0 0) @?= 8+      sizeOf (Rot 1 0) @?= 8+      sizeOf (Transform vec2Zero rotIdentity) @?= 16+      sizeOf (Mat22 vec2Zero vec2Zero) @?= 16+      sizeOf (AABB vec2Zero vec2Zero) @?= 16+      sizeOf (Plane vec2Zero 0) @?= 12+  , testCase "Vec2 round-trips" $+      roundTrip (Vec2 1 2) >>= (@?= Vec2 1 2)+  , testCase "Transform round-trips" $ do+      let t = Transform (Vec2 1 2) (Rot 0.6 0.8)+      roundTrip t >>= (@?= t)+  , testCase "AABB round-trips" $ do+      let a = AABB (Vec2 (-1) (-2)) (Vec2 3 4)+      roundTrip a >>= (@?= a)+  ]++mathFunctionsTests :: [TestTree]+mathFunctionsTests =+  [ testCase "finite vector is valid" $+      isValidVec2 (Vec2 1 2) >>= (@?= True)+  , testCase "NaN vector is invalid" $+      isValidVec2 (Vec2 (0 / 0) 0) >>= (@?= False)+  , testCase "infinite vector is invalid" $+      isValidVec2 (Vec2 (1 / 0) 0) >>= (@?= False)+  , testCase "identity rotation is valid" $+      isValidRot rotIdentity >>= (@?= True)+  , testCase "identity transform is valid" $+      isValidTransform transformIdentity >>= (@?= True)+  , testCase "computeCosSin 0 = (1, 0)" $ do+      CosSin c s <- computeCosSin 0+      approx 1 c+      approx 0 s+  , testCase "makeRot 0 is the identity" $ do+      Rot c s <- makeRot 0+      approx 1 c+      approx 0 s+  , testCase "rotGetAngle recovers the angle" $ do+      approx 1.2 (rotGetAngle (Rot (cos 1.2) (sin 1.2)))+      approx 0 . rotGetAngle =<< makeRot 0+      -- makeRot uses an approximate cos/sin, so its round-trip is loose+      r <- makeRot 1.2+      let a = rotGetAngle r+      assertBool ("~1.2 through makeRot, got " <> show a) (abs (a - 1.2) < 5e-3)+  , testCase "makeRot pi/2 is a quarter turn" $ do+      Rot c s <- makeRot (pi / 2)+      approx 0 c+      approx 1 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 @?= Vec2 0 (-10)+      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 @?= rotIdentity+      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 @?= 1+      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 "Circle round-trips" $ do+      let c = Circle (Vec2 1 2) 0.5+      roundTrip c >>= (@?= c)+  , testCase "Capsule round-trips" $ do+      let c = Capsule (Vec2 0 0) (Vec2 0 1) 0.25+      roundTrip c >>= (@?= c)+  , testCase "Segment round-trips" $ do+      let s = Segment (Vec2 (-1) 0) (Vec2 1 0)+      roundTrip s >>= (@?= s)+  , testCase "shape types case as patterns" $+      withWorld (Vec2 0 0) $ \w -> do+        bd <- defaultBodyDef+        b <- Body.create w bd+        sd <- defaultShapeDef+        sh <- Shape.createCircle b sd (Circle vec2Zero 0.5)+        ty <- Shape.getType sh+        let name = case ty of+              CircleShape -> "circle"+              CapsuleShape -> "capsule"+              SegmentShape -> "segment"+              PolygonShape -> "polygon"+              ChainSegmentShape -> "chain segment"+              _ -> "unknown"+        name @?= ("circle" :: String)+  , testCase "a warm SimplexCache round-trips its indices" $ do+      let cache = SimplexCache (VS.fromList [1, 2, 3]) (VS.fromList [4, 5, 6])+      roundTrip cache >>= (@?= cache)+  , testCase "a SimplexCache count follows its shorter index array" $+      roundTrip (SimplexCache (VS.fromList [1, 2]) (VS.fromList [3]))+        >>= (@?= SimplexCache (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 @?= 3+        VS.length cache.indexB @?= 3+  , testCase "Counters reads a live world with full-length arrays" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        _ <- dynamicCircle w (Pos 0 5) 0.5+        _ <- dynamicCircle w (Pos 3 5) 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+        assertBool "byte count is positive" (counters.byteCount > 0)+  ]++-- | Build a world with the given gravity and default settings.+withWorld :: Vec2 -> (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 circle at the given position.+dynamicCircle :: WorldId -> Pos -> Float -> IO BodyId+dynamicCircle w position radius = do+  bd <- defaultBodyDef+  b <- Body.create w bd{type_ = DynamicBody, position = position}+  sd <- defaultShapeDef+  _ <- Shape.createCircle b sd (Circle vec2Zero radius)+  pure b++createOpenChain :: BodyId -> VS.Vector Vec2 -> IO ChainId+createOpenChain body points = do+  cd <- Chain.defaultChainDef+  VS.unsafeWith points $ \pts ->+    Chain.create+      body+      cd+        { Chain.points = pts+        , Chain.pointCount = fromIntegral (VS.length points)+        , Chain.ghost1 = extrapolate (VS.head points) (points VS.! 1)+        , Chain.ghost2 = extrapolate (VS.last points) (points VS.! (VS.length points - 2))+        }+  where+    extrapolate end inner = vec2Sub (vec2Scale 2 end) inner++posY :: Pos -> Float+posY p = y+  where+    Vec2 _ y = toVec2 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 (Vec2 0 (-10)) $ \w -> do+        World.getGravity w >>= (@?= Vec2 0 (-10))+        World.setGravity w (Vec2 0 (-20))+        World.getGravity w >>= (@?= Vec2 0 (-20))+  , testCase "a body and its shape wire up correctly" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        bd <- defaultBodyDef+        b <- Body.create w bd{type_ = DynamicBody, position = Pos 0 10}+        Body.isValid b >>= (@?= True)+        Body.getType b >>= (@?= DynamicBody)+        Body.getPosition b >>= (@?= Pos 0 10)+        sd <- defaultShapeDef+        s <- Shape.createCircle b sd (Circle vec2Zero 0.5)+        Shape.isValid s >>= (@?= True)+        Shape.getType s >>= (@?= CircleShape)+        Shape.getBody s >>= (@?= b)+  , testCase "a dynamic body falls under gravity" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        b <- dynamicCircle w (Pos 0 10) 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)+        Vec2 _ vy <- Body.getLinearVelocity b+        assertBool ("velocity is downward, vy = " <> show vy) (vy < -8)+  , testCase "a dynamic body rests on a static ground segment" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        gd <- defaultBodyDef+        ground <- Body.create w gd+        gsd <- defaultShapeDef+        _ <- Shape.createSegment ground gsd (Segment (Vec2 (-20) 0) (Vec2 20 0))+        ball <- dynamicCircle w (Pos 0 5) 0.5+        replicateM_ 240 (World.step w (1 / 60) 4)+        y <- posY <$> Body.getPosition ball+        -- the ball comes to rest on top of the segment: y ~= 0.5+        assertBool ("ball rests on the ground, y = " <> show y) (y > 0.3 && y < 0.7)+  ]++{- | Exercise a spread of functions that only exist thanks to the generator:+Tier A scalar/bool accessors, a coerced enum, and the @b2AABB@-returning+shim (b2World_GetBounds), none of which the hand-written layer bound.+-}+generatedTests :: [TestTree]+generatedTests =+  [ testCase "body mass, density and awake flag are readable" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        b <- dynamicCircle w (Pos 0 10) 0.5+        m <- Body.getMass b+        assertBool ("mass is positive, m = " <> show m) (m > 0)+        Body.getGravityScale b >>= approx 1+        Body.setAwake b True+        Body.isAwake b >>= (@?= True)+  , testCase "shape density defaults to 1" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        bd <- defaultBodyDef+        b <- Body.create w bd{type_ = DynamicBody}+        sd <- defaultShapeDef+        shp <- Shape.createCircle b sd (Circle vec2Zero 0.5)+        Shape.getDensity shp >>= approx 1+        Shape.getType shp >>= (@?= CircleShape)+  , testCase "world reports awake bodies and finite bounds" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        _ <- dynamicCircle w (Pos 0 10) 0.5+        World.step w (1 / 60) 4+        n <- World.getAwakeBodyCount w+        assertBool ("at least one awake body, n = " <> show n) (n >= 1)+        World.isSleepingEnabled w >>= (@?= True)+        bounds <- World.getBounds w+        isValidAABB bounds >>= (@?= True)+  ]++{- | 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 (Vec2 0 (-10)) $ \w -> do+        gd <- defaultBodyDef+        anchor <- Body.create w gd{Body.position = Pos 0 10}+        bd <- defaultBodyDef+        hanging <- Body.create w bd{type_ = DynamicBody, position = Pos 0 8}+        sd <- defaultShapeDef+        _ <- Shape.createCircle hanging sd (Circle vec2Zero 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.getRestLength j >>= approx 2+        replicateM_ 120 (World.step w (1 / 60) 4)+        -- rigid length 2 below the anchor at (0, 10): the body 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 defaultPrismaticJointDef+      rt defaultRevoluteJointDef+      rt defaultWeldJointDef+      rt defaultWheelJointDef+      rt defaultFilterJointDef+  ]++{- | Exercise the hand-written event-buffer marshalling (roadmap phase 5): the+generated @World.getBodyEvents@ returns the raw buffer, "Box2D.Events" copies+the elements into a list.+-}+eventTests :: [TestTree]+eventTests =+  [ testCase "a moved body appears in the body move events" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        b <- dynamicCircle w (Pos 0 10) 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 (Vec2 0 (-10)) $ \w -> do+        gd <- defaultBodyDef+        sensorBody <- Body.create w gd+        gsd <- defaultShapeDef+        sensor <-+          Shape.createCircle+            sensorBody+            gsd{isSensor = 1, enableSensorEvents = 1}+            (Circle vec2Zero 1)+        bd <- defaultBodyDef+        visitor <- Body.create w bd{type_ = DynamicBody, position = Pos 0 4}+        sd <- defaultShapeDef+        _ <-+          Shape.createCircle+            visitor+            sd{enableSensorEvents = 1}+            (Circle vec2Zero 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 (Vec2 0 (-10)) $ \w -> do+        gd <- defaultBodyDef+        ground <- Body.create w gd+        gsd <- defaultShapeDef+        _ <-+          Shape.createSegment+            ground+            gsd{enableContactEvents = 1, enableHitEvents = 1}+            (Segment (Vec2 (-10) 0) (Vec2 10 0))+        bd <- defaultBodyDef+        ball <- Body.create w bd{type_ = DynamicBody, position = Pos 0 3}+        sd <- defaultShapeDef+        bshape <-+          Shape.createCircle+            ball+            sd{enableContactEvents = 1, enableHitEvents = 1}+            (Circle vec2Zero 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 (Vec2 0 (-10)) $ \w -> do+        gd <- defaultBodyDef+        anchor <- Body.create w gd{Body.position = Pos 0 10}+        bd <- defaultBodyDef+        hanging <- Body.create w bd{type_ = DynamicBody, position = Pos 0 8}+        sd <- defaultShapeDef+        _ <- Shape.createCircle hanging sd (Circle vec2Zero 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'++{- | Exercise the Polygon/Hull/ChainDef/ExplosionDef value APIs requested by+downstream (apecs-box2d): boxes, arbitrary hulls, chain terrain, explosions+and integer user data.+-}+polygonTests :: [TestTree]+polygonTests =+  [ testCase "makeBox round-trips and gives a unit box the right mass" $+      withWorld (Vec2 0 0) $ \w -> do+        box <- Collision.makeBox 0.5 0.5+        VS.length (box.vertices) @?= 4+        VS.length (box.normals) @?= 4+        roundTrip box >>= (@?= box)+        bd <- defaultBodyDef+        b <- Body.create w bd{type_ = DynamicBody}+        sd <- defaultShapeDef+        sh <- Shape.createPolygon b sd box+        Shape.getType sh >>= (@?= PolygonShape)+        -- a 1x1 box at density 1+        md <- Body.getMassData b+        approx 1 (md.mass)+  , testCase "computeHull and makePolygon build a triangle" $+      VS.unsafeWith (VS.fromList [Vec2 0 0, Vec2 1 0, Vec2 0 1]) $ \pts -> do+        hull <- Collision.computeHull pts 3+        VS.length (hull.points) @?= 3+        tri <- Collision.makePolygon hull 0+        VS.length (tri.vertices) @?= 3+        tri.radius @?= 0+  , testCase "a chain assembles from a point array" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        gd <- defaultBodyDef+        ground <- Body.create w gd+        c <- createOpenChain ground (VS.fromList [Vec2 (-6) 2, Vec2 (-4) 0, Vec2 0 0, Vec2 4 0, Vec2 6 2])+        Chain.isValid c >>= (@?= True)+        n <- Chain.getSegmentCount c+        assertBool ("chain has segments, n = " <> show n) (n > 0)+        Chain.destroy c+  , testCase "an explosion pushes a nearby body away" $+      withWorld (Vec2 0 0) $ \w -> do+        b <- dynamicCircle w (Pos 1 0) 0.5+        ed <- defaultExplosionDef+        World.explode+          w+          ed+            { position = Pos 0 0+            , radius = 2+            , impulsePerLength = 5+            }+        Vec2 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 (Vec2 0 0) $ \w -> do+        bd <- defaultBodyDef+        b <- Body.create w bd+        sd <- defaultShapeDef+        sh <- Shape.createCircle b sd (Circle vec2Zero 0.5)+        UD.setUserIndex b 42+        UD.getUserIndex b >>= (@?= 42)+        UD.setUserIndex sh maxBound+        UD.getUserIndex sh >>= (@?= maxBound)+        -- negative ids survive the pointer pun+        UD.setUserIndex b (-7)+        UD.getUserIndex b >>= (@?= (-7))+  ]++-- | A static body carrying a circle at the given position.+staticCircle :: WorldId -> Pos -> Float -> IO ShapeId+staticCircle w position radius = do+  bd <- defaultBodyDef+  b <- Body.create w bd{Body.position = position}+  sd <- defaultShapeDef+  Shape.createCircle b sd (Circle vec2Zero 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 (Vec2 0 0) $ \w -> do+        near <- staticCircle w (Pos 2 0) 0.5+        _far <- staticCircle w (Pos 4 0) 0.5+        qf <- defaultQueryFilter+        r <- World.castRayClosest w (Pos 0 0) (Vec2 10 0) qf+        r.hit @?= 1+        r.shapeId @?= near+        -- the near circle's surface is at x = 1.5, 15% along the ray+        approx 0.15 (r.fraction)+        miss <- World.castRayClosest w (Pos 0 0) (Vec2 (-10) 0) qf+        miss.hit @?= 0+  , testCase "body mass data matches the circle's area" $+      withWorld (Vec2 0 0) $ \w -> do+        b <- dynamicCircle w (Pos 0 0) 0.5+        md <- Body.getMassData b+        -- density 1 * pi r^2+        approx (pi * 0.25) (md.mass)+  , testCase "a local-frame ray cast hits a circle" $ do+      out <- Collision.castRayCircle (Circle vec2Zero 1) (RayCastInput (Vec2 (-3) 0) (Vec2 6 0) 1)+      out.hit @?= 1+      -- enters the unit circle at x = -1: a third of the way along (6, 0)+      approx (1 / 3) (out.fraction)+  ]++-- | The C custom-filter callback: @bool (b2ShapeId, b2ShapeId, void*)@.+type CustomFilterCb = ShapeId -> ShapeId -> Ptr () -> IO CBool++foreign import ccall "wrapper"+  mkCustomFilterCb :: CustomFilterCb -> IO (FunPtr CustomFilterCb)++{- | Exercise the raw-'FunPtr' callback passthrough: wrap a Haskell closure and+confirm the engine invokes it during the (safe) 'Box2D.World.step'.+-}+callbackTests :: [TestTree]+callbackTests =+  [ testCase "a custom-filter callback fires during the step" $+      withWorld (Vec2 0 0) $ \w -> do+        fired <- newIORef False+        cb <- mkCustomFilterCb $ \_ _ _ -> writeIORef fired True >> pure 1+        World.setCustomFilterCallback w (castFunPtr cb) nullPtr+        let overlappingCircle = do+              bd <- defaultBodyDef+              b <- Body.create w bd{type_ = DynamicBody}+              sd <- defaultShapeDef+              _ <- Shape.createCircle b sd{enableCustomFiltering = 1} (Circle vec2Zero 0.5)+              pure b+        _ <- overlappingCircle+        _ <- overlappingCircle+        replicateM_ 8 (World.step w (1 / 60) 4)+        -- unhook before freeing the wrapper so the world never dangles it+        World.setCustomFilterCallback w nullFunPtr nullPtr+        freeHaskellFunPtr cb+        readIORef fired >>= (@?= True)+  , testCase "overlap and cast-ray closures visit shapes (cast via the C trampoline)" $+      withWorld (Vec2 0 0) $ \w -> do+        _ <- staticCircle w (Pos 2 0) 0.5+        _ <- staticCircle w (Pos 4 0) 0.5+        _ <- staticCircle w (Pos 40 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 (Pos 0 0) (AABB (Vec2 0 (-1)) (Vec2 5 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 -> modifyIORef' hitsRef ((sid, point, normal, frac) :) >> pure 1)+            (\fcn ctx -> World.castRay w (Pos 0 0) (Vec2 10 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 _, Vec2 nx _, _) -> do+          assertBool ("hit point on a surface, x = " <> show px) (px > 1 && px < 4)+          approx (-1) nx+  , testCase "double mode keeps fractions of a metre far from the origin" $ do+      dp <- Base.isDoublePrecision+      when dp $ withWorld (Vec2 0 (-10)) $ \w -> do+        b <- dynamicCircle w (Pos 10000000.25 10) 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 "a pre-solve manifold edit disables contacts" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        gd <- defaultBodyDef+        ground <- Body.create w gd+        gsd <- defaultShapeDef+        _ <-+          Shape.createSegment+            ground+            gsd{enablePreSolveEvents = 1}+            (Segment (Vec2 (-10) 0) (Vec2 10 0))+        bd <- defaultBodyDef+        ball <- Body.create w bd{type_ = DynamicBody, position = Pos 0 2}+        sd <- defaultShapeDef+        _ <-+          Shape.createCircle+            ball+            sd{enablePreSolveEvents = 1}+            (Circle vec2Zero 0.5)+        fired <- newIORef (0 :: Int)+        let dropPoints _ _ pManifold = do+              modifyIORef' fired (+ 1)+              m <- peek pManifold+              poke pManifold (Manifold m.normal m.rollingImpulse VS.empty)+        CB.withPreSolveFcns (Just dropPoints) Nothing $ \solveFcn continuousFcn ctx -> do+          World.setPreSolveCallback w solveFcn continuousFcn ctx+          replicateM_ 240 (World.step w (1 / 60) 4)+          World.setPreSolveCallback w nullFunPtr 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 < 0)+  , testCase "friction and restitution mixing closures fire on contact" $+      withWorld (Vec2 0 (-10)) $ \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.createSegment ground gsd (Segment (Vec2 (-10) 0) (Vec2 10 0))+            _ <- dynamicCircle w (Pos 0 1) 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 (Vec2 0 0) $ \w -> do+        _ <- staticCircle w (Pos 0 0) 1+        qf <- defaultQueryFilter+        planes <- newIORef []+        CB.withPlaneResultFcn+          (\sid pr -> modifyIORef' planes ((sid, pr) :) >> pure True)+          (\fcn ctx -> World.collideMover w (Pos 0 0) (Capsule (Vec2 1.2 0) (Vec2 1.2 0.5) 0.4) qf fcn ctx)+        ps <- readIORef planes+        assertBool "at least one plane gathered" (not (null ps))+        assertBool "a plane registered a hit" (any ((== 1) . (.hit) . snd) ps)+  , testCase "dynamic-tree query and ray-cast closures visit proxies" $+      bracket (DynamicTree.create 16) DynamicTree.destroy $ \tree -> do+        p0 <- DynamicTree.createProxy tree (AABB (Vec2 0 0) (Vec2 1 1)) 1 42+        _ <- DynamicTree.createProxy tree (AABB (Vec2 3 0) (Vec2 4 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.overlapAABB tree (AABB (Vec2 (-0.5) 0.25) (Vec2 0.5 0.75)) maxBound fcn ctx+        readIORef seen >>= (@?= [(fromIntegral p0, 42)])+        -- a ray along y = 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.castRay tree (RayCastInput (Vec2 (-1) 0.5) (Vec2 6 0) 1) maxBound fcn ctx)+        readIORef hits >>= (@?= 2)+  , 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+        -- track how many callback invocations overlap, and widen the window a+        -- little so genuine parallelism actually shows up+        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 = Vec2 0 (-10)+                , enableSleep = 0+                , workerCount = fromIntegral ts.workerCount+                , enqueueTask = ts.enqueue+                , finishTask = ts.finish+                }+          World.setCustomFilterCallback w fcn nullPtr+          -- a static ground plus a 16x16 grid of custom-filtered circles: lots+          -- of broad-phase pairs churning through the parallel solver+          gd <- defaultBodyDef+          ground <- Body.create w gd+          gsd <- defaultShapeDef+          _ <- Shape.createSegment ground gsd (Segment (Vec2 (-40) 0) (Vec2 40 0))+          bd <- defaultBodyDef+          sd <- defaultShapeDef+          forM_ [0 .. 255 :: Int] $ \i -> do+            let+              col = fromIntegral (i `mod` 16)+              row = fromIntegral (i `div` 16)+              pos = Pos (col - 8) (1.5 + row * 1.1)+            b <- Body.create w bd{type_ = DynamicBody, position = pos}+            _ <- Shape.createCircle b sd{enableCustomFiltering = 1} (Circle vec2Zero 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 (Vec2 0 0) $ \w -> do+        _ <- dynamicCircle w (Pos 1 2) 0.5+        bd <- defaultBodyDef+        boxBody <- Body.create w bd{type_ = DynamicBody, position = Pos 5 2}+        sd <- defaultShapeDef+        boxShape <- Collision.makeBox 0.5 0.5+        _ <- Shape.createPolygon boxBody sd boxShape+        World.step w (1 / 60) 4+        circles <- newIORef []+        polys <- newIORef []+        boxes <- newIORef (0 :: Int)+        base <- defaultDebugDraw+        let draw =+              CB.noDraw+                { CB.solidCircle = Just $ \(WorldTransform p _) _ r c -> modifyIORef' circles ((p, r, c) :)+                , CB.solidPolygon = Just $ \_ vs _ c -> modifyIORef' polys ((VS.length vs, c) :)+                , CB.bounds = Just $ \_ _ -> modifyIORef' boxes (+ 1)+                }+        CB.withDraw draw base{drawBounds = 1} (CB.draw w)+        readIORef circles >>= \cs -> case cs of+          [(p, r, _)] -> do+            let Vec2 px py = toVec2 p+            r @?= 0.5+            approx 1 px+            approx 2 py+          _ -> assertFailure ("expected one circle, got " <> show (length cs))+        readIORef polys >>= \ps -> case ps of+          [(n, _)] -> n @?= 4+          _ -> assertFailure ("expected one polygon, got " <> show (length ps))+        readIORef boxes >>= \n ->+          assertBool ("shape bounds were drawn, n = " <> show n) (n >= 2)+  , testCase "an exception in a draw callback surfaces at draw, not at the callback" $+      withWorld (Vec2 0 0) $ \w -> do+        _ <- dynamicCircle w (Pos 1 2) 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.solidCircle = Just $ \_ _ _ _ -> readIORef frame >>= \i -> error ("boom" <> show i)+                , -- the engine keeps walking, but every later slot is skipped+                  CB.bounds = Just $ \_ _ -> modifyIORef' later (+ 1)+                }+        CB.withDraw draw base{drawBounds = 1} $ \target -> do+          CB.draw w target `catchErrorCall` \msg -> msg @?= "boom1"+          readIORef later >>= (@?= 0)+          -- the trap is cleared, so the next frame starts clean+          writeIORef frame 2+          CB.draw w target `catchErrorCall` \msg -> msg @?= "boom2"+        World.isValid w >>= (@?= True)+  , testCase "an unset slot never enters Haskell, so its exception cannot fire" $+      withWorld (Vec2 0 0) $ \w -> do+        _ <- dynamicCircle w (Pos 1 2) 0.5+        World.step w (1 / 60) 4+        seen <- newIORef (0 :: Int)+        base <- defaultDebugDraw+        -- bounds is left unset: the trampoline sees NULL and returns+        let draw = CB.noDraw{CB.solidCircle = Just $ \_ _ _ _ -> modifyIORef' seen (+ 1)}+        CB.withDraw draw base{drawBounds = 1} (CB.draw w)+        readIORef seen >>= (@?= 1)+  , testCase "withRecorder collects a frame for rendering after draw returns" $+      withWorld (Vec2 0 0) $ \w -> do+        b <- dynamicCircle w (Pos 1 2) 0.5+        World.step w (1 / 60) 4+        base <- defaultDebugDraw+        ((), cmds) <- CB.withRecorder base{drawBounds = 1} (CB.draw w)+        let circles = [(xf, r) | CB.DrawSolidCircle xf _ r _ <- cmds]+        case circles of+          [(WorldTransform p _, r)] -> do+            r @?= 0.5+            let Vec2 px py = toVec2 p+            approx 1 px+            approx 2 py+          _ -> assertFailure ("expected one circle, got " <> show (length circles))+        assertBool "bounds were recorded too" (any isBounds 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+      let+        xf = WorldTransform (Pos 10 20) (Rot 0 1) -- a quarter turn+        Vec2 x y = toVec2 (CB.toWorld xf (Vec2 1 0))+      approx 10 x+      approx 21 y+  ]++isBounds :: CB.Cmd -> Bool+isBounds (CB.DrawBounds _ _) = True+isBounds _ = 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"++-- | 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)++moverTests :: [TestTree]+moverTests =+  [ testCase "makeProxy fills a peekable ShapeProxy and withShapeProxy pokes one back" $ do+      let points = VS.fromList [Vec2 0 0, Vec2 1 0, Vec2 0 1]+      proxy <- VS.unsafeWith points $ \pts -> Collision.makeProxy pts (VS.length points) 0.25+      proxy.points @?= points+      proxy.radius @?= 0.25+      roundTrip proxy >>= (@?= proxy)+      withShapeProxy points 0.25 $ \p -> peek p >>= (@?= proxy)+  , testCase "a begun contact exposes its manifold through Contact.getData" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        gd <- defaultBodyDef+        ground <- Body.create w gd+        gsd <- defaultShapeDef+        _ <- Shape.createCircle ground gsd{enableContactEvents = 1} (Circle vec2Zero 5)+        bd <- defaultBodyDef+        ball <- Body.create w bd{type_ = DynamicBody, position = Pos 0 5.6}+        sd <- defaultShapeDef+        bshape <- Shape.createCircle ball sd{enableContactEvents = 1} (Circle vec2Zero 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)+        let m = cd.manifold+        assertBool "the manifold has a contact point" (not (VS.null (m.points)))+        let Vec2 _ 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 (Vec2 0 1) 0) (1 / 0) 0 1+      (delta, planes', iters) <- Mover.solvePlanes (Vec2 0 (-1)) (VS.singleton plane)+      let Vec2 _ 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 (Vec2 3 (-5)) planes'+      clipped @?= Vec2 3 0+  , testCase "chain contact and hit events toggle across all segments" $+      withWorld (Vec2 0 (-10)) $ \w -> do+        gd <- defaultBodyDef+        ground <- Body.create w gd+        c <- createOpenChain ground (VS.fromList [Vec2 (-6) 2, Vec2 (-4) 0, Vec2 4 0, Vec2 6 2])+        n <- Chain.getSegmentCount c+        segments <- allocaArray n $ \buf -> do+          got <- Chain.getSegments c buf n+          peekArray got buf+        assertBool "the chain has segments" (not (null segments))+        mapM_ (\s -> Shape.areContactEventsEnabled s >>= (@?= False)) segments+        Chain.enableContactEvents c True+        Chain.enableHitEvents c True+        mapM_ (\s -> Shape.areContactEventsEnabled s >>= (@?= True)) segments+        mapM_ (\s -> Shape.areHitEventsEnabled s >>= (@?= True)) segments+  , testCase "the default length scale is one unit per meter" $+      getLengthUnitsPerMeter >>= (@?= 1)+  , testCase "the pure vector helpers follow the engine conventions" $ do+      vec2Add (Vec2 1 2) (Vec2 3 4) @?= Vec2 4 6+      vec2MulSub (Vec2 1 1) 2 (Vec2 0 1) @?= Vec2 1 (-1)+      vec2Cross (Vec2 1 0) (Vec2 0 1) @?= 1+      vec2Normalize (Vec2 3 4) @?= Vec2 0.6 0.8+      vec2Normalize vec2Zero @?= vec2Zero+      rotateVector rotIdentity (Vec2 5 6) @?= Vec2 5 6+      invRotateVector (rotMul rotIdentity (Rot 0 1)) (rotateVector (Rot 0 1) (Vec2 5 6)) @?= Vec2 5 6+      planeSeparation (Plane (Vec2 0 1) 2) (Vec2 0 5) @?= 3+  ]
+ upstream/LICENSE view
@@ -0,0 +1,21 @@+MIT License++Copyright (c) 2022 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/box2d/base.h view
@@ -0,0 +1,201 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include <stdbool.h>+#include <stddef.h>+#include <stdint.h>++// Compile-time options. Edit box2d/config.h, or define BOX2D_USER_CONFIG to+// point at your own copy.+#ifdef BOX2D_USER_CONFIG+#include BOX2D_USER_CONFIG+#endif+#include "config.h"++/// Used to indicate an unset or invalid index value.+/// @ingroup base+#define B2_NULL_INDEX ( -1 )++// clang-format off+// +// Shared library macros+// Predefine BOX2D_EXPORT to reuse an existing export/import scheme, for example+// when compiling Box2D into another shared library.+#ifndef BOX2D_EXPORT+#if defined( _WIN32 ) && defined( box2d_EXPORTS )+	// build the Windows DLL+	#define BOX2D_EXPORT __declspec( dllexport )+#elif defined( _WIN32 ) && defined( BOX2D_DLL )+	// using the Windows DLL+	#define BOX2D_EXPORT __declspec( dllimport )+#elif defined( box2d_EXPORTS )+	// building or using the shared library+	#define BOX2D_EXPORT __attribute__( ( visibility( "default" ) ) )+#else+	// static library+	#define BOX2D_EXPORT+#endif+#endif++// C++ macros+#ifdef __cplusplus+	#define B2_API extern "C" BOX2D_EXPORT+	#define B2_INLINE inline+	#define B2_ALIGN_AS(N) alignas(N)+#if defined( _MSC_VER )+	#define B2_FORCE_INLINE __forceinline+#elif defined( __GNUC__ ) || defined( __clang__ )+	#define B2_FORCE_INLINE inline __attribute__((always_inline))+#else+	#define B2_FORCE_INLINE inline+#endif++	#define B2_LITERAL(T) T+	#define B2_ZERO_INIT {}+#else+	#define B2_API BOX2D_EXPORT+	#define B2_INLINE static inline+	#define B2_ALIGN_AS(N) _Alignas(N)++#if defined( _MSC_VER )+	#define B2_FORCE_INLINE static __forceinline+#elif defined( __GNUC__ ) || defined( __clang__ )+	#define B2_FORCE_INLINE static inline __attribute__((always_inline))+#else+	#define B2_FORCE_INLINE static inline+#endif++	// Used for C literals like (b2Vec2){1.0f, 2.0f} where C++ requires b2Vec2{1.0f, 2.0f}+	#define B2_LITERAL(T) (T)+	#define B2_ZERO_INIT {0}+#endif+// clang-format on++#if defined( BOX2D_VALIDATE ) && !defined( NDEBUG )+#define B2_ENABLE_VALIDATION 1+#else+#define B2_ENABLE_VALIDATION 0+#endif++/**+ * @defgroup base Base+ * Base functionality+ * @{+ */++/// 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* b2AllocCallbackFcn( size_t size, int alignment );++/// Prototype for user free function+/// @param mem the memory previously allocated through `b2AllocCallbackFcn`+/// @param size the allocation size in bytes+typedef void b2FreeCallbackFcn( void* mem, size_t size );++/// Prototype for the user assert callback. Return 0 to skip the debugger break.+typedef int b2AssertCallbackFcn( const char* condition, const char* fileName, int lineNumber );++/// Prototype for user log callback. Used to log warnings.+typedef void b2LogCallbackFcn( const char* message );++/// This allows the user to override the allocation functions. These should be set during+/// application startup.+B2_API void b2SetAllocator( b2AllocCallbackFcn* allocFcn, b2FreeCallbackFcn* freeFcn );++/// @return the total bytes allocated by Box2D+B2_API size_t b2GetByteCount( void );++/// Override the default assert function+/// @param assertFcn a non-null assert callback+B2_API void b2SetAssertCallback( b2AssertCallbackFcn* assertFcn );++/// Override the default log function+/// @param logFcn a non-null log callback+B2_API void b2SetLogCallback( b2LogCallbackFcn* logFcn );++/// Version numbering scheme.+/// See https://semver.org/+typedef struct b2Version+{+	/// Significant changes+	int major;++	/// Incremental changes+	int minor;++	/// Bug fixes+	int revision;+} b2Version;++/// Significant API and behavior changes.+#define B2_VERSION_MAJOR 3++/// API changes and features.+#define B2_VERSION_MINOR 2++/// Bug fixes. No API changes.+#define B2_VERSION_REVISION 0++/// Get the current version of Box2D+B2_API b2Version b2GetVersion( void );++/// @return true if the library was built with BOX2D_DOUBLE_PRECISION (large world mode)+B2_API bool b2IsDoublePrecision( void );++/// @return true if the current CPU has the AVX2 instruction set.+B2_API bool b2IsAVX2Available( void );++/**@}*/++//! @cond++// see https://github.com/scottt/debugbreak+#if defined( _MSC_VER )+#define B2_BREAKPOINT __debugbreak()+#elif defined( __GNUC__ ) || defined( __clang__ )+#define B2_BREAKPOINT __builtin_trap()+#else+// Unknown compiler+#include <assert.h>+#define B2_BREAKPOINT assert( 0 )+#endif++#if !defined( NDEBUG ) || defined( B2_ENABLE_ASSERT )+B2_API int b2InternalAssert( const char* condition, const char* fileName, int lineNumber );+#define B2_ASSERT( condition )                                                                     \+	( (void)( ( !!( condition ) ) ||                                                               \+			  ( b2InternalAssert( #condition, __FILE__, (int)( __LINE__ ) ), 0 ) ) )+#else+#define B2_ASSERT( ... ) ( (void)0 )+#endif++// Validation is used in debug builds+#if B2_ENABLE_VALIDATION+#define B2_VALIDATE( condition ) B2_ASSERT( condition )+#else+#define B2_VALIDATE( ... ) ( (void)0 )+#endif++//! @endcond++/// Get the absolute number of system ticks. The value is platform specific.+B2_API uint64_t b2GetTicks( void );++/// Get the milliseconds passed from an initial tick value.+B2_API float b2GetMilliseconds( uint64_t ticks );++/// Get the milliseconds passed from an initial tick value. Resets the passed in value to the+/// current tick value.+B2_API float b2GetMillisecondsAndReset( uint64_t* ticks );++/// Yield to be used in a busy loop.+B2_API void b2Yield( void );++/// Starting hash value.+#define B2_HASH_INIT 5381++/// Simple djb2 hash function for determinism testing+B2_API uint32_t b2Hash( uint32_t hash, const uint8_t* data, int count );
+ upstream/include/box2d/box2d.h view
@@ -0,0 +1,1697 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "base.h"+#include "collision.h"+#include "constants.h"+#include "id.h"+#include "types.h"++#include <stdbool.h>++#if defined( BOX2D_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 b2CreateWorld b2CreateWorldDoublePrecision+#endif++/**+ * @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/+ * @{+ */++/// Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You+/// may create up to B2_MAX_WORLDS worlds. Each world is completely independent and may be simulated+/// in parallel.+/// @return the world id.+B2_API b2WorldId b2CreateWorld( const b2WorldDef* def );++/// Destroy a world+B2_API void b2DestroyWorld( b2WorldId worldId );++/// World id validation. Provides validation for up to 64K allocations.+B2_API bool b2World_IsValid( b2WorldId 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.+B2_API void b2World_Step( b2WorldId worldId, float timeStep, int subStepCount );++/// Call this to draw shapes and other debug draw data+B2_API void b2World_Draw( b2WorldId worldId, b2DebugDraw* draw );++/// Call this to get the world bounds based on the union of all shape bounds.+B2_API b2AABB b2World_GetBounds( b2WorldId worldId );++/// Get the body events for the current time step. The event data is transient. Do not store a+/// reference to this data.+B2_API b2BodyEvents b2World_GetBodyEvents( b2WorldId worldId );++/// Get sensor events for the current time step. The event data is transient. Do not store a+/// reference to this data.+B2_API b2SensorEvents b2World_GetSensorEvents( b2WorldId worldId );++/// Get contact events for the current time step. The event data is transient. Do not store a+/// reference to this data.+B2_API b2ContactEvents b2World_GetContactEvents( b2WorldId worldId );++/// Get the joint events for the current time step. The event data is transient. Do not store a+/// reference to this data.+B2_API b2JointEvents b2World_GetJointEvents( b2WorldId worldId );++/// Overlap test for all shapes that *potentially* overlap the provided AABB. The AABB is relative+/// to the origin, which keeps the test precise far from the world origin in large world mode. Near+/// the origin pass b2Pos_zero and a world AABB.+B2_API b2TreeStats b2World_OverlapAABB( b2WorldId worldId, b2Pos origin, b2AABB aabb,+										b2QueryFilter filter, b2OverlapResultCallbackFcn* fcn,+										void* context );++/// Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative+/// to the origin. Near the origin pass b2Pos_zero and world points.+B2_API b2TreeStats b2World_OverlapShape( b2WorldId worldId, b2Pos origin, const b2ShapeProxy* proxy,+										 b2QueryFilter filter, b2OverlapResultCallbackFcn* 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+B2_API b2TreeStats b2World_CastRay( b2WorldId worldId, b2Pos origin, b2Vec2 translation,+									b2QueryFilter filter, b2CastResultCallbackFcn* 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 b2World_CastRay() and does not allow for custom+/// filtering.+B2_API b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Pos origin, b2Vec2 translation,+										   b2QueryFilter filter );++/// Cast a shape through the world. Similar to a ray cast except that a shape is cast instead of a+/// point. The proxy points are relative to the origin. Callback points are world positions.+/// @see b2World_CastRay+B2_API b2TreeStats b2World_CastShape( b2WorldId worldId, b2Pos origin, const b2ShapeProxy* proxy,+									  b2Vec2 translation, b2QueryFilter filter,+									  b2CastResultCallbackFcn* 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. The mover capsule is relative to the origin. Near the+/// origin pass b2Pos_zero and a world capsule. Returns the translation fraction at the first hit.+B2_API float b2World_CastMover( b2WorldId worldId, b2Pos origin, const b2Capsule* mover,+								b2Vec2 translation, b2QueryFilter filter );++/// Collide a capsule mover with the world, gathering collision planes that can be fed to+/// b2SolvePlanes. Useful for kinematic character movement. The mover capsule and the resulting+/// planes are relative to the origin.+B2_API void b2World_CollideMover( b2WorldId worldId, b2Pos origin, const b2Capsule* mover,+								  b2QueryFilter filter, b2PlaneResultCallbackFcn* 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 b2WorldDef+B2_API void b2World_EnableSleeping( b2WorldId worldId, bool flag );++/// Is body sleeping enabled?+B2_API bool b2World_IsSleepingEnabled( b2WorldId 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 b2WorldDef+B2_API void b2World_EnableContinuous( b2WorldId worldId, bool flag );++/// Is continuous collision enabled?+B2_API bool b2World_IsContinuousEnabled( b2WorldId 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 b2WorldDef+B2_API void b2World_SetRestitutionThreshold( b2WorldId worldId, float value );++/// Get the restitution speed threshold. Usually in meters per second.+B2_API float b2World_GetRestitutionThreshold( b2WorldId worldId );++/// Set the number of iterations used by the restitution solver.+/// @see B2_MAX_RESTITUTION_ITERATIONS+B2_API void b2World_SetRestitutionIterations( b2WorldId worldId, int iterations );++/// Get the number of iterations used by the restitution solver.+B2_API int b2World_GetRestitutionIterations( b2WorldId worldId );++/// Enable/disable restitution propagation.+/// @see b2WorldDef::enableRestitutionPropagation+B2_API void b2World_EnableRestitutionPropagation( b2WorldId worldId, bool flag );++/// Is restitution propagation enabled?+B2_API bool b2World_IsRestitutionPropagationEnabled( b2WorldId worldId );++/// Adjust the hit event threshold. This controls the collision speed needed to generate a+/// b2ContactHitEvent. Usually in meters per second.+/// @see b2WorldDef::hitEventThreshold+B2_API void b2World_SetHitEventThreshold( b2WorldId worldId, float value );++/// Get the hit event speed threshold. Usually in meters per second.+B2_API float b2World_GetHitEventThreshold( b2WorldId worldId );++/// Register the custom filter callback. This is optional.+B2_API void b2World_SetCustomFilterCallback( b2WorldId worldId, b2CustomFilterCallbackFcn* fcn,+											 void* context );++/// Register the pre-solve and pre-continuous callbacks. This is optional.+B2_API void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveCallbackFcn* preSolveFcn,+										 b2PreContinuousCallbackFcn* preContinuousFcn,+										 void* context );++/// Set the gravity vector for the entire world. Box2D has no concept of an up direction and this is+/// left as a decision for the application. Usually in m/s^2.+/// @see b2WorldDef+B2_API void b2World_SetGravity( b2WorldId worldId, b2Vec2 gravity );++/// Get the gravity vector+B2_API b2Vec2 b2World_GetGravity( b2WorldId worldId );++/// Apply a radial explosion+/// @param worldId The world id+/// @param explosionDef The explosion definition+B2_API void b2World_Explode( b2WorldId worldId, const b2ExplosionDef* 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 pushSpeed The maximum contact constraint push out speed (meters per second)+/// @note Advanced feature+B2_API void b2World_SetContactTuning( b2WorldId worldId, float hertz, float dampingRatio,+									  float pushSpeed );++/// Get the contact tuning.+B2_API void b2World_GetContactTuning( b2WorldId worldId, float* hertz, float* dampingRatio,+									  float* pushSpeed );++/// Set the contact point recycling distance. Setting this to zero disables contact point recycling.+/// Usually in meters.+B2_API void b2World_SetContactRecycleDistance( b2WorldId worldId, float recycleDistance );++/// Get the contact point recycling distance. Usually in meters.+B2_API float b2World_GetContactRecycleDistance( b2WorldId worldId );++/// Set the maximum linear speed. Usually in m/s.+B2_API void b2World_SetMaxLinearSpeed( b2WorldId worldId, float maximumLinearSpeed );++/// Get the maximum linear speed. Usually in m/s.+B2_API float b2World_GetMaxLinearSpeed( b2WorldId worldId );++/// Enable/disable constraint warm starting. Advanced feature for testing. Disabling warm starting+/// greatly reduces stability and provides no performance gain.+B2_API void b2World_EnableWarmStarting( b2WorldId worldId, bool flag );++/// Is constraint warm starting enabled?+B2_API bool b2World_IsWarmStartingEnabled( b2WorldId worldId );++/// Force SSE2 to be used even when AVX2 is available. This is for testing.+B2_API void b2World_EnableSSE2Fallback( b2WorldId worldId, bool flag );++/// Get the number of awake bodies.+B2_API int b2World_GetAwakeBodyCount( b2WorldId worldId );++/// Get the current world performance profile+B2_API b2Profile b2World_GetProfile( b2WorldId worldId );++/// Get world counters and sizes+B2_API b2Counters b2World_GetCounters( b2WorldId worldId );++/// Get max capacity. This can be used with b2WorldDef to avoid run-time allocations and copies+B2_API b2Capacity b2World_GetMaxCapacity( b2WorldId worldId );++/// Set the user data pointer.+B2_API void b2World_SetUserData( b2WorldId worldId, void* userData );++/// Get the user data pointer.+B2_API void* b2World_GetUserData( b2WorldId worldId );++/// Set the friction callback. Passing NULL resets to default.+B2_API void b2World_SetFrictionCallback( b2WorldId worldId, b2FrictionCallbackFcn* callback );++/// Set the restitution callback. Passing NULL resets to default.+B2_API void b2World_SetRestitutionCallback( b2WorldId worldId, b2RestitutionCallbackFcn* callback );++/// Set the worker count. Must be in the range [1, B2_MAX_WORKERS]+B2_API void b2World_SetWorkerCount( b2WorldId worldId, int count );++/// Get the worker count.+B2_API int b2World_GetWorkerCount( b2WorldId worldId );++/// Dump memory stats to box2d_memory.txt+B2_API void b2World_DumpMemoryStats( b2WorldId worldId );++/// This is for internal testing+B2_API void b2World_RebuildStaticTree( b2WorldId worldId );++/// Compute a hash of the simulation state. Use this to detect desyncs, for example in rollback+/// resimulation.+B2_API uint64_t b2World_GetStateHash( b2WorldId worldId );++/** @} */++/**+ * @defgroup recording Recording+ * These functions allow you to record a simulation for later playback+ * @{+ */++/// Opaque handle for a recording buffer. Create one, hand it to b2World_StartRecording, then save+/// its bytes and destroy it. The buffer grows as the world records into it.+typedef struct b2Recording b2Recording;++/// Create a recording buffer. The buffer grows on demand. @p byteCapacity pre-sizes it to avoid+/// reallocations during a known-length session. Pass 0 for a small default.+/// @return A recording handle, freed with b2DestroyRecording+B2_API b2Recording* b2CreateRecording( int byteCapacity );++/// Destroy a recording buffer and free its memory.+B2_API void b2DestroyRecording( b2Recording* recording );++/// Get a pointer to the recorded bytes, for saving to a file or transmitting. Valid until the next+/// recording call or b2DestroyRecording.+B2_API const uint8_t* b2Recording_GetData( const b2Recording* recording );++/// Get the number of recorded bytes.+B2_API int b2Recording_GetSize( const b2Recording* recording );++/// Begin recording the world into @p recording. Serializes a snapshot of the current world as the+/// seed, then logs every mutating call. Call at a step boundary, before or after creating bodies.+/// Start before the first step to capture the whole session. No effect if already recording or if+/// the world is locked.+/// @param worldId The world to record+/// @param recording A recording buffer from b2CreateRecording+B2_API void b2World_StartRecording( b2WorldId worldId, b2Recording* recording );++/// Stop recording. The recording buffer keeps its bytes. Save and destroy it yourself.+/// @param worldId The world being recorded+B2_API void b2World_StopRecording( b2WorldId worldId );++/// Save a recording buffer to a file. Convenience wrapper over your own file I/O.+/// @return false if the file could not be written+B2_API bool b2Recording_SaveToFile( const b2Recording* recording, const char* path );++/// Load a recording from a file into a new recording buffer. Convenience wrapper over your own file+/// I/O. Destroy the result with b2DestroyRecording.+/// @return NULL if the file is missing or unreadable+B2_API b2Recording* b2CreateRecordingFromFile( const char* path );++/** @} */++/**+ * @defgroup snapshot Snapshot+ * Save and restore the full simulation state of a world as a byte image. Unlike a recording, which+ * is a stream of API events, a snapshot is a single instant. Useful for rollback, undo, and+ * deterministic save games. Call between world steps.+ *+ * A snapshot carries only simulation state. Application hooks are not included. So callbacks and+ * user data are not preserved. However, if you apply a snapshot to an existing world then the+ * callbacks stay.+ *+ * All the ids remain valid when a world is restored in place: b2BodyId, b2ShapeId, b2JointId,+ * b2ChainId, and b2ContactId.+ *+ * @warning Experimental. The snapshot API and image format will change in future versions.+ * @{+ */++/// Write a snapshot of the world's simulation state into a caller-owned buffer. Call once with+/// image == NULL to get the required size, then again with a buffer of at least that size. Must be+/// called at a step boundary.+/// @param worldId The world to snapshot+/// @param image Destination buffer, or NULL to query the size+/// @param capacity Size of image in bytes, ignored when querying+/// @return The number of bytes the snapshot needs. If it exceeds capacity nothing is written.+///         Returns 0 if the world is mid-step.+B2_API int b2World_GetSnapshot( b2WorldId worldId, uint8_t* image, int capacity );++/// Restore a world's simulation state from a snapshot image. The b2WorldId and ids held from this+/// same world stay valid for objects that existed at the snapshot instant. Callbacks and threading+/// configuration are preserved. User data on shapes, bodies, and joints is nulled.+/// @param worldId The world to restore into+/// @param image A snapshot image produced by b2World_GetSnapshot+/// @param size Size of image in bytes+/// @return true on success. On a rejected image (bad magic, version, or layout) the world is left+///         unchanged. A corrupt payload detected after the rebuild begins returns false and leaves+///         the world unusable, so the caller must destroy it.+B2_API bool b2World_RestoreFromSnapshot( b2WorldId worldId, const uint8_t* image, int size );++/// Create a new world from a snapshot image. This is mainly useful for replay. Callbacks are reset+/// to default.+/// @param image A snapshot image produced by b2World_GetSnapshot+/// @param size Size of image in bytes+/// @param workerCount Worker count for the new world. 0 uses the serial single-worker fallback.+/// @return The new world id, or b2_nullWorldId on failure.+B2_API b2WorldId b2CreateWorldFromSnapshot( const uint8_t* image, int size, int workerCount );++/** @} */++/**+ * @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}+/// b2BodyDef bodyDef = b2DefaultBodyDef();+/// b2BodyId myBodyId = b2CreateBody(myWorldId, &bodyDef);+/// @endcode+/// @warning This function is locked during callbacks.+B2_API b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* 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.+B2_API void b2DestroyBody( b2BodyId 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.+B2_API bool b2Body_IsValid( b2BodyId id );++/// Get the body type: static, kinematic, or dynamic+B2_API b2BodyType b2Body_GetType( b2BodyId bodyId );++/// Change the body type. This is an expensive operation. This automatically updates the mass+/// properties regardless of the automatic mass setting.+B2_API void b2Body_SetType( b2BodyId bodyId, b2BodyType type );++/// Set the body name. See B2_NAME_LENGTH.+B2_API void b2Body_SetName( b2BodyId bodyId, const char* name );++/// Get the body name.+B2_API const char* b2Body_GetName( b2BodyId bodyId );++/// Set the user data for a body+B2_API void b2Body_SetUserData( b2BodyId bodyId, void* userData );++/// Get the user data stored in a body+B2_API void* b2Body_GetUserData( b2BodyId bodyId );++/// Get the world position of a body. This is the location of the body origin.+B2_API b2Pos b2Body_GetPosition( b2BodyId bodyId );++/// Get the world rotation of a body as a cosine/sine pair (complex number)+B2_API b2Rot b2Body_GetRotation( b2BodyId bodyId );++/// Get the world transform of a body.+B2_API b2WorldTransform b2Body_GetTransform( b2BodyId 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 b2BodyDef::position and b2BodyDef::rotation+B2_API void b2Body_SetTransform( b2BodyId bodyId, b2Pos position, b2Rot rotation );++/// Get a local point on a body given a world point+B2_API b2Vec2 b2Body_GetLocalPoint( b2BodyId bodyId, b2Pos worldPoint );++/// Get a world point on a body given a local point+B2_API b2Pos b2Body_GetWorldPoint( b2BodyId bodyId, b2Vec2 localPoint );++/// Get a local vector on a body given a world vector+B2_API b2Vec2 b2Body_GetLocalVector( b2BodyId bodyId, b2Vec2 worldVector );++/// Get a world vector on a body given a local vector+B2_API b2Vec2 b2Body_GetWorldVector( b2BodyId bodyId, b2Vec2 localVector );++/// Get the linear velocity of a body's center of mass. Usually in meters per second.+B2_API b2Vec2 b2Body_GetLinearVelocity( b2BodyId bodyId );++/// Get the angular velocity of a body in radians per second+B2_API float b2Body_GetAngularVelocity( b2BodyId bodyId );++/// Set the linear velocity of a body. Usually in meters per second.+B2_API void b2Body_SetLinearVelocity( b2BodyId bodyId, b2Vec2 linearVelocity );++/// Set the angular velocity of a body in radians per second+B2_API void b2Body_SetAngularVelocity( b2BodyId bodyId, float angularVelocity );++/// Set the velocity to reach the given transform after a given time step. The result will be close+/// but may not be exact. This is meant for kinematic bodies. The target is not applied if the+/// velocity would be below the sleep threshold and the body is currently asleep.+/// @param bodyId The body id+/// @param target The target transform for the body+/// @param timeStep The time step of the next call to b2World_Step+/// @param wake Option to wake the body or not+B2_API void b2Body_SetTargetTransform( b2BodyId bodyId, b2WorldTransform target, float timeStep,+									   bool wake );++/// Get the linear velocity of a local point attached to a body. Usually in meters per second.+B2_API b2Vec2 b2Body_GetLocalPointVelocity( b2BodyId bodyId, b2Vec2 localPoint );++/// Get the linear velocity of a world point attached to a body. Usually in meters per second.+B2_API b2Vec2 b2Body_GetWorldPointVelocity( b2BodyId bodyId, b2Pos 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+B2_API void b2Body_ApplyForce( b2BodyId bodyId, b2Vec2 force, b2Pos 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+B2_API void b2Body_ApplyForceToCenter( b2BodyId bodyId, b2Vec2 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 about the z-axis (out of the screen), usually in N*m.+/// @param wake also wake up the body+B2_API void b2Body_ApplyTorque( b2BodyId bodyId, float torque, bool wake );++/// Clear the force and torque on this body. Forces and torques are automatically cleared after each+/// world step. So this only needs to be called if the application wants to remove the effect of+/// previous calls to apply forces and torques before the world step is called.+/// @param bodyId The body id+B2_API void b2Body_ClearForces( b2BodyId bodyId );++/// 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.+B2_API void b2Body_ApplyLinearImpulse( b2BodyId bodyId, b2Vec2 impulse, b2Pos 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.+B2_API void b2Body_ApplyLinearImpulseToCenter( b2BodyId bodyId, b2Vec2 impulse, bool wake );++/// Apply an angular impulse. The impulse is ignored if the body is not awake. This optionally wakes+/// the body.+/// @param bodyId The body id+/// @param impulse the angular impulse, 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.+B2_API void b2Body_ApplyAngularImpulse( b2BodyId bodyId, float impulse, bool wake );++/// Get the mass of the body, usually in kilograms+B2_API float b2Body_GetMass( b2BodyId bodyId );++/// Get the rotational inertia of the body, usually in kg*m^2+B2_API float b2Body_GetRotationalInertia( b2BodyId bodyId );++/// Get the center of mass position of the body in local space.+B2_API b2Vec2 b2Body_GetLocalCenter( b2BodyId bodyId );++/// Get the center of mass position of the body in world space.+B2_API b2Pos b2Body_GetWorldCenter( b2BodyId bodyId );++/// Override the body's mass properties. Normally this is computed automatically using the shape+/// geometry and density. This information is lost if a shape is added or removed or if the body+/// type changes. If motion locks are used to fix rotation then the rotational inertia value is+/// ignored and rotational inertia remains fixed at zero.+B2_API void b2Body_SetMassData( b2BodyId bodyId, b2MassData massData );++/// Get the mass data for a body+B2_API b2MassData b2Body_GetMassData( b2BodyId 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 b2Body_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. Note that sensor shapes may have mass.+B2_API void b2Body_UpdateMassFromShapes( b2BodyId bodyId );++/// Adjust the linear damping. Normally this is set in b2BodyDef before creation.+B2_API void b2Body_SetLinearDamping( b2BodyId bodyId, float linearDamping );++/// Get the current linear damping.+B2_API float b2Body_GetLinearDamping( b2BodyId bodyId );++/// Adjust the angular damping. Normally this is set in b2BodyDef before creation.+B2_API void b2Body_SetAngularDamping( b2BodyId bodyId, float angularDamping );++/// Get the current angular damping.+B2_API float b2Body_GetAngularDamping( b2BodyId bodyId );++/// Adjust the gravity scale. Normally this is set in b2BodyDef before creation.+/// @see b2BodyDef::gravityScale+B2_API void b2Body_SetGravityScale( b2BodyId bodyId, float gravityScale );++/// Get the current gravity scale+B2_API float b2Body_GetGravityScale( b2BodyId bodyId );++/// @return true if this body is awake+B2_API bool b2Body_IsAwake( b2BodyId 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.+B2_API void b2Body_SetAwake( b2BodyId bodyId, bool awake );++/// Wake bodies touching this body. Works for static bodies.+B2_API void b2Body_WakeTouching( b2BodyId bodyId );++/// Enable or disable sleeping for this body. If sleeping is disabled the body will wake (and the+/// entire island).+B2_API void b2Body_EnableSleeping( b2BodyId bodyId, bool enableSleep );++/// Returns true if sleeping is enabled for this body+B2_API bool b2Body_IsSleepingEnabled( b2BodyId bodyId );++/// Set the sleep threshold, usually in meters per second+B2_API void b2Body_SetSleepThreshold( b2BodyId bodyId, float sleepThreshold );++/// Get the sleep threshold, usually in meters per second.+B2_API float b2Body_GetSleepThreshold( b2BodyId bodyId );++/// Set the continuous collision safety factor. Smaller is safer but can lead to hitching.+/// Recommended range [0.01, 0.5]. Non-dimensional.+B2_API void b2Body_SetSafetyFactor( b2BodyId bodyId, float safetyFactor );++/// Get the continuous collision safety factor. Non-dimensional.+B2_API float b2Body_GetSafetyFactor( b2BodyId bodyId );++/// Returns true if this body is enabled+B2_API bool b2Body_IsEnabled( b2BodyId bodyId );++/// Disable a body by removing it completely from the simulation. This is expensive.+B2_API void b2Body_Disable( b2BodyId bodyId );++/// Enable a body by adding it to the simulation. This is expensive.+B2_API void b2Body_Enable( b2BodyId bodyId );++/// Set the motion locks on this body. This causes mass properties to be recomputed because fixed+/// rotation sets the rotational inertia to zero.+B2_API void b2Body_SetMotionLocks( b2BodyId bodyId, b2MotionLocks locks );++/// Get the motion locks for this body.+B2_API b2MotionLocks b2Body_GetMotionLocks( b2BodyId bodyId );++/// Set this body to be a bullet. A bullet does continuous collision detection against dynamic+/// and kinematic bodies (but not other bullets). This only works on dynamic bodies.+B2_API void b2Body_SetBullet( b2BodyId bodyId, bool flag );++/// Is this body a bullet?+B2_API bool b2Body_IsBullet( b2BodyId 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 b2BodyDef::enableContactRecycling+B2_API void b2Body_EnableContactRecycling( b2BodyId bodyId, bool flag );++/// Is contact recycling enabled on this body?+B2_API bool b2Body_IsContactRecyclingEnabled( b2BodyId bodyId );++/// Enable/disable contact events on all shapes.+/// @see b2ShapeDef::enableContactEvents+/// @warning changing this at runtime may cause mismatched begin/end touch events+B2_API void b2Body_EnableContactEvents( b2BodyId bodyId, bool flag );++/// Enable/disable hit events on all shapes+/// @see b2ShapeDef::enableHitEvents+B2_API void b2Body_EnableHitEvents( b2BodyId bodyId, bool flag );++/// Get the world that owns this body+B2_API b2WorldId b2Body_GetWorld( b2BodyId bodyId );++/// Get the number of shapes on this body+B2_API int b2Body_GetShapeCount( b2BodyId 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+B2_API int b2Body_GetShapes( b2BodyId bodyId, b2ShapeId* shapeArray, int capacity );++/// Get the number of joints on this body+B2_API int b2Body_GetJointCount( b2BodyId 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+B2_API int b2Body_GetJoints( b2BodyId bodyId, b2JointId* jointArray, int capacity );++/// Get the maximum capacity required for retrieving all the touching contacts on a body+B2_API int b2Body_GetContactCapacity( b2BodyId bodyId );++/// Get the touching contact data for a body.+/// @note Box2D uses speculative collision so some contact points may be separated.+/// @returns the number of elements filled in the provided array+/// @warning do not ignore the return value, it specifies the valid number of elements+B2_API int b2Body_GetContactData( b2BodyId bodyId, b2ContactData* contactData, int capacity );++/// Get the current world AABB that contains all the attached shapes. Note that this may not+/// encompass the body origin. If there are no shapes attached then the returned AABB is empty and+/// centered on the body origin.+B2_API b2AABB b2Body_ComputeBounds( b2BodyId bodyId );++/// The minimum distance from any point on the body shapes to the center of mass.+B2_API float b2Body_GetMinExtent( b2BodyId bodyId );++/// The maximum distance from any point on the body shapes to the center of mass.+B2_API float b2Body_GetMaxExtent( b2BodyId bodyId );++/// The maximum distance from any point on the body shapes to the body origin. Conservative.+B2_API float b2Body_GetMaxExtentFromOrigin( b2BodyId bodyId );++/** @} */++/**+ * @defgroup shape Shape+ * Functions to create, destroy, and access shapes. Shapes bind raw geometry to bodies and hold+ * material properties including friction and restitution.+ * @{+ */++/// Create a circle shape and attach it to a body. The shape definition and geometry are fully+/// cloned. Contacts are not created until the next time step.+/// @return the shape id for accessing the shape+B2_API b2ShapeId b2CreateCircleShape( b2BodyId bodyId, const b2ShapeDef* def,+									  const b2Circle* circle );++/// Create a line segment 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 or b2_nullShapeId if the segment is too short.+B2_API b2ShapeId b2CreateSegmentShape( b2BodyId bodyId, const b2ShapeDef* def,+									   const b2Segment* segment );++/// Create a chain segment shape and attach it to a body. The shape definition and geometry are+/// fully cloned. The segment is not owned by any chain shape (b2Shape_GetParentChain returns+/// b2_nullChainId). Contacts are not created until the next time step. The chainId on the segment+/// is ignored. This uses more memory than using a chain shape, so I don't recommend using this.+/// @return the shape id, or b2_nullShapeId if the segment is too short.+B2_API b2ShapeId b2CreateChainSegmentShape( b2BodyId bodyId, const b2ShapeDef* def,+											const b2ChainSegment* chainSegment );++/// 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, this will be b2_nullShapeId if the length is too+/// small.+B2_API b2ShapeId b2CreateCapsuleShape( b2BodyId bodyId, const b2ShapeDef* def,+									   const b2Capsule* capsule );++/// Create a polygon 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+B2_API b2ShapeId b2CreatePolygonShape( b2BodyId bodyId, const b2ShapeDef* def,+									   const b2Polygon* polygon );++/// 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 b2Body_UpdateMassFromShapes+B2_API void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass );++/// Shape identifier validation. Provides validation for up to 64K allocations.+B2_API bool b2Shape_IsValid( b2ShapeId id );++/// Get the type of a shape+B2_API b2ShapeType b2Shape_GetType( b2ShapeId shapeId );++/// Get the id of the body that a shape is attached to+B2_API b2BodyId b2Shape_GetBody( b2ShapeId shapeId );++/// Get the world that owns this shape+B2_API b2WorldId b2Shape_GetWorld( b2ShapeId shapeId );++/// Returns true if the shape is a sensor. It is not possible to change a shape from sensor to solid+/// dynamically because this breaks the contract for sensor events.+B2_API bool b2Shape_IsSensor( b2ShapeId shapeId );++/// Set the user data for a shape+B2_API void b2Shape_SetUserData( b2ShapeId shapeId, void* userData );++/// Get the user data for a shape. This is useful when you get a shape id from an event or query.+B2_API void* b2Shape_GetUserData( b2ShapeId shapeId );++/// Set the mass density of a shape, usually in kg/m^2. This will optionally update the mass+/// properties on the parent body.+/// @see b2ShapeDef::density, b2Body_UpdateMassFromShapes+B2_API void b2Shape_SetDensity( b2ShapeId shapeId, float density, bool updateBodyMass );++/// Get the density of a shape, usually in kg/m^2+B2_API float b2Shape_GetDensity( b2ShapeId shapeId );++/// Set the friction on a shape+B2_API void b2Shape_SetFriction( b2ShapeId shapeId, float friction );++/// Get the friction of a shape+B2_API float b2Shape_GetFriction( b2ShapeId shapeId );++/// Set the shape restitution (bounciness)+B2_API void b2Shape_SetRestitution( b2ShapeId shapeId, float restitution );++/// Get the shape restitution+B2_API float b2Shape_GetRestitution( b2ShapeId shapeId );++/// Set the user material identifier+B2_API void b2Shape_SetUserMaterial( b2ShapeId shapeId, uint64_t material );++/// Get the user material identifier+B2_API uint64_t b2Shape_GetUserMaterial( b2ShapeId shapeId );++/// Set the shape surface material+B2_API void b2Shape_SetSurfaceMaterial( b2ShapeId shapeId,+										const b2SurfaceMaterial* surfaceMaterial );++/// Get the shape surface material+B2_API b2SurfaceMaterial b2Shape_GetSurfaceMaterial( b2ShapeId shapeId );++/// Get the shape filter+B2_API b2Filter b2Shape_GetFilter( b2ShapeId shapeId );++/// Set the current filter. This is almost as expensive as recreating the shape. This may cause+/// contacts to be immediately destroyed. However contacts are not created until the next world+/// step. Sensor overlap state is also not updated until the next world step.+/// @see b2ShapeDef::filter+B2_API void b2Shape_SetFilter( b2ShapeId shapeId, b2Filter filter );++/// Enable sensor events for this shape.+/// @see b2ShapeDef::enableSensorEvents+B2_API void b2Shape_EnableSensorEvents( b2ShapeId shapeId, bool flag );++/// Returns true if sensor events are enabled.+B2_API bool b2Shape_AreSensorEventsEnabled( b2ShapeId shapeId );++/// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for+/// sensors.+/// @see b2ShapeDef::enableContactEvents+/// @warning changing this at run-time may lead to lost begin/end events+B2_API void b2Shape_EnableContactEvents( b2ShapeId shapeId, bool flag );++/// Returns true if contact events are enabled+B2_API bool b2Shape_AreContactEventsEnabled( b2ShapeId 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 b2PreSolveCallbackFcn+B2_API void b2Shape_EnablePreSolveEvents( b2ShapeId shapeId, bool flag );++/// Returns true if pre-solve events are enabled+B2_API bool b2Shape_ArePreSolveEventsEnabled( b2ShapeId shapeId );++/// Enable contact hit events for this shape. Ignored for sensors.+/// @see b2WorldDef::hitEventThreshold+B2_API void b2Shape_EnableHitEvents( b2ShapeId shapeId, bool flag );++/// Returns true if hit events are enabled+B2_API bool b2Shape_AreHitEventsEnabled( b2ShapeId shapeId );++/// Test a world point for overlap with a shape+B2_API bool b2Shape_TestPoint( b2ShapeId shapeId, b2Pos point );++/// Ray cast a single shape. The ray starts at the origin and extends by the translation. The output+/// point is a world position.+B2_API b2WorldCastOutput b2Shape_CastRay( b2ShapeId shapeId, b2Pos origin, b2Vec2 translation );++/// Get a copy of the shape's circle. Asserts the type is correct.+B2_API b2Circle b2Shape_GetCircle( b2ShapeId shapeId );++/// Get a copy of the shape's line segment. Asserts the type is correct.+B2_API b2Segment b2Shape_GetSegment( b2ShapeId shapeId );++/// Get a copy of the shape's chain segment. These come from chain shapes. Asserts the type is+/// correct.+B2_API b2ChainSegment b2Shape_GetChainSegment( b2ShapeId shapeId );++/// Get a copy of the shape's capsule. Asserts the type is correct.+B2_API b2Capsule b2Shape_GetCapsule( b2ShapeId shapeId );++/// Get a copy of the shape's convex polygon. Asserts the type is correct.+B2_API b2Polygon b2Shape_GetPolygon( b2ShapeId shapeId );++/// Allows you to change a shape to be a circle or update the current circle. This does not modify+/// the mass properties.+/// @see b2Body_UpdateMassFromShapes+B2_API void b2Shape_SetCircle( b2ShapeId shapeId, const b2Circle* circle );++/// Allows you to change a shape to be a capsule or update the current capsule. This does not modify+/// the mass properties.+/// @see b2Body_UpdateMassFromShapes+B2_API void b2Shape_SetCapsule( b2ShapeId shapeId, const b2Capsule* capsule );++/// Allows you to change a shape to be a segment or update the current segment.+B2_API void b2Shape_SetSegment( b2ShapeId shapeId, const b2Segment* segment );++/// Allows you to change a shape to be a polygon or update the current polygon. This does not modify+/// the mass properties.+/// @see b2Body_UpdateMassFromShapes+B2_API void b2Shape_SetPolygon( b2ShapeId shapeId, const b2Polygon* polygon );++/// Allows you to change a shape to be a loose chain segment or update the current chain segment.+/// The resulting shape does not belong to a chain shape. Asserts if the shape is already a chain+/// segment owned by a chain shape (chainId != B2_NULL_INDEX).+B2_API void b2Shape_SetChainSegment( b2ShapeId shapeId, const b2ChainSegment* chainSegment );++/// Get the parent chain id if the shape type is a chain segment, otherwise returns b2_nullChainId.+B2_API b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId );++/// Get the maximum capacity required for retrieving all the touching contacts on a shape+B2_API int b2Shape_GetContactCapacity( b2ShapeId shapeId );++/// Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or+/// shapeIdB on the contact data.+/// @note Box2D uses speculative collision so some contact points may be separated.+/// @returns the number of elements filled in the provided array+/// @warning do not ignore the return value, it specifies the valid number of elements+B2_API int b2Shape_GetContactData( b2ShapeId shapeId, b2ContactData* 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 b2Shape_GetSensorData+B2_API int b2Shape_GetSensorCapacity( b2ShapeId shapeId );++/// Get the overlap data for a sensor shape computed during the previous world step.+/// @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 visitorIds+/// @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 b2Shape_IsValid to confirm each overlap+B2_API int b2Shape_GetSensorData( b2ShapeId shapeId, b2ShapeId* visitorIds, int capacity );++/// Get the current world AABB+B2_API b2AABB b2Shape_GetBounds( b2ShapeId shapeId );++/// Compute the mass data for a shape+B2_API b2MassData b2Shape_ComputeMassData( b2ShapeId shapeId );++/// Get the closest point on a shape to a target point. Target and result are world positions.+B2_API b2Pos b2Shape_GetClosestPoint( b2ShapeId shapeId, b2Pos 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 wake should this wake the body+B2_API void b2Shape_ApplyWind( b2ShapeId shapeId, b2Vec2 wind, float drag, float lift, bool wake );++/** @} */++/**+ * @defgroup chain Chain+ * @brief Chain shapes allow you to create a polygonal chain that is resistant to ghost collisions.+ * See https://box2d.org/posts/2020/06/ghost-collisions/+ * @{+ */++/// Create a chain shape+/// @see b2ChainDef for details+B2_API b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def );++/// Destroy a chain shape+B2_API void b2DestroyChain( b2ChainId chainId );++/// Get the world that owns this chain shape+B2_API b2WorldId b2Chain_GetWorld( b2ChainId chainId );++/// Get the number of segments on this chain+B2_API int b2Chain_GetSegmentCount( b2ChainId chainId );++/// Fill a user array with chain segment shape ids up to the specified capacity. Returns the actual+/// number of segments returned.+B2_API int b2Chain_GetSegments( b2ChainId chainId, b2ShapeId* segmentArray, int capacity );++/// Set the chain material on all segments.+B2_API void b2Chain_SetAllSurfaceMaterials( b2ChainId chainId, const b2SurfaceMaterial* material );++/// Set a chain material by segment index.+B2_API void b2Chain_SetSurfaceMaterial( b2ChainId chainId, const b2SurfaceMaterial* material,+										int segmentIndex );++/// Get a chain material by segment index.+B2_API b2SurfaceMaterial b2Chain_GetSurfaceMaterial( b2ChainId chainId, int segmentIndex );++/// Chain identifier validation. Provides validation for up to 64K allocations.+B2_API bool b2Chain_IsValid( b2ChainId id );++/** @} */++/**+ * @defgroup joint Joint+ * @brief Joints allow you to connect rigid bodies together while allowing various forms of relative+ * motion.+ * @{+ */++/// Destroy a joint.+B2_API void b2DestroyJoint( b2JointId jointId );++/// Joint identifier validation. Provides validation for up to 64K allocations.+B2_API bool b2Joint_IsValid( b2JointId id );++/// Get the joint type+B2_API b2JointType b2Joint_GetType( b2JointId jointId );++/// Get body A id on a joint+B2_API b2BodyId b2Joint_GetBodyA( b2JointId jointId );++/// Get body B id on a joint+B2_API b2BodyId b2Joint_GetBodyB( b2JointId jointId );++/// Get the world that owns this joint+B2_API b2WorldId b2Joint_GetWorld( b2JointId jointId );++/// Set the local frame on bodyA+B2_API void b2Joint_SetLocalFrameA( b2JointId jointId, b2Transform localFrame );++/// Get the local frame on bodyA+B2_API b2Transform b2Joint_GetLocalFrameA( b2JointId jointId );++/// Set the local frame on bodyB+B2_API void b2Joint_SetLocalFrameB( b2JointId jointId, b2Transform localFrame );++/// Get the local frame on bodyB+B2_API b2Transform b2Joint_GetLocalFrameB( b2JointId jointId );++/// Toggle collision between connected bodies+B2_API void b2Joint_SetCollideConnected( b2JointId jointId, bool shouldCollide );++/// Is collision allowed between connected bodies?+B2_API bool b2Joint_GetCollideConnected( b2JointId jointId );++/// Set the user data on a joint+B2_API void b2Joint_SetUserData( b2JointId jointId, void* userData );++/// Get the user data on a joint+B2_API void* b2Joint_GetUserData( b2JointId jointId );++/// Wake the bodies connected to this joint+B2_API void b2Joint_WakeBodies( b2JointId jointId );++/// Get the current constraint force for this joint. Usually in Newtons.+B2_API b2Vec2 b2Joint_GetConstraintForce( b2JointId jointId );++/// Get the current constraint torque for this joint. Usually in Newton * meters.+B2_API float b2Joint_GetConstraintTorque( b2JointId jointId );++/// Get the current linear separation error for this joint. Does not consider admissible movement.+/// Usually in meters.+B2_API float b2Joint_GetLinearSeparation( b2JointId jointId );++/// Get the current angular separation error for this joint. Does not consider admissible movement.+/// Radians.+B2_API float b2Joint_GetAngularSeparation( b2JointId 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)+B2_API void b2Joint_SetConstraintTuning( b2JointId jointId, float hertz, float dampingRatio );++/// Get the joint constraint tuning. Advanced feature.+B2_API void b2Joint_GetConstraintTuning( b2JointId jointId, float* hertz, float* dampingRatio );++/// Set the force threshold for joint events (Newtons)+B2_API void b2Joint_SetForceThreshold( b2JointId jointId, float threshold );++/// Get the force threshold for joint events (Newtons)+B2_API float b2Joint_GetForceThreshold( b2JointId jointId );++/// Set the torque threshold for joint events (N-m)+B2_API void b2Joint_SetTorqueThreshold( b2JointId jointId, float threshold );++/// Get the torque threshold for joint events (N-m)+B2_API float b2Joint_GetTorqueThreshold( b2JointId jointId );++/**+ * @defgroup distance_joint Distance Joint+ * @brief Functions for the distance joint.+ * @{+ */++/// Create a distance joint+/// @see b2DistanceJointDef for details+B2_API b2JointId b2CreateDistanceJoint( b2WorldId worldId, const b2DistanceJointDef* def );++/// Set the rest length of a distance joint+/// @param jointId The id for a distance joint+/// @param length The new distance joint length+B2_API void b2DistanceJoint_SetRestLength( b2JointId jointId, float length );++/// Get the rest length of a distance joint+B2_API float b2DistanceJoint_GetRestLength( b2JointId jointId );++/// Enable/disable the distance joint spring. When disabled the distance joint is rigid.+B2_API void b2DistanceJoint_EnableSpring( b2JointId jointId, bool enableSpring );++/// Is the distance joint spring enabled?+B2_API bool b2DistanceJoint_IsSpringEnabled( b2JointId jointId );++/// Set the force range for the spring.+B2_API void b2DistanceJoint_SetSpringForceRange( b2JointId jointId, float lowerForce,+												 float upperForce );++/// Get the force range for the spring.+B2_API void b2DistanceJoint_GetSpringForceRange( b2JointId jointId, float* lowerForce,+												 float* upperForce );++/// Set the spring stiffness in Hertz+B2_API void b2DistanceJoint_SetSpringHertz( b2JointId jointId, float hertz );++/// Set the spring damping ratio, non-dimensional+B2_API void b2DistanceJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio );++/// Get the spring Hertz+B2_API float b2DistanceJoint_GetSpringHertz( b2JointId jointId );++/// Get the spring damping ratio+B2_API float b2DistanceJoint_GetSpringDampingRatio( b2JointId jointId );++/// Get the current spring force.+B2_API float b2DistanceJoint_GetSpringForce( b2JointId 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.+B2_API void b2DistanceJoint_EnableLimit( b2JointId jointId, bool enableLimit );++/// Is the distance joint limit enabled?+B2_API bool b2DistanceJoint_IsLimitEnabled( b2JointId jointId );++/// Set the minimum and maximum length parameters of a distance joint+B2_API void b2DistanceJoint_SetLimits( b2JointId jointId, float minLength, float maxLength );++/// Get the distance joint minimum length+B2_API float b2DistanceJoint_GetLowerLimit( b2JointId jointId );++/// Get the distance joint maximum length+B2_API float b2DistanceJoint_GetUpperLimit( b2JointId jointId );++/// Get the current length of a distance joint+B2_API float b2DistanceJoint_GetCurrentLength( b2JointId jointId );++/// Enable/disable the distance joint motor+B2_API void b2DistanceJoint_EnableMotor( b2JointId jointId, bool enableMotor );++/// Is the distance joint motor enabled?+B2_API bool b2DistanceJoint_IsMotorEnabled( b2JointId jointId );++/// Set the distance joint motor speed, usually in meters per second+B2_API void b2DistanceJoint_SetTargetMotorSpeed( b2JointId jointId, float motorSpeed );++/// Get the distance joint motor speed, usually in meters per second+B2_API float b2DistanceJoint_GetTargetMotorSpeed( b2JointId jointId );++/// Set the distance joint maximum motor force, usually in newtons+B2_API void b2DistanceJoint_SetMaxMotorForce( b2JointId jointId, float force );++/// Get the distance joint maximum motor force, usually in newtons+B2_API float b2DistanceJoint_GetMaxMotorForce( b2JointId jointId );++/// Get the distance joint current motor force, usually in newtons+B2_API float b2DistanceJoint_GetCurrentMotorForce( b2JointId jointId );++/** @} */++/**+ * @defgroup filter_joint Filter Joint+ * @brief Functions for the filter joint.+ *+ * The filter joint is used to disable collision between two bodies. As a side effect of being a+ * joint, it also keeps the two bodies in the same simulation island.+ * @{+ */++/// Create a filter joint.+/// @see b2FilterJointDef for details+B2_API b2JointId b2CreateFilterJoint( b2WorldId worldId, const b2FilterJointDef* def );++/**@}*/++/**+ * @defgroup motor_joint Motor Joint+ * @brief Functions for the motor joint.+ *+ * 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 b2MotorJointDef for details+B2_API b2JointId b2CreateMotorJoint( b2WorldId worldId, const b2MotorJointDef* def );++/// Set the desired relative linear velocity in meters per second+B2_API void b2MotorJoint_SetLinearVelocity( b2JointId jointId, b2Vec2 velocity );++/// Get the desired relative linear velocity in meters per second+B2_API b2Vec2 b2MotorJoint_GetLinearVelocity( b2JointId jointId );++/// Set the desired relative angular velocity in radians per second+B2_API void b2MotorJoint_SetAngularVelocity( b2JointId jointId, float velocity );++/// Get the desired relative angular velocity in radians per second+B2_API float b2MotorJoint_GetAngularVelocity( b2JointId jointId );++/// Set the motor joint maximum force, usually in newtons+B2_API void b2MotorJoint_SetMaxVelocityForce( b2JointId jointId, float maxForce );++/// Get the motor joint maximum force, usually in newtons+B2_API float b2MotorJoint_GetMaxVelocityForce( b2JointId jointId );++/// Set the motor joint maximum torque, usually in newton-meters+B2_API void b2MotorJoint_SetMaxVelocityTorque( b2JointId jointId, float maxTorque );++/// Get the motor joint maximum torque, usually in newton-meters+B2_API float b2MotorJoint_GetMaxVelocityTorque( b2JointId jointId );++/// Set the spring linear hertz stiffness+B2_API void b2MotorJoint_SetLinearHertz( b2JointId jointId, float hertz );++/// Get the spring linear hertz stiffness+B2_API float b2MotorJoint_GetLinearHertz( b2JointId jointId );++/// Set the spring linear damping ratio. Use 1.0 for critical damping.+B2_API void b2MotorJoint_SetLinearDampingRatio( b2JointId jointId, float damping );++/// Get the spring linear damping ratio.+B2_API float b2MotorJoint_GetLinearDampingRatio( b2JointId jointId );++/// Set the spring angular hertz stiffness+B2_API void b2MotorJoint_SetAngularHertz( b2JointId jointId, float hertz );++/// Get the spring angular hertz stiffness+B2_API float b2MotorJoint_GetAngularHertz( b2JointId jointId );++/// Set the spring angular damping ratio. Use 1.0 for critical damping.+B2_API void b2MotorJoint_SetAngularDampingRatio( b2JointId jointId, float damping );++/// Get the spring angular damping ratio.+B2_API float b2MotorJoint_GetAngularDampingRatio( b2JointId jointId );++/// Set the maximum spring force in newtons.+B2_API void b2MotorJoint_SetMaxSpringForce( b2JointId jointId, float maxForce );++/// Get the maximum spring force in newtons.+B2_API float b2MotorJoint_GetMaxSpringForce( b2JointId jointId );++/// Set the maximum spring torque in newtons * meters+B2_API void b2MotorJoint_SetMaxSpringTorque( b2JointId jointId, float maxTorque );++/// Get the maximum spring torque in newtons * meters+B2_API float b2MotorJoint_GetMaxSpringTorque( b2JointId jointId );++/**@}*/++/**+ * @defgroup mover_joint Mover Joint+ * @brief Functions for the mover joint.+ *+ * The mover joint is designed for a dynamic character mover.+ *+ * @warning Experimental. The mover joint may change in future versions.+ * @{+ */++/// Create a mover joint+/// @see b2MoverJointDef for details+B2_API b2JointId b2CreateMoverJoint( b2WorldId worldId, const b2MoverJointDef* def );++/// Set the desired relative linear velocity in meters per second+B2_API void b2MoverJoint_SetLinearVelocity( b2JointId jointId, b2Vec2 velocity );++/// Get the desired relative linear velocity in meters per second+B2_API b2Vec2 b2MoverJoint_GetLinearVelocity( b2JointId jointId );++/// Set the joint maximum force, usually in newtons+B2_API void b2MoverJoint_SetMaxVelocityForce( b2JointId jointId, b2Vec2 maxForce );++/// Get the joint maximum force, usually in newtons+B2_API b2Vec2 b2MoverJoint_GetMaxVelocityForce( b2JointId jointId );++/**@}*/++/**+ * @defgroup pogo_joint Pogo Joint+ * @brief Functions for the pogo joint. Used for dynamic character movers.+ *+ * @warning Experimental. The pogo joint may change in future versions.+ * @{+ */++/// Create a pogo joint+/// @see b2PogoJointDef for details+B2_API b2JointId b2CreatePogoJoint( b2WorldId worldId, const b2PogoJointDef* def );++/// Set the rest length of a pogo joint+/// @param jointId The id for a pogo joint+/// @param length The new pogo joint length+B2_API void b2PogoJoint_SetRestLength( b2JointId jointId, float length );++/// Get the rest length of a pogo joint+B2_API float b2PogoJoint_GetRestLength( b2JointId jointId );++/// Set the spring stiffness in Hertz+B2_API void b2PogoJoint_SetSpringHertz( b2JointId jointId, float hertz );++/// Get the spring Hertz+B2_API float b2PogoJoint_GetSpringHertz( b2JointId jointId );++/// Set the spring damping ratio, non-dimensional+B2_API void b2PogoJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio );++/// Get the spring damping ratio+B2_API float b2PogoJoint_GetSpringDampingRatio( b2JointId jointId );++/// Get the current length of the pogo.+B2_API float b2PogoJoint_GetCurrentLength( b2JointId jointId );++/// Get the internal pogo velocity.+B2_API float b2PogoJoint_GetCurrentSpeed( b2JointId jointId );++/// Get the internal pogo impulse.+B2_API float b2PogoJoint_GetImpulse( b2JointId jointId );++/** @} */++/**+ * @defgroup prismatic_joint Prismatic Joint+ * @brief A prismatic joint allows for translation along a single axis with no rotation.+ *+ * 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 b2PrismaticJointDef for details+B2_API b2JointId b2CreatePrismaticJoint( b2WorldId worldId, const b2PrismaticJointDef* def );++/// Enable/disable the joint spring.+B2_API void b2PrismaticJoint_EnableSpring( b2JointId jointId, bool enableSpring );++/// Is the prismatic joint spring enabled or not?+B2_API bool b2PrismaticJoint_IsSpringEnabled( b2JointId 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.+B2_API void b2PrismaticJoint_SetSpringHertz( b2JointId jointId, float hertz );++/// Get the prismatic joint stiffness in Hertz+B2_API float b2PrismaticJoint_GetSpringHertz( b2JointId jointId );++/// Set the prismatic joint damping ratio (non-dimensional)+B2_API void b2PrismaticJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio );++/// Get the prismatic spring damping ratio (non-dimensional)+B2_API float b2PrismaticJoint_GetSpringDampingRatio( b2JointId jointId );++/// Set the prismatic joint spring target translation, usually in meters+B2_API void b2PrismaticJoint_SetTargetTranslation( b2JointId jointId, float translation );++/// Get the prismatic joint spring target translation, usually in meters+B2_API float b2PrismaticJoint_GetTargetTranslation( b2JointId jointId );++/// Enable/disable a prismatic joint limit+B2_API void b2PrismaticJoint_EnableLimit( b2JointId jointId, bool enableLimit );++/// Is the prismatic joint limit enabled?+B2_API bool b2PrismaticJoint_IsLimitEnabled( b2JointId jointId );++/// Get the prismatic joint lower limit+B2_API float b2PrismaticJoint_GetLowerLimit( b2JointId jointId );++/// Get the prismatic joint upper limit+B2_API float b2PrismaticJoint_GetUpperLimit( b2JointId jointId );++/// Set the prismatic joint limits+B2_API void b2PrismaticJoint_SetLimits( b2JointId jointId, float lower, float upper );++/// Enable/disable a prismatic joint motor+B2_API void b2PrismaticJoint_EnableMotor( b2JointId jointId, bool enableMotor );++/// Is the prismatic joint motor enabled?+B2_API bool b2PrismaticJoint_IsMotorEnabled( b2JointId jointId );++/// Set the prismatic joint motor speed, usually in meters per second+B2_API void b2PrismaticJoint_SetTargetMotorSpeed( b2JointId jointId, float motorSpeed );++/// Get the prismatic joint motor speed, usually in meters per second+B2_API float b2PrismaticJoint_GetTargetMotorSpeed( b2JointId jointId );++/// Set the prismatic joint maximum motor force, usually in newtons+B2_API void b2PrismaticJoint_SetMaxMotorForce( b2JointId jointId, float force );++/// Get the prismatic joint maximum motor force, usually in newtons+B2_API float b2PrismaticJoint_GetMaxMotorForce( b2JointId jointId );++/// Get the current joint translation, usually in meters.+B2_API float b2PrismaticJoint_GetCurrentTranslation( b2JointId jointId );++/// Get the current joint translation velocity, usually in meters per second.+B2_API float b2PrismaticJoint_GetCurrentSpeed( b2JointId jointId );++/// Get the prismatic joint current motor force, usually in newtons+B2_API float b2PrismaticJoint_GetCurrentMotorForce( b2JointId jointId );++/** @} */++/**+ * @defgroup revolute_joint Revolute Joint+ * @brief A revolute joint allows for relative rotation in the 2D plane with no relative+ * translation.+ *+ * Also called a *hinge* or *pin* joint.+ * @{+ */++/// Create a revolute joint+/// @see b2RevoluteJointDef for details+B2_API b2JointId b2CreateRevoluteJoint( b2WorldId worldId, const b2RevoluteJointDef* def );++/// Enable/disable the revolute joint spring+B2_API void b2RevoluteJoint_EnableSpring( b2JointId jointId, bool enableSpring );++/// Is the revolute angular spring enabled?+B2_API bool b2RevoluteJoint_IsSpringEnabled( b2JointId jointId );++/// Set the revolute joint spring stiffness in Hertz+B2_API void b2RevoluteJoint_SetSpringHertz( b2JointId jointId, float hertz );++/// Get the revolute joint spring stiffness in Hertz+B2_API float b2RevoluteJoint_GetSpringHertz( b2JointId jointId );++/// Set the revolute joint spring damping ratio, non-dimensional+B2_API void b2RevoluteJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio );++/// Get the revolute joint spring damping ratio, non-dimensional+B2_API float b2RevoluteJoint_GetSpringDampingRatio( b2JointId jointId );++/// Set the revolute joint spring target angle, radians+B2_API void b2RevoluteJoint_SetTargetAngle( b2JointId jointId, float angle );++/// Get the revolute joint spring target angle, radians+B2_API float b2RevoluteJoint_GetTargetAngle( b2JointId jointId );++/// Enable/disable the revolute joint limit+B2_API void b2RevoluteJoint_EnableLimit( b2JointId jointId, bool enableLimit );++/// Is the revolute joint limit enabled?+B2_API bool b2RevoluteJoint_IsLimitEnabled( b2JointId jointId );++/// Get the revolute joint lower limit in radians+B2_API float b2RevoluteJoint_GetLowerLimit( b2JointId jointId );++/// Get the revolute joint upper limit in radians+B2_API float b2RevoluteJoint_GetUpperLimit( b2JointId jointId );++/// Set the revolute joint limits in radians. It is expected that lower <= upper and that -0.99 *+/// B2_PI <= lower && upper <= 0.99 * B2_PI.+B2_API void b2RevoluteJoint_SetLimits( b2JointId jointId, float lower, float upper );++/// Enable/disable a revolute joint motor+B2_API void b2RevoluteJoint_EnableMotor( b2JointId jointId, bool enableMotor );++/// Is the revolute joint motor enabled?+B2_API bool b2RevoluteJoint_IsMotorEnabled( b2JointId jointId );++/// Set the revolute joint motor speed in radians per second+B2_API void b2RevoluteJoint_SetTargetMotorSpeed( b2JointId jointId, float motorSpeed );++/// Get the revolute joint motor speed in radians per second+B2_API float b2RevoluteJoint_GetTargetMotorSpeed( b2JointId jointId );++/// Set the revolute joint maximum motor torque, usually in newton-meters+B2_API void b2RevoluteJoint_SetMaxMotorTorque( b2JointId jointId, float torque );++/// Get the revolute joint maximum motor torque, usually in newton-meters+B2_API float b2RevoluteJoint_GetMaxMotorTorque( b2JointId jointId );++/// Get the revolute joint current angle in radians. This is the relative rotation of the joint+/// frames.+B2_API float b2RevoluteJoint_GetCurrentAngle( b2JointId jointId );++/// Get the current revolute joint motor speed in radians per second+B2_API float b2RevoluteJoint_GetCurrentSpeed( b2JointId jointId );++/// Get the revolute joint current motor torque, usually in newton-meters+B2_API float b2RevoluteJoint_GetCurrentMotorTorque( b2JointId jointId );++/**@}*/++/**+ * @defgroup weld_joint Weld Joint+ * @brief A weld joint fully constrains the relative transform between two bodies while allowing for+ * springiness+ *+ * A weld joint constrains the relative rotation and translation between two bodies. Both rotation+ * and translation can have damped springs.+ *+ * @note The accuracy of the weld joint is limited by the accuracy of the solver. Long chains of+ * weld joints may flex.+ * @{+ */++/// Create a weld joint+/// @see b2WeldJointDef for details+B2_API b2JointId b2CreateWeldJoint( b2WorldId worldId, const b2WeldJointDef* def );++/// Set the weld joint linear stiffness in Hertz. 0 is rigid.+B2_API void b2WeldJoint_SetLinearHertz( b2JointId jointId, float hertz );++/// Get the weld joint linear stiffness in Hertz+B2_API float b2WeldJoint_GetLinearHertz( b2JointId jointId );++/// Set the weld joint linear damping ratio (non-dimensional)+B2_API void b2WeldJoint_SetLinearDampingRatio( b2JointId jointId, float dampingRatio );++/// Get the weld joint linear damping ratio (non-dimensional)+B2_API float b2WeldJoint_GetLinearDampingRatio( b2JointId jointId );++/// Set the weld joint angular stiffness in Hertz. 0 is rigid.+B2_API void b2WeldJoint_SetAngularHertz( b2JointId jointId, float hertz );++/// Get the weld joint angular stiffness in Hertz+B2_API float b2WeldJoint_GetAngularHertz( b2JointId jointId );++/// Set weld joint angular damping ratio, non-dimensional+B2_API void b2WeldJoint_SetAngularDampingRatio( b2JointId jointId, float dampingRatio );++/// Get the weld joint angular damping ratio, non-dimensional+B2_API float b2WeldJoint_GetAngularDampingRatio( b2JointId jointId );++/** @} */++/**+ * @defgroup wheel_joint Wheel Joint+ * The wheel joint can be used to simulate wheels on vehicles.+ *+ * The wheel joint restricts body B to move along a local axis in body A. Body B is free to rotate.+ * Supports a linear spring, linear limits, and a rotational motor.+ *+ * @{+ */++/// Create a wheel joint+/// @see b2WheelJointDef for details+B2_API b2JointId b2CreateWheelJoint( b2WorldId worldId, const b2WheelJointDef* def );++/// Enable/disable the wheel joint spring+B2_API void b2WheelJoint_EnableSpring( b2JointId jointId, bool enableSpring );++/// Is the wheel joint spring enabled?+B2_API bool b2WheelJoint_IsSpringEnabled( b2JointId jointId );++/// Set the wheel joint stiffness in Hertz+B2_API void b2WheelJoint_SetSpringHertz( b2JointId jointId, float hertz );++/// Get the wheel joint stiffness in Hertz+B2_API float b2WheelJoint_GetSpringHertz( b2JointId jointId );++/// Set the wheel joint damping ratio, non-dimensional+B2_API void b2WheelJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio );++/// Get the wheel joint damping ratio, non-dimensional+B2_API float b2WheelJoint_GetSpringDampingRatio( b2JointId jointId );++/// Enable/disable the wheel joint limit+B2_API void b2WheelJoint_EnableLimit( b2JointId jointId, bool enableLimit );++/// Is the wheel joint limit enabled?+B2_API bool b2WheelJoint_IsLimitEnabled( b2JointId jointId );++/// Get the wheel joint lower limit+B2_API float b2WheelJoint_GetLowerLimit( b2JointId jointId );++/// Get the wheel joint upper limit+B2_API float b2WheelJoint_GetUpperLimit( b2JointId jointId );++/// Set the wheel joint limits+B2_API void b2WheelJoint_SetLimits( b2JointId jointId, float lower, float upper );++/// Enable/disable the wheel joint motor+B2_API void b2WheelJoint_EnableMotor( b2JointId jointId, bool enableMotor );++/// Is the wheel joint motor enabled?+B2_API bool b2WheelJoint_IsMotorEnabled( b2JointId jointId );++/// Set the wheel joint motor speed in radians per second+B2_API void b2WheelJoint_SetTargetMotorSpeed( b2JointId jointId, float motorSpeed );++/// Get the wheel joint motor speed in radians per second+B2_API float b2WheelJoint_GetTargetMotorSpeed( b2JointId jointId );++/// Set the wheel joint maximum motor torque, usually in newton-meters+B2_API void b2WheelJoint_SetMaxMotorTorque( b2JointId jointId, float torque );++/// Get the wheel joint maximum motor torque, usually in newton-meters+B2_API float b2WheelJoint_GetMaxMotorTorque( b2JointId jointId );++/// Get the wheel joint current motor torque, usually in newton-meters+B2_API float b2WheelJoint_GetCurrentMotorTorque( b2JointId jointId );++/**@}*/++/**@}*/++/**+ * @defgroup contact Contact+ * Access to contacts+ * @{+ */++/// Contact identifier validation. Provides validation for up to 2^32 allocations.+B2_API bool b2Contact_IsValid( b2ContactId id );++/// Get the data for a contact. The manifold may have no points if the contact is not touching.+B2_API b2ContactData b2Contact_GetData( b2ContactId contactId );++/**@}*/++/**+ * @defgroup replay Replay+ * These functions allow you to replay a recorded simulation. This functionality is built to serve+ * the replay viewer in the sample app. It is unlikely that any game or application would use these+ * features, so consider them internal.+ * @{+ */++/// Replay a recording by re-running the engine and asserting recorded ids and state match.+/// @param data Recorded bytes.+/// @param size Number of recorded bytes+/// @param workerCount Worker count to use for replay. 0 uses the serial single-worker fallback.+/// @return true if replay completed without divergence, false on any mismatch+B2_API bool b2ValidateRecording( const void* data, int size, int workerCount );++/// Open a recording for incremental playback and replay up to the first step. The player copies the+/// bytes, so you may free or destroy the source buffer immediately after this call.+/// @param data Recorded bytes+/// @param size Number of recorded bytes+/// @param workerCount Worker count for the replay world. 0 uses the serial single-worker fallback.+/// @return A player handle, or NULL if the recording is malformed+B2_API b2Replay* b2CreateReplay( const void* data, int size, int workerCount );++/// Close a player and free its replay world and file buffer.+B2_API void b2DestroyReplay( b2Replay* player );++/// Advance the replay by one recorded step.+/// @return true if a step executed, false once the end of the recording is reached+B2_API bool b2Replay_StepFrame( b2Replay* player );++/// Get the id of the replayed world.+B2_API b2WorldId b2Replay_GetWorldId( const b2Replay* player );++/// Rewind the player to the first step, recreating the replay world from the file.+B2_API void b2Replay_Restart( b2Replay* player );++/// Seek to a recorded step. Seeking backward rewinds and re-runs from the nearest keyframe. Clamps+/// to the recording bounds.+B2_API void b2Replay_SeekFrame( b2Replay* player, int targetFrame );++/// Get the number of steps replayed so far.+B2_API int b2Replay_GetFrame( const b2Replay* player );++/// Get static metadata for the recording (frame count, recorded tuning, time).+B2_API b2ReplayInfo b2Replay_GetInfo( const b2Replay* player );++/// Returns true once the end of the recording has been reached.+B2_API bool b2Replay_IsAtEnd( const b2Replay* player );++/// Returns true if a recorded state hash failed to reproduce, meaning replay diverged.+B2_API bool b2Replay_HasDiverged( const b2Replay* player );++/// Get the first step at which replay diverged, or -1 if it has not diverged.+B2_API int b2Replay_GetDivergeFrame( const b2Replay* player );++/// 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.+B2_API void b2Replay_SetKeyframePolicy( b2Replay* player, size_t budgetBytes,+										int minIntervalFrames );++/// Get the keyframe memory budget in bytes.+B2_API size_t b2Replay_GetKeyframeBudget( const b2Replay* player );++/// Get the finest keyframe spacing in frames.+B2_API int b2Replay_GetKeyframeMinInterval( const b2Replay* player );++/// Get 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 right now.+B2_API int b2Replay_GetKeyframeInterval( const b2Replay* player );++/// Get the memory currently held by keyframe snapshots, in bytes.+B2_API size_t b2Replay_GetKeyframeBytes( const b2Replay* player );++/// Draw spatial queries recorded during the most recently replayed frame. Call after b2World_Draw+/// so queries are layered on top of the world.+/// @param player A valid player handle+/// @param draw Debug draw callbacks. NULL draw function pointers are skipped.+/// @param queryIndex Index into the frame's queries to draw, or -1 to draw all of them.+B2_API void b2Replay_DrawFrameQueries( b2Replay* player, b2DebugDraw* draw, int queryIndex );++/// Get the number of spatial queries recorded for the most recently replayed frame.+B2_API int b2Replay_GetFrameQueryCount( const b2Replay* player );++/// Get a recorded query from the most recently replayed frame by index.+B2_API b2ReplayQueryInfo b2Replay_GetFrameQuery( const b2Replay* player, int index );++/// Get one result of a recorded query from the most recently replayed frame.+B2_API b2ReplayQueryHit b2Replay_GetFrameQueryHit( const b2Replay* player, int queryIndex,+												   int hitIndex );++/// Get the number of body slots tracked for the outliner. This is the creation-order span and+/// includes holes for destroyed bodies, so it only grows as the replay advances.+B2_API int b2Replay_GetBodyCount( const b2Replay* player );++/// Get a tracked body by creation ordinal. Returns b2_nullBodyId for a destroyed slot, an ordinal+/// not yet reached at the current frame, or an out-of-range index. Validate with b2Body_IsValid.+B2_API b2BodyId b2Replay_GetBodyId( const b2Replay* player, int index );++/** @} */
+ upstream/include/box2d/collision.h view
@@ -0,0 +1,886 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "base.h"+#include "math_types.h"++#include <stdbool.h>++typedef struct b2SimplexCache b2SimplexCache;+typedef struct b2Hull b2Hull;++/**+ * @defgroup geometry Geometry+ * @brief Geometry types and algorithms+ *+ * Definitions of circles, capsules, segments, and polygons. Various algorithms to compute hulls,+ * mass properties, and so on. Functions should take the shape as the first argument to assist+ * editor auto-complete.+ * @{+ */++/// The maximum number of vertices on a convex polygon. Changing this affects performance even if+/// you don't use more vertices.+#define B2_MAX_POLYGON_VERTICES 8++/// Low level ray cast input data+typedef struct b2RayCastInput+{+	/// Start point of the ray cast+	b2Vec2 origin;++	/// Translation of the ray cast+	b2Vec2 translation;++	/// The maximum fraction of the translation to consider, typically 1+	float maxFraction;+} b2RayCastInput;++/// A distance proxy is used by the GJK algorithm. It encapsulates any shape. You can provide+/// between 1 and B2_MAX_POLYGON_VERTICES points and a radius.+typedef struct b2ShapeProxy+{+	/// The point cloud+	b2Vec2 points[B2_MAX_POLYGON_VERTICES];++	/// The number of points. Must be greater than 0.+	int count;++	/// The external radius of the point cloud. May be zero.+	float radius;+} b2ShapeProxy;++/// Low level shape cast input in generic form. This allows casting an arbitrary point cloud wrapped+/// with a radius. For example, a circle 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 b2ShapeCastInput+{+	/// A generic shape+	b2ShapeProxy proxy;++	/// The translation of the shape cast+	b2Vec2 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;+} b2ShapeCastInput;++/// Low level ray cast or shape cast output data. The hit point is in the local or relative frame of+/// the input. Returns a zero fraction and normal in the case of initial overlap.+typedef struct b2CastOutput+{+	/// The surface normal at the hit point+	b2Vec2 normal;++	/// The surface hit point+	b2Vec2 point;++	/// The fraction of the input translation at collision+	float fraction;++	/// The number of iterations used+	int iterations;++	/// Did the cast hit?+	bool hit;+} b2CastOutput;++#if defined( BOX2D_DOUBLE_PRECISION )++/// World-space cast output. The hit point is a world position. The normal stays a float direction.+typedef struct b2WorldCastOutput+{+	b2Vec2 normal;	///< The surface normal at the hit point+	b2Pos point;	///< The surface hit point in world space+	float fraction; ///< The fraction of the input translation at collision+	int iterations; ///< The number of iterations used+	bool hit;		///< Did the cast hit?+} b2WorldCastOutput;++#else++/// This is an alias in single precision.+typedef b2CastOutput b2WorldCastOutput;++#endif++/// This holds the mass data computed for a shape.+typedef struct b2MassData+{+	/// The mass of the shape, usually in kilograms.+	float mass;++	/// The position of the shape's centroid relative to the shape's origin.+	b2Vec2 center;++	/// The rotational inertia of the shape about the shape center.+	float rotationalInertia;+} b2MassData;++/// A solid circle+typedef struct b2Circle+{+	/// The local center+	b2Vec2 center;++	/// The radius+	float radius;+} b2Circle;++/// A solid capsule can be viewed as two semicircles connected by a rectangle.+typedef struct b2Capsule+{+	/// Local center of the first semicircle+	b2Vec2 center1;++	/// Local center of the second semicircle+	b2Vec2 center2;++	/// The radius of the semicircles+	float radius;+} b2Capsule;++/// A solid convex polygon. It is assumed that the interior of the polygon is to the left of each+/// edge. Polygons have a maximum number of vertices equal to B2_MAX_POLYGON_VERTICES. In most cases+/// you should not need many vertices for a convex polygon.+/// @warning DO NOT fill this out manually, instead use a helper function like b2MakePolygon or+/// b2MakeBox.+typedef struct b2Polygon+{+	/// The polygon vertices+	b2Vec2 vertices[B2_MAX_POLYGON_VERTICES];++	/// The outward normal vectors of the polygon sides+	b2Vec2 normals[B2_MAX_POLYGON_VERTICES];++	/// The centroid of the polygon+	b2Vec2 centroid;++	/// The external radius for rounded polygons+	float radius;++	/// The number of polygon vertices+	int count;+} b2Polygon;++/// A line segment with two-sided collision.+typedef struct b2Segment+{+	/// The first point+	b2Vec2 point1;++	/// The second point+	b2Vec2 point2;+} b2Segment;++/// A line segment with one-sided collision. Only collides on the right side. Several of these are+/// generated for a chain shape.+/// ghost1 -> point1 -> point2 -> ghost2+typedef struct b2ChainSegment+{+	/// The tail ghost vertex+	b2Vec2 ghost1;++	/// The line segment+	b2Segment segment;++	/// The head ghost vertex+	b2Vec2 ghost2;++	/// The owning chain shape index (internal usage only)+	int chainId;+} b2ChainSegment;++/// Validate ray cast input data (NaN, etc)+B2_API bool b2IsValidRay( const b2RayCastInput* input );++/// Make a convex polygon from a convex hull. This will assert if the hull is not valid.+/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull+B2_API b2Polygon b2MakePolygon( const b2Hull* hull, float radius );++/// Make an offset convex polygon from a convex hull. This will assert if the hull is not valid.+/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull+B2_API b2Polygon b2MakeOffsetPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation );++/// Make an offset convex polygon from a convex hull with a radius. This will assert if the hull is+/// not valid.+/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull+B2_API b2Polygon b2MakeOffsetRoundedPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation,+											 float radius );++/// Make a square polygon, bypassing the need for a convex hull.+/// @param halfWidth the half-width+B2_API b2Polygon b2MakeSquare( float halfWidth );++/// Make a box (rectangle) polygon, bypassing the need for a convex hull.+/// @param halfWidth the half-width (x-axis)+/// @param halfHeight the half-height (y-axis)+B2_API b2Polygon b2MakeBox( float halfWidth, float halfHeight );++/// Make a rounded box, bypassing the need for a convex hull.+/// @param halfWidth the half-width (x-axis)+/// @param halfHeight the half-height (y-axis)+/// @param radius the radius of the rounded extension+B2_API b2Polygon b2MakeRoundedBox( float halfWidth, float halfHeight, float radius );++/// Make an offset box, bypassing the need for a convex hull.+/// @param halfWidth the half-width (x-axis)+/// @param halfHeight the half-height (y-axis)+/// @param center the local center of the box+/// @param rotation the local rotation of the box+B2_API b2Polygon b2MakeOffsetBox( float halfWidth, float halfHeight, b2Vec2 center,+								  b2Rot rotation );++/// Make an offset rounded box, bypassing the need for a convex hull.+/// @param halfWidth the half-width (x-axis)+/// @param halfHeight the half-height (y-axis)+/// @param center the local center of the box+/// @param rotation the local rotation of the box+/// @param radius the radius of the rounded extension+B2_API b2Polygon b2MakeOffsetRoundedBox( float halfWidth, float halfHeight, b2Vec2 center,+										 b2Rot rotation, float radius );++/// Transform a polygon. This is useful for transferring a shape from one body to another.+B2_API b2Polygon b2TransformPolygon( b2Transform transform, const b2Polygon* polygon );++/// Compute mass properties of a circle+B2_API b2MassData b2ComputeCircleMass( const b2Circle* shape, float density );++/// Compute mass properties of a capsule+B2_API b2MassData b2ComputeCapsuleMass( const b2Capsule* shape, float density );++/// Compute mass properties of a polygon+B2_API b2MassData b2ComputePolygonMass( const b2Polygon* shape, float density );++/// Compute the bounding box of a transformed circle+B2_API b2AABB b2ComputeCircleBounds( const b2Circle* shape, b2WorldTransform transform );++/// Compute the bounding box of a transformed capsule+B2_API b2AABB b2ComputeCapsuleBounds( const b2Capsule* shape, b2WorldTransform transform );++/// Compute the bounding box of a transformed polygon+B2_API b2AABB b2ComputePolygonBounds( const b2Polygon* shape, b2WorldTransform transform );++/// Compute the bounding box of a transformed line segment+B2_API b2AABB b2ComputeSegmentBounds( const b2Segment* shape, b2WorldTransform transform );++/// Test a point for overlap with a circle in local space+B2_API bool b2PointInCircle( const b2Circle* shape, b2Vec2 point );++/// Test a point for overlap with a capsule in local space+B2_API bool b2PointInCapsule( const b2Capsule* shape, b2Vec2 point );++/// Test a point for overlap with a convex polygon in local space+B2_API bool b2PointInPolygon( const b2Polygon* shape, b2Vec2 point );++/// Ray cast versus circle shape in local space.+B2_API b2CastOutput b2CastRayCircle( const b2Circle* shape, const b2RayCastInput* input );++/// Ray cast versus capsule shape in local space.+B2_API b2CastOutput b2CastRayCapsule( const b2Capsule* shape, const b2RayCastInput* input );++/// Ray cast versus segment shape in local space. Optionally treat the segment as one-sided with+/// hits from the left side being treated as a miss.+B2_API b2CastOutput b2CastRaySegment( const b2Segment* shape, const b2RayCastInput* input,+									  bool oneSided );++/// Ray cast versus polygon shape in local space.+B2_API b2CastOutput b2CastRayPolygon( const b2Polygon* shape, const b2RayCastInput* input );++/// Shape cast versus a circle.+B2_API b2CastOutput b2CastShapeCircle( const b2Circle* shape, const b2ShapeCastInput* input );++/// Shape cast versus a capsule.+B2_API b2CastOutput b2CastShapeCapsule( const b2Capsule* shape, const b2ShapeCastInput* input );++/// Shape cast versus a line segment.+B2_API b2CastOutput b2CastShapeSegment( const b2Segment* shape, const b2ShapeCastInput* input );++/// Shape cast versus a convex polygon.+B2_API b2CastOutput b2CastShapePolygon( const b2Polygon* shape, const b2ShapeCastInput* input );++/// A convex hull. Used to create convex polygons.+/// @warning Do not modify these values directly, instead use b2ComputeHull()+typedef struct b2Hull+{+	/// The final points of the hull+	b2Vec2 points[B2_MAX_POLYGON_VERTICES];++	/// The number of points+	int count;+} b2Hull;++/// Compute the convex hull of a set of points. Returns an empty hull if it fails. Some failure+/// cases:+/// - all points very close together+/// - all points on a line+/// - less than 3 points+/// - more than B2_MAX_POLYGON_VERTICES points+///+/// This welds close points and removes collinear points.+/// @warning Do not modify a hull once it has been computed+B2_API b2Hull b2ComputeHull( const b2Vec2* points, int count );++/// This determines if a hull is valid. Checks for:+/// - convexity+/// - collinear points+///+/// This is expensive and should not be called at runtime.+B2_API bool b2ValidateHull( const b2Hull* hull );++/**@}*/++/**+ * @defgroup distance Distance+ * Functions for computing the distance between shapes.+ *+ * These are advanced functions you can use to perform distance calculations. There are functions+ * for computing the closest points between shapes, doing linear shape casts, and doing rotational+ * shape casts. The latter is called time of impact (TOI).+ * @{+ */++/// Result of computing the distance between two line segments+typedef struct b2SegmentDistanceResult+{+	/// The closest point on the first segment+	b2Vec2 closest1;++	/// The closest point on the second segment+	b2Vec2 closest2;++	/// The barycentric coordinate on the first segment+	float fraction1;++	/// The barycentric coordinate on the second segment+	float fraction2;++	/// The squared distance between the closest points+	float distanceSquared;+} b2SegmentDistanceResult;++/// Compute the distance between two line segments, clamping at the end points if needed.+B2_API b2SegmentDistanceResult b2ComputeSegmentDistance( b2Vec2 p1, b2Vec2 q1, b2Vec2 p2,+														 b2Vec2 q2 );++/// Used to warm start the GJK simplex. If you call b2ComputeShapeDistance multiple times with+/// nearby transforms this might improve performance. The distance cache must be initialized to zero+/// on the first call.+typedef struct b2SimplexCache+{+	/// The number of stored simplex points+	uint16_t count;++	/// The cached simplex indices on shape A+	uint8_t indexA[3];++	/// The cached simplex indices on shape B+	uint8_t indexB[3];+} b2SimplexCache;++static const b2SimplexCache b2_emptySimplexCache = B2_ZERO_INIT;++/// Input for b2ComputeShapeDistance+typedef struct b2DistanceInput+{+	/// The proxy for shape A+	b2ShapeProxy proxyA;++	/// The proxy for shape B+	b2ShapeProxy proxyB;++	/// Transform of shape B in shape A's frame, the relative pose B in A (b2InvMulTransforms(+	/// worldA, worldB )). The query is origin independent and runs in frame A.+	b2Transform transform;++	/// Should the proxy radius be considered?+	bool useRadii;+} b2DistanceInput;++/// Output for b2ComputeShapeDistance+typedef struct b2DistanceOutput+{+	b2Vec2 pointA;	  ///< Closest point on shapeA, in shape A's frame+	b2Vec2 pointB;	  ///< Closest point on shapeB, in shape A's frame+	b2Vec2 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+} b2DistanceOutput;++/// Simplex vertex for debugging the GJK algorithm+typedef struct b2SimplexVertex+{+	b2Vec2 wA;	///< support point in proxyA+	b2Vec2 wB;	///< support point in proxyB+	b2Vec2 w;	///< wB - wA+	float a;	///< barycentric coordinate for closest point+	int indexA; ///< wA index+	int indexB; ///< wB index+} b2SimplexVertex;++/// Simplex from the GJK algorithm+typedef struct b2Simplex+{+	b2SimplexVertex v1, v2, v3; ///< vertices+	int count;					///< number of valid vertices+} b2Simplex;++/// Compute the closest points between two shapes represented as point clouds. b2SimplexCache cache+/// is input/output. On the first call set b2SimplexCache.count to zero. The underlying GJK+/// algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0+/// for these.+B2_API b2DistanceOutput b2ComputeShapeDistance( const b2DistanceInput* input, b2SimplexCache* cache,+												b2Simplex* simplexes, int simplexCapacity );++/// Input parameters for b2CastShape+typedef struct b2ShapeCastPairInput+{+	b2ShapeProxy proxyA;   ///< The proxy for shape A+	b2ShapeProxy proxyB;   ///< The proxy for shape B+	b2Transform transform; ///< Transform of shape B in shape A's frame, the relative pose B in A+	b2Vec2 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+} b2ShapeCastPairInput;++/// 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 return a zero fraction in the output.+B2_API b2CastOutput b2CastShape( const b2ShapeCastPairInput* input );++/// Make a proxy for use in overlap, shape cast, and related functions. This is a deep copy of the+/// points.+B2_API b2ShapeProxy b2MakeProxy( const b2Vec2* points, int count, float radius );++/// Make a proxy with a transform. This is a deep copy of the points.+B2_API b2ShapeProxy b2MakeOffsetProxy( const b2Vec2* points, int count, float radius,+									   b2Vec2 position, b2Rot rotation );++/// 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 b2Sweep+{+	b2Vec2 localCenter; ///< Local center of mass position+	b2Vec2 c1;			///< Starting center of mass world position+	b2Vec2 c2;			///< Ending center of mass world position+	b2Rot q1;			///< Starting world rotation+	b2Rot q2;			///< Ending world rotation+} b2Sweep;++/// Evaluate the transform sweep at a specific time.+B2_API b2Transform b2GetSweepTransform( const b2Sweep* sweep, float time );++/// Time of impact input+typedef struct b2TOIInput+{+	b2ShapeProxy proxyA; ///< The proxy for shape A+	b2ShapeProxy proxyB; ///< The proxy for shape B+	b2Sweep sweepA;		 ///< The movement of shape A+	b2Sweep sweepB;		 ///< The movement of shape B+	float maxFraction;	 ///< Defines the sweep interval [0, maxFraction]+} b2TOIInput;++/// Describes the TOI output+typedef enum b2TOIState+{+	b2_toiStateUnknown,+	b2_toiStateFailed,+	b2_toiStateOverlapped,+	b2_toiStateHit,+	b2_toiStateSeparated+} b2TOIState;++/// Time of impact output+typedef struct b2TOIOutput+{+	/// The type of result+	b2TOIState state;++	/// The hit point+	b2Vec2 point;++	/// The hit normal+	b2Vec2 normal;++	/// The sweep time of the collision+	float fraction;+} b2TOIOutput;++/// Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction+/// between [0,maxFraction]. 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.+B2_API b2TOIOutput b2ComputeTimeOfImpact( const b2TOIInput* input );++/**@}*/++/**+ * @defgroup collision Collision+ * @brief Functions for colliding pairs of shapes+ *+ * Each function takes the shapes in their own local frames and the transform of shape B in shape+ * A's frame (b2InvMulWorldTransforms( worldA, worldB )). The manifold is returned in frame A.+ * @{+ */++/// A manifold point is a contact point belonging to a contact manifold. It holds details related to+/// the geometry and dynamics of the contact points. Box2D uses speculative collision so some+/// contact points may be separated. You may use the totalNormalImpulse to determine if there was an+/// interaction during the time step.+typedef struct b2ManifoldPoint+{+	/// Location of the contact point relative to bodyA's center of mass in world space. This can be+	/// converted to a world point using:+	/// b2Pos worldPointA = b2OffsetPos(b2Body_GetWorldCenter(myBodyIdA), anchorA);+	b2Vec2 anchorA;++	/// Similar for body B.+	b2Vec2 anchorB;++	/// The separation of the contact point, negative if penetrating+	float separation;++	/// The impulse along the manifold normal vector.+	float normalImpulse;++	/// The friction impulse+	float tangentImpulse;++	/// The total normal impulse applied across sub-stepping and restitution. This is important to+	/// identify speculative contact points that had an interaction in the time step. This includes+	/// the warm starting impulse, the sub-step delta impulse, and the restitution impulse.+	float totalNormalImpulse;++	/// Relative normal velocity pre-solve. Negative when approaching. This is only computed if hit+	/// events are enabled. It may be computed otherwise as a side effect of SIMD bundling, but that+	/// is not guaranteed.+	float normalVelocity;++	/// Cached separation used for contact recycling+	float baseSeparation;++	/// Uniquely identifies a contact point between two shapes+	uint16_t id;++	/// Did this contact point exist the previous step?+	bool persisted;+} b2ManifoldPoint;++/// A contact manifold describes the contact points between colliding shapes.+/// @note Box2D uses speculative collision so some contact points may be separated.+typedef struct b2Manifold+{+	/// The unit normal vector in world space, points from shape A to shape B.+	b2Vec2 normal;++	/// Angular impulse applied for rolling resistance. N * m * s = kg * m^2 / s+	float rollingImpulse;++	/// The manifold points, up to two are possible in 2D.+	b2ManifoldPoint points[2];++	/// The number of contact points, will be 0, 1, or 2.+	int pointCount;++} b2Manifold;++/// Contact manifold point in local coordinates (frame A).+typedef struct b2LocalManifoldPoint+{+	/// Contact point in frame A+	b2Vec2 point;++	/// The separation of the contact point, negative if penetrating. May be positive or negative.+	float separation;++	/// Uniquely identifies a contact point between two shapes+	uint16_t id;+} b2LocalManifoldPoint;++/// Contact manifold in local coordinates (frame A).+typedef struct b2LocalManifold+{+	/// The unit normal vector in frame A, points from shape A to shape B+	b2Vec2 normal;++	/// The manifold points, up to two are possible in 2D+	b2LocalManifoldPoint points[2];++	/// The number of contact points, will be 0, 1, or 2+	int pointCount;++} b2LocalManifold;++/// Compute the contact manifold between two circles+B2_API b2LocalManifold b2CollideCircles( const b2Circle* circleA, const b2Circle* circleB,+										 b2Transform xf );++/// Compute the contact manifold between a capsule and circle+B2_API b2LocalManifold b2CollideCapsuleAndCircle( const b2Capsule* capsuleA,+												  const b2Circle* circleB, b2Transform xf );++/// Compute the contact manifold between a segment and a circle+B2_API b2LocalManifold b2CollideSegmentAndCircle( const b2Segment* segmentA,+												  const b2Circle* circleB, b2Transform xf );++/// Compute the contact manifold between a polygon and a circle+B2_API b2LocalManifold b2CollidePolygonAndCircle( const b2Polygon* polygonA,+												  const b2Circle* circleB, b2Transform xf );++/// Compute the contact manifold between two capsules+B2_API b2LocalManifold b2CollideCapsules( const b2Capsule* capsuleA, const b2Capsule* capsuleB,+										  b2Transform xf );++/// Compute the contact manifold between a segment and a capsule+B2_API b2LocalManifold b2CollideSegmentAndCapsule( const b2Segment* segmentA,+												   const b2Capsule* capsuleB, b2Transform xf );++/// Compute the contact manifold between a polygon and capsule+B2_API b2LocalManifold b2CollidePolygonAndCapsule( const b2Polygon* polygonA,+												   const b2Capsule* capsuleB, b2Transform xf );++/// Compute the contact manifold between two polygons+B2_API b2LocalManifold b2CollidePolygons( const b2Polygon* polygonA, const b2Polygon* polygonB,+										  b2Transform xf );++/// Compute the contact manifold between a segment and a polygon+B2_API b2LocalManifold b2CollideSegmentAndPolygon( const b2Segment* segmentA,+												   const b2Polygon* polygonB, b2Transform xf );++/// Compute the contact manifold between a chain segment and a circle+B2_API b2LocalManifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA,+													   const b2Circle* circleB, b2Transform xf );++/// Compute the contact manifold between a chain segment and a capsule+B2_API b2LocalManifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA,+														const b2Capsule* capsuleB, b2Transform xf,+														b2SimplexCache* cache );++/// Compute the contact manifold between a chain segment and a rounded polygon+B2_API b2LocalManifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA,+														const b2Polygon* polygonB, b2Transform xf,+														b2SimplexCache* cache );++/**@}*/++/**+ * @defgroup tree Dynamic Tree+ * The dynamic tree is a binary AABB tree to organize and query large numbers of geometric objects+ *+ * Box2D uses the dynamic tree internally to sort collision shapes into a binary bounding volume+ * hierarchy. This data structure may have uses in games for organizing other geometry data and may+ * be used independently of Box2D rigid body simulation.+ *+ * A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. A dynamic tree arranges+ * data in a binary tree to accelerate queries such as AABB queries and ray casts. Leaf nodes are+ * proxies with an AABB. These are used to hold a user collision object. Nodes are pooled and+ * relocatable, so I use node indices rather than pointers. The dynamic tree is made available for+ * advanced users that would like to use it to organize spatial game data besides rigid bodies.+ * @{+ */++/// Opaque dynamic tree. Details are internal.+typedef struct b2DynamicTree b2DynamicTree;++/// These are performance results returned by dynamic tree queries.+typedef struct b2TreeStats+{+	/// Number of internal nodes visited during the query+	int nodeVisits;++	/// Number of leaf nodes visited during the query+	int leafVisits;+} b2TreeStats;++/// Constructing the tree initializes the node pool.+B2_API b2DynamicTree* b2CreateDynamicTree( int proxyCapacity );++/// Destroy the tree, freeing the node pool.+B2_API void b2DestroyDynamicTree( b2DynamicTree* tree );++/// Create a proxy. Provide an AABB and a userData value.+B2_API int b2CreateTreeProxy( b2DynamicTree* tree, b2AABB aabb, uint64_t categoryBits,+							  uint64_t userData );++/// Destroy a proxy. This asserts if the id is invalid.+B2_API void b2DestroyTreeProxy( b2DynamicTree* tree, int proxyId );++/// Move a proxy to a new AABB by removing and reinserting into the tree.+B2_API void b2DynamicTree_MoveProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb );++/// Modify the category bits on a proxy.+B2_API void b2DynamicTree_SetCategoryBits( b2DynamicTree* tree, int proxyId,+										   uint64_t categoryBits );++/// Get the category bits on a proxy.+B2_API uint64_t b2DynamicTree_GetCategoryBits( b2DynamicTree* tree, int proxyId );++/// This function receives proxies found in the AABB query.+/// @return true if the query should continue+typedef bool b2TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* context );++/// Query an AABB for overlapping proxies. The callback is called for each proxy that overlaps the+/// supplied AABB.+/// @return performance data+B2_API b2TreeStats b2DynamicTree_OverlapAABB( const b2DynamicTree* tree, b2AABB aabb,+											  uint64_t maskBits, b2TreeQueryCallbackFcn* callback,+											  void* context );++/// Query an AABB for overlapping proxies. The callback is called for each proxy that overlaps the+/// supplied AABB. No filtering is performed.+/// @return performance data+B2_API b2TreeStats b2DynamicTree_OverlapAABBAll( const b2DynamicTree* tree, b2AABB aabb,+												 b2TreeQueryCallbackFcn* callback, void* context );++/// This function receives clipped ray cast input for a proxy. The function returns the new ray+/// fraction.+/// - return a value of 0 to terminate the ray cast+/// - return a value less than input->maxFraction to clip the ray+/// - return a value of input->maxFraction to continue the ray cast without clipping+typedef float b2TreeRayCastCallbackFcn( const b2RayCastInput* input, int proxyId, uint64_t userData,+										void* context );++/// 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 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.+/// @param maskBits mask bits used to filter proxies.+/// @param callback a callback 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+B2_API b2TreeStats b2DynamicTree_CastRay( const b2DynamicTree* tree, const b2RayCastInput* input,+										  uint64_t maskBits, b2TreeRayCastCallbackFcn* callback,+										  void* context );++/// Input for casting an AABB through a dynamic tree+typedef struct b2BoxCastInput+{+	/// The AABB to cast, in the tree's frame.+	b2AABB box;++	/// The sweep translation.+	b2Vec2 translation;++	/// The maximum fraction of the translation to consider, typically 1.+	float maxFraction;+} b2BoxCastInput;++/// 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 b2TreeBoxCastCallbackFcn( const b2BoxCastInput* input, int proxyId, uint64_t userData,+										void* context );++/// Cast a swept AABB through the tree. This has performance roughly equal to k * log(n), where k is+/// the number of collisions and n is the number of proxies in the tree.+/// @param tree the dynamic tree to cast through+/// @param input the AABB cast input. The box sweeps from its origin to origin + maxFraction *+/// translation.+/// @param maskBits filter bits: `bool accept = (maskBits & proxy->categoryBits) != 0;`+/// @param callback a callback that is called for each proxy the swept box may hit+/// @param context user context that is passed to the callback+/// @return performance data+B2_API b2TreeStats b2DynamicTree_CastBox( const b2DynamicTree* tree, const b2BoxCastInput* input,+										  uint64_t maskBits, b2TreeBoxCastCallbackFcn* callback,+										  void* context );++/// Get the height of the binary tree.+B2_API int b2DynamicTree_GetHeight( const b2DynamicTree* tree );++/// Get the ratio of the sum of the internal node areas to the root area.+B2_API float b2DynamicTree_GetAreaRatio( const b2DynamicTree* tree );++/// Get the bounding box that contains the entire tree+B2_API b2AABB b2DynamicTree_GetRootBounds( const b2DynamicTree* tree );++/// Get the number of proxies created+B2_API int b2DynamicTree_GetProxyCount( const b2DynamicTree* tree );++/// Get the number of bytes used by this tree+B2_API size_t b2DynamicTree_GetByteCount( const b2DynamicTree* tree );++/// Get proxy user data+B2_API uint64_t b2DynamicTree_GetUserData( const b2DynamicTree* tree, int proxyId );++/// Get the AABB of a proxy+B2_API b2AABB b2DynamicTree_GetProxyBounds( const b2DynamicTree* tree, int proxyId );++/// Validate the tree.+B2_API void b2DynamicTree_Validate( const b2DynamicTree* tree );++/**@}*/++/**+ * @defgroup character Character mover+ * Character movement solver+ * @{+ */++/// These are the collision planes returned from b2World_CollideMover. The plane and point are+/// relative to the query origin, matching the mover capsule.+typedef struct b2PlaneResult+{+	/// The collision plane between the mover and a convex shape+	b2Plane plane;++	/// The collision point on the shape.+	b2Vec2 point;++	/// Did the collision register a hit? If not this plane should be ignored.+	bool hit;+} b2PlaneResult;++/// These are collision planes that can be fed to b2SolvePlanes. Normally this is assembled by the+/// user from plane results in b2PlaneResult.+typedef struct b2CollisionPlane+{+	/// The collision plane between the mover and some shape.+	b2Plane plane;++	/// Setting this to FLT_MAX makes the plane as rigid as possible. Lower values can make the+	/// plane collision soft. Usually in meters.+	float pushLimit;++	/// The push on the mover determined by b2SolvePlanes. Usually in meters.+	float push;++	/// Indicates if b2ClipVector should clip against this plane. Should be false for soft+	/// collision.+	bool clipVelocity;+} b2CollisionPlane;++/// Result returned by b2SolvePlanes.+typedef struct b2PlaneSolverResult+{+	/// The final relative translation.+	b2Vec2 delta;++	/// The number of iterations used by the plane solver. For diagnostics.+	int iterationCount;+} b2PlaneSolverResult;++/// Solves the position of a mover that satisfies the given collision planes.+/// @param targetDelta the desired translation from the position used to generate the collision+/// planes+/// @param planes the collision planes+/// @param count the number of collision planes+B2_API b2PlaneSolverResult b2SolvePlanes( b2Vec2 targetDelta, b2CollisionPlane* planes, int count );++/// Clips the velocity against the given collision planes. Planes with zero push or clipVelocity set+/// to false are skipped.+B2_API b2Vec2 b2ClipVector( b2Vec2 vector, const b2CollisionPlane* planes, int count );++/**@}*/
+ upstream/include/box2d/config.h view
@@ -0,0 +1,30 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++// Box2D compile-time options.+//+// Normally set by the CMake BOX2D_* options. If you build Box2D without its+// CMake (dropping the sources into another project), set them here so the+// library and your code agree. Edit this file, or keep your settings elsewhere+// and point Box2D at them from your build:+//+//   #define BOX2D_USER_CONFIG "my_box2d_config.h"+//+// A define passed on the compiler command line still wins over this file.++// Large world mode. Stores world positions in double precision. Affects ABI.+// #define BOX2D_DOUBLE_PRECISION++// Build the scalar fallback instead of AVX2/SSE2/NEON.+// #define BOX2D_DISABLE_SIMD++// Enable internal validation in debug builds.+// #define BOX2D_VALIDATE++// Decorate the public API with your own export macro instead of Box2D's+// box2d_EXPORTS/BOX2D_DLL scheme, for example when compiling Box2D into another+// shared library. A single value cannot switch between dllexport and dllimport,+// so this suits embedding more than shipping Box2D as its own DLL.+// #define BOX2D_EXPORT MYENGINE_API
+ upstream/include/box2d/constants.h view
@@ -0,0 +1,87 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "math_functions.h"++/**+ * @defgroup constants Constants+ * Constants. Some can be configured. I try to include most internal constants here for convenience+ * and clarity. This is why they are not all configurable.+ * @{+ */++/// Used to detect bad values. Positions greater than about 16km will have precision problems in+/// single precision, so 100km as a limit should be fine in all cases. In large world mode the+/// broad-phase starts to have excessive padding at 10,000km, so a value beyond 100,000km is+/// extreme.+#if defined( BOX2D_DOUBLE_PRECISION )+#define B2_HUGE ( ( 100000.0f * 1000.0f ) * b2_lengthUnitsPerMeter )+#else+#define B2_HUGE ( ( 100.0f * 1000.0f ) * b2_lengthUnitsPerMeter )+#endif++/// Maximum parallel workers. Used for some fixed size arrays.+#define B2_MAX_WORKERS 32++/// Maximum number of tasks queued per world step. b2EnqueueTaskCallbackFcn will never be called+/// more than this per world step. This is related to B2_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+/// Box2D tasks, which may help with creating stable user task pointers.+#define B2_MAX_TASKS 256++/// Maximum number of colors in the constraint graph. Constraints that cannot find a color are added+/// to the overflow set, which is solved single-threaded. The compound barrel benchmark has minor+/// overflow with 24 colors+#define B2_GRAPH_COLOR_COUNT 24++/// A small length used as a collision and constraint tolerance. Usually it is chosen to be+/// numerically significant, but visually insignificant. In meters. Normally this is 0.5cm.+/// @warning modifying this can have a significant impact on stability+#define B2_LINEAR_SLOP ( 0.005f * b2_lengthUnitsPerMeter )++/// Maximum number of simultaneous worlds that can be allocated+#ifndef B2_MAX_WORLDS+#define B2_MAX_WORLDS 128+#endif++/// Maximum length of the body name. Can be 0 if you don't need names.+#ifndef B2_NAME_LENGTH+#define B2_NAME_LENGTH 10+#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 * B2_PI or greater will break continuous collision.+#define B2_MAX_ROTATION ( 0.25f * B2_PI )++/// Box2D uses limited speculative collision. This reduces jitter. Normally this is 2cm.+/// @warning modifying this can have a significant impact on performance and stability+#define B2_SPECULATIVE_DISTANCE ( 4.0f * B2_LINEAR_SLOP )++/// The default contact recycling distance.+#define B2_CONTACT_RECYCLE_DISTANCE ( 10.0f * B2_LINEAR_SLOP )++/// The default contact recycling world angle threshold. 0.98 ~= 11.5 degrees+#define B2_CONTACT_RECYCLE_COS_ANGLE ( 0.98f )++/// 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. Normally this is 5cm. This has been+/// tuned on benchmarks and I found that keeping the margin small improves performance by reducing+/// the number of contacts. I used to keep this larger in the past to avoid tree insert and remove+/// churn. But the broad-phase no longer works that way and now uses a two-phase refit and rebuild+/// setup similar to Jolt.+/// @warning modifying this can have a significant impact on performance+#define B2_MAX_AABB_MARGIN ( 0.05f * b2_lengthUnitsPerMeter )++/// For small objects the margin is limited to this fraction times the maximum extent+#define B2_AABB_MARGIN_FRACTION 0.125f++/// The time that a body must be still before it will go to sleep. In seconds.+#define B2_TIME_TO_SLEEP 0.5f++/// The maximum number of restitution iterations. Needed to avoid a solver overflow.+#define B2_MAX_RESTITUTION_ITERATIONS 63++/** @} */
+ upstream/include/box2d/id.h view
@@ -0,0 +1,202 @@+// SPDX-FileCopyrightText: 2023 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 Box2D objects. These should be considered opaque data and+ * passed by value. Include this header if you need the id types and not the whole Box2D API. All+ * ids are considered null if initialized to zero.+ *+ * For example in C++:+ *+ * @code{.cxx}+ * b2WorldId worldId = {};+ * @endcode+ *+ * Or in C:+ *+ * @code{.c}+ * b2WorldId worldId = {0};+ * @endcode+ *+ * These are both considered null.+ *+ * @warning Do not use the internals of these ids. They are subject to change. Ids should be treated+ * as opaque objects.+ * @warning You should use ids to access objects in Box2D. Do not access files within the src+ * folder. Such usage is unsupported.+ * @{+ */++/// World id references a world instance. This should be treated as an opaque handle.+typedef struct b2WorldId+{+	uint16_t index1;+	uint16_t generation;+} b2WorldId;++/// Body id references a body instance. This should be treated as an opaque handle.+typedef struct b2BodyId+{+	int32_t index1;+	uint16_t world0;+	uint16_t generation;+} b2BodyId;++/// Shape id references a shape instance. This should be treated as an opaque handle.+typedef struct b2ShapeId+{+	int32_t index1;+	uint16_t world0;+	uint16_t generation;+} b2ShapeId;++/// Chain id references a chain instance. This should be treated as an opaque handle.+typedef struct b2ChainId+{+	int32_t index1;+	uint16_t world0;+	uint16_t generation;+} b2ChainId;++/// Joint id references a joint instance. This should be treated as an opaque handle.+typedef struct b2JointId+{+	int32_t index1;+	uint16_t world0;+	uint16_t generation;+} b2JointId;++/// Contact id references a contact instance. This should be treated as an opaque handle.+typedef struct b2ContactId+{+	int32_t index1;+	uint16_t world0;+	int16_t padding;+	uint32_t generation;+} b2ContactId;++// clang-format off+#ifdef __cplusplus+/// A null id. Works for any id type.+#define B2_NULL_ID {}+/// Inline function.+#define B2_ID_INLINE inline+#else+/// A null id. Works for any id type.+#define B2_NULL_ID { 0 }+/// Inline function.+#define B2_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 b2WorldId b2_nullWorldId = B2_NULL_ID;+static const b2BodyId b2_nullBodyId = B2_NULL_ID;+static const b2ShapeId b2_nullShapeId = B2_NULL_ID;+static const b2ChainId b2_nullChainId = B2_NULL_ID;+static const b2JointId b2_nullJointId = B2_NULL_ID;+static const b2ContactId b2_nullContactId = B2_NULL_ID;++/// Macro to determine if any id is null.+#define B2_IS_NULL( id ) ( ( id ).index1 == 0 )++/// Macro to determine if any id is non-null.+#define B2_IS_NON_NULL( id ) ( ( id ).index1 != 0 )++/// Compare two ids for equality. Doesn't work for b2WorldId. Don't mix types.+#define B2_ID_EQUALS( id1, id2 )                                                                   \+	( ( id1 ).index1 == ( id2 ).index1 && ( id1 ).world0 == ( id2 ).world0 &&                      \+	  ( id1 ).generation == ( id2 ).generation )++/// Store a world id into a uint32_t.+B2_ID_INLINE uint32_t b2StoreWorldId( b2WorldId id )+{+	return ( (uint32_t)id.index1 << 16 ) | (uint32_t)id.generation;+}++/// Load a uint32_t into a world id.+B2_ID_INLINE b2WorldId b2LoadWorldId( uint32_t x )+{+	b2WorldId id = { (uint16_t)( x >> 16 ), (uint16_t)( x ) };+	return id;+}++/// Store a body id into a uint64_t.+B2_ID_INLINE uint64_t b2StoreBodyId( b2BodyId id )+{+	return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation;+}++/// Load a uint64_t into a body id.+B2_ID_INLINE b2BodyId b2LoadBodyId( uint64_t x )+{+	b2BodyId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) };+	return id;+}++/// Store a shape id into a uint64_t.+B2_ID_INLINE uint64_t b2StoreShapeId( b2ShapeId id )+{+	return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation;+}++/// Load a uint64_t into a shape id.+B2_ID_INLINE b2ShapeId b2LoadShapeId( uint64_t x )+{+	b2ShapeId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) };+	return id;+}++/// Store a chain id into a uint64_t.+B2_ID_INLINE uint64_t b2StoreChainId( b2ChainId id )+{+	return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation;+}++/// Load a uint64_t into a chain id.+B2_ID_INLINE b2ChainId b2LoadChainId( uint64_t x )+{+	b2ChainId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) };+	return id;+}++/// Store a joint id into a uint64_t.+B2_ID_INLINE uint64_t b2StoreJointId( b2JointId id )+{+	return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation;+}++/// Load a uint64_t into a joint id.+B2_ID_INLINE b2JointId b2LoadJointId( uint64_t x )+{+	b2JointId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) };+	return id;+}++/// Store a contact id.+B2_ID_INLINE void b2StoreContactId( b2ContactId 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.+B2_ID_INLINE b2ContactId b2LoadContactId( uint32_t values[3] )+{+	b2ContactId 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/box2d/math_functions.h view
@@ -0,0 +1,858 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "base.h"+#include "math_types.h"++#include <float.h>+#include <math.h>+#include <stdbool.h>++/**+ * @addtogroup math+ * @{+ */++/// Is this a valid number? Not NaN or infinity.+B2_API bool b2IsValidFloat( float a );++/// Is this a valid vector? Not NaN or infinity.+B2_API bool b2IsValidVec2( b2Vec2 v );++/// Is this a valid rotation? Not NaN or infinity. Is normalized.+B2_API bool b2IsValidRot( b2Rot q );++/// Is this a valid transform? Not NaN or infinity. Rotation is normalized.+B2_API bool b2IsValidTransform( b2Transform t );++/// Is this a valid bounding box? Not NaN or infinity. Upper bound greater than or equal to lower+/// bound.+B2_API bool b2IsValidAABB( b2AABB aabb );++/// Is this a valid plane? Normal is a unit vector. Not NaN or infinity.+B2_API bool b2IsValidPlane( b2Plane a );++/// Is this a valid world position? Not NaN or infinity.+B2_API bool b2IsValidPos( b2Pos p );++/// Is this a valid world transform? Not NaN or infinity. Rotation is normalized.+B2_API bool b2IsValidWorldTransform( b2WorldTransform t );++/// @return the minimum of two integers+B2_INLINE int b2MinInt( int a, int b )+{+	return a < b ? a : b;+}++/// @return the maximum of two integers+B2_INLINE int b2MaxInt( int a, int b )+{+	return a > b ? a : b;+}++/// @return the absolute value of an integer+B2_INLINE int b2AbsInt( int a )+{+	return a < 0 ? -a : a;+}++/// @return an integer clamped between a lower and upper bound+B2_INLINE int b2ClampInt( int a, int lower, int upper )+{+	return a < lower ? lower : ( a > upper ? upper : a );+}++/// https://en.wikipedia.org/wiki/Floor_and_ceiling_functions+B2_INLINE int b2CeilingInt( int numerator, int denominator )+{+	B2_VALIDATE( denominator > 0 && numerator >= 0 );+	return ( numerator + denominator - 1 ) / denominator;+}++/// @return the minimum of two floats+B2_INLINE float b2MinFloat( float a, float b )+{+	return a < b ? a : b;+}++/// @return the maximum of two floats+B2_INLINE float b2MaxFloat( float a, float b )+{+	return a > b ? a : b;+}++/// @return the absolute value of a float+B2_INLINE float b2AbsFloat( float a )+{+	return a < 0 ? -a : a;+}++/// @return a float clamped between a lower and upper bound+B2_INLINE float b2ClampFloat( float a, float lower, float upper )+{+	return a < lower ? lower : ( a > upper ? upper : a );+}++/// 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+B2_API float b2Atan2( float y, float x );++/// Compute the cosine and sine of an angle in radians. Implemented for cross-platform determinism.+B2_API b2CosSin b2ComputeCosSin( float radians );++/// Vector dot product+B2_INLINE float b2Dot( b2Vec2 a, b2Vec2 b )+{+	return a.x * b.x + a.y * b.y;+}++/// Vector cross product. In 2D this yields a scalar.+B2_INLINE float b2Cross( b2Vec2 a, b2Vec2 b )+{+	return a.x * b.y - a.y * b.x;+}++/// Perform the cross product on a vector and a scalar. In 2D this produces a vector.+B2_INLINE b2Vec2 b2CrossVS( b2Vec2 v, float s )+{+	return B2_LITERAL( b2Vec2 ){ s * v.y, -s * v.x };+}++/// Perform the cross product on a scalar and a vector. In 2D this produces a vector.+B2_INLINE b2Vec2 b2CrossSV( float s, b2Vec2 v )+{+	return B2_LITERAL( b2Vec2 ){ -s * v.y, s * v.x };+}++/// Get a left pointing perpendicular vector. Equivalent to b2CrossSV(1.0f, v)+B2_INLINE b2Vec2 b2LeftPerp( b2Vec2 v )+{+	return B2_LITERAL( b2Vec2 ){ -v.y, v.x };+}++/// Get a right pointing perpendicular vector. Equivalent to b2CrossVS(v, 1.0f)+B2_INLINE b2Vec2 b2RightPerp( b2Vec2 v )+{+	return B2_LITERAL( b2Vec2 ){ v.y, -v.x };+}++/// Vector addition+B2_INLINE b2Vec2 b2Add( b2Vec2 a, b2Vec2 b )+{+	return B2_LITERAL( b2Vec2 ){ a.x + b.x, a.y + b.y };+}++/// Vector subtraction+B2_INLINE b2Vec2 b2Sub( b2Vec2 a, b2Vec2 b )+{+	return B2_LITERAL( b2Vec2 ){ a.x - b.x, a.y - b.y };+}++/// Vector negation+B2_INLINE b2Vec2 b2Neg( b2Vec2 a )+{+	return B2_LITERAL( b2Vec2 ){ -a.x, -a.y };+}++/// Vector linear interpolation+/// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/+B2_INLINE b2Vec2 b2Lerp( b2Vec2 a, b2Vec2 b, float t )+{+	return B2_LITERAL( b2Vec2 ){ ( 1.0f - t ) * a.x + t * b.x, ( 1.0f - t ) * a.y + t * b.y };+}++/// Component-wise multiplication+B2_INLINE b2Vec2 b2Mul( b2Vec2 a, b2Vec2 b )+{+	return B2_LITERAL( b2Vec2 ){ a.x * b.x, a.y * b.y };+}++/// Multiply a scalar and vector+B2_INLINE b2Vec2 b2MulSV( float s, b2Vec2 v )+{+	return B2_LITERAL( b2Vec2 ){ s * v.x, s * v.y };+}++/// a + s * b+B2_INLINE b2Vec2 b2MulAdd( b2Vec2 a, float s, b2Vec2 b )+{+	return B2_LITERAL( b2Vec2 ){ a.x + s * b.x, a.y + s * b.y };+}++/// a - s * b+B2_INLINE b2Vec2 b2MulSub( b2Vec2 a, float s, b2Vec2 b )+{+	return B2_LITERAL( b2Vec2 ){ a.x - s * b.x, a.y - s * b.y };+}++/// Component-wise absolute vector+B2_INLINE b2Vec2 b2Abs( b2Vec2 a )+{+	b2Vec2 b;+	b.x = b2AbsFloat( a.x );+	b.y = b2AbsFloat( a.y );+	return b;+}++/// Component-wise minimum vector+B2_INLINE b2Vec2 b2Min( b2Vec2 a, b2Vec2 b )+{+	b2Vec2 c;+	c.x = b2MinFloat( a.x, b.x );+	c.y = b2MinFloat( a.y, b.y );+	return c;+}++/// Component-wise maximum vector+B2_INLINE b2Vec2 b2Max( b2Vec2 a, b2Vec2 b )+{+	b2Vec2 c;+	c.x = b2MaxFloat( a.x, b.x );+	c.y = b2MaxFloat( a.y, b.y );+	return c;+}++/// Component-wise clamp vector v into the range [a, b]+B2_INLINE b2Vec2 b2Clamp( b2Vec2 v, b2Vec2 a, b2Vec2 b )+{+	b2Vec2 c;+	c.x = b2ClampFloat( v.x, a.x, b.x );+	c.y = b2ClampFloat( v.y, a.y, b.y );+	return c;+}++/// Get the length of this vector (the norm)+B2_INLINE float b2Length( b2Vec2 v )+{+	return sqrtf( v.x * v.x + v.y * v.y );+}++/// Get the distance between two points+B2_INLINE float b2Distance( b2Vec2 a, b2Vec2 b )+{+	float dx = b.x - a.x;+	float dy = b.y - a.y;+	return sqrtf( dx * dx + dy * dy );+}++/// Convert a vector into a unit vector if possible, otherwise returns the zero vector.+B2_FORCE_INLINE b2Vec2 b2Normalize( b2Vec2 a )+{+	float lengthSquared = a.x * a.x + a.y * a.y;+	if ( lengthSquared > 1000.0f * FLT_MIN )+	{+		float s = 1.0f / sqrtf( lengthSquared );+		b2Vec2 u = { s * a.x, s * a.y };+		return u;+	}+	return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f };+}++/// Determines if the provided vector is normalized (norm(a) == 1).+B2_INLINE bool b2IsNormalized( b2Vec2 a )+{+	float aa = b2Dot( a, a );+	return b2AbsFloat( 1.0f - aa ) < 100.0f * FLT_EPSILON;+}++/// Convert a vector into a unit vector if possible, otherwise returns the zero vector. Also outputs+/// the length.+B2_INLINE b2Vec2 b2GetLengthAndNormalize( float* length, b2Vec2 a )+{+	float lengthSquared = a.x * a.x + a.y * a.y;+	if ( lengthSquared > 1000.0f * FLT_MIN )+	{+		*length = sqrtf( lengthSquared );+		float s = 1.0f / *length;+		b2Vec2 u = { s * a.x, s * a.y };+		return u;+	}++	*length = 0.0f;+	return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f };+}++/// Normalize rotation+B2_INLINE b2Rot b2NormalizeRot( b2Rot q )+{+	float mag = sqrtf( q.s * q.s + q.c * q.c );+	float invMag = mag > 0.0f ? 1.0f / mag : 0.0f;+	b2Rot qn = { q.c * invMag, q.s * invMag };+	return qn;+}++/// Integrate rotation from angular velocity+/// @param q1 initial rotation+/// @param deltaAngle the angular displacement in radians+B2_INLINE b2Rot b2IntegrateRotation( b2Rot q1, float deltaAngle )+{+	// dc/dt = -omega * sin(t)+	// ds/dt = omega * cos(t)+	// c2 = c1 - omega * h * s1+	// s2 = s1 + omega * h * c1+	b2Rot q2 = { q1.c - deltaAngle * q1.s, q1.s + deltaAngle * q1.c };+	float mag = sqrtf( q2.s * q2.s + q2.c * q2.c );+	float invMag = mag > 0.0f ? 1.0f / mag : 0.0f;+	b2Rot qn = { q2.c * invMag, q2.s * invMag };+	return qn;+}++/// Get the length squared of this vector+B2_INLINE float b2LengthSquared( b2Vec2 v )+{+	return v.x * v.x + v.y * v.y;+}++/// Get the distance squared between points+B2_INLINE float b2DistanceSquared( b2Vec2 a, b2Vec2 b )+{+	b2Vec2 c = { b.x - a.x, b.y - a.y };+	return c.x * c.x + c.y * c.y;+}++/// Make a rotation using an angle in radians+B2_INLINE b2Rot b2MakeRot( float radians )+{+	b2CosSin cs = b2ComputeCosSin( radians );+	return B2_LITERAL( b2Rot ){ cs.cosine, cs.sine };+}++/// Make a rotation using a unit vector+B2_INLINE b2Rot b2MakeRotFromUnitVector( b2Vec2 unitVector )+{+	B2_ASSERT( b2IsNormalized( unitVector ) );+	return B2_LITERAL( b2Rot ){ unitVector.x, unitVector.y };+}++/// Compute the rotation between two unit vectors+B2_API b2Rot b2ComputeRotationBetweenUnitVectors( b2Vec2 v1, b2Vec2 v2 );++/// Is this rotation normalized?+B2_INLINE bool b2IsNormalizedRot( b2Rot q )+{+	// larger tolerance due to failure on mingw 32-bit+	float qq = q.s * q.s + q.c * q.c;+	return 1.0f - 0.0006f < qq && qq < 1.0f + 0.0006f;+}++/// Get the inverse of a rotation+B2_INLINE b2Rot b2InvertRot( b2Rot a )+{+	return B2_LITERAL( b2Rot ){ a.c, -a.s };+}++/// Normalized linear interpolation+/// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/+/// https://web.archive.org/web/20170825184056/http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/+B2_INLINE b2Rot b2NLerp( b2Rot q1, b2Rot q2, float t )+{+	float omt = 1.0f - t;+	b2Rot q = {+		omt * q1.c + t * q2.c,+		omt * q1.s + t * q2.s,+	};++	float mag = sqrtf( q.s * q.s + q.c * q.c );+	float invMag = mag > 0.0f ? 1.0f / mag : 0.0f;+	b2Rot qn = { q.c * invMag, q.s * invMag };+	return qn;+}++/// Compute the angular velocity necessary to rotate between two rotations over a given time+/// @param q1 initial rotation+/// @param q2 final rotation+/// @param inv_h inverse time step+B2_INLINE float b2ComputeAngularVelocity( b2Rot q1, b2Rot q2, float inv_h )+{+	// ds/dt = omega * cos(t)+	// dc/dt = -omega * sin(t)+	// s2 = s1 + omega * h * c1+	// c2 = c1 - omega * h * s1++	// omega * h * s1 = c1 - c2+	// omega * h * c1 = s2 - s1+	// omega * h = (c1 - c2) * s1 + (s2 - s1) * c1;+	// omega * h = s1 * c1 - c2 * s1 + s2 * c1 - s1 * c1+	// omega * h = s2 * c1 - c2 * s1 = sin(a2 - a1) ~= a2 - a1 for small delta+	float omega = inv_h * ( q2.s * q1.c - q2.c * q1.s );+	return omega;+}++/// Get the angle in radians in the range [-pi, pi]+B2_INLINE float b2Rot_GetAngle( b2Rot q )+{+	return b2Atan2( q.s, q.c );+}++/// Get the x-axis+B2_INLINE b2Vec2 b2Rot_GetXAxis( b2Rot q )+{+	b2Vec2 v = { q.c, q.s };+	return v;+}++/// Get the y-axis+B2_INLINE b2Vec2 b2Rot_GetYAxis( b2Rot q )+{+	b2Vec2 v = { -q.s, q.c };+	return v;+}++/// Multiply two rotations: q * r+B2_INLINE b2Rot b2MulRot( b2Rot q, b2Rot r )+{+	// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]+	// [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]+	// s(q + r) = qs * rc + qc * rs+	// c(q + r) = qc * rc - qs * rs+	b2Rot qr;+	qr.s = q.s * r.c + q.c * r.s;+	qr.c = q.c * r.c - q.s * r.s;+	return qr;+}++/// Transpose multiply two rotations: inv(a) * b This rotates a vector local in frame b into a+/// vector local in frame a+B2_INLINE b2Rot b2InvMulRot( b2Rot a, b2Rot b )+{+	// [ ac as] * [bc -bs] = [ac*bc+as*bs -ac*bs+as*bc]+	// [-as ac]   [bs  bc]   [-as*bc+ac*bs as*bs+ac*bc]+	// s(b - a) = ac * bs - as * bc+	// c(b - a) = ac * bc + as * bs+	b2Rot r;+	r.s = a.c * b.s - a.s * b.c;+	r.c = a.c * b.c + a.s * b.s;+	return r;+}++/// The angle of rotation b relative to rotation a. Radians.+B2_INLINE float b2RelativeAngle( b2Rot a, b2Rot b )+{+	// sin(b - a) = bs * ac - bc * as+	// cos(b - a) = bc * ac + bs * as+	float s = a.c * b.s - a.s * b.c;+	float c = a.c * b.c + a.s * b.s;+	return b2Atan2( s, c );+}++/// Convert any angle into the range [-pi, pi].+B2_INLINE float b2UnwindAngle( float radians )+{+	// This function can be implemented using remainderf.+	// However that is a libc dependency not present in WASM.+	// This function doesn't work well with huge angles. So unwind your angles regularly.+	B2_VALIDATE( b2AbsFloat( radians ) < 1.0e4f );+	float x = b2ClampFloat( radians, -1.0e6f, 1.0e6f );++	// Work in doubles. Adding and removing this constant rounds to the nearest integer.+	// https://stackoverflow.com/questions/17035464/a-fast-method-to-round-a-double-to-a-32-bit-int-explained+	double twoPi = 2.0f * B2_PI;+	double roundToNearest = 6755399441055744.0;+	double a = x;+	double k = ( a / twoPi + roundToNearest ) - roundToNearest;+	return (float)( a - k * twoPi );+}++/// Rotate a vector.+B2_INLINE b2Vec2 b2RotateVector( b2Rot q, b2Vec2 v )+{+	return B2_LITERAL( b2Vec2 ){ q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y };+}++/// Inverse rotate a vector.+B2_INLINE b2Vec2 b2InvRotateVector( b2Rot q, b2Vec2 v )+{+	return B2_LITERAL( b2Vec2 ){ q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y };+}++/// Transform a point (e.g. local space to world space).+B2_INLINE b2Vec2 b2TransformPoint( b2Transform t, const b2Vec2 p )+{+	float x = ( t.q.c * p.x - t.q.s * p.y ) + t.p.x;+	float y = ( t.q.s * p.x + t.q.c * p.y ) + t.p.y;++	return B2_LITERAL( b2Vec2 ){ x, y };+}++/// Inverse transform a point (e.g. world space to local space).+B2_INLINE b2Vec2 b2InvTransformPoint( b2Transform t, const b2Vec2 p )+{+	float vx = p.x - t.p.x;+	float vy = p.y - t.p.y;+	return B2_LITERAL( b2Vec2 ){ t.q.c * vx + t.q.s * vy, -t.q.s * vx + t.q.c * vy };+}++/// 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.+/// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p+///    = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p+B2_INLINE b2Transform b2MulTransforms( b2Transform A, b2Transform B )+{+	b2Transform C;+	C.q = b2MulRot( A.q, B.q );+	C.p = b2Add( b2RotateVector( A.q, B.p ), A.p );+	return C;+}++/// Creates a transform that converts a local point in frame B to a local point in frame A.+/// v2 = A.q' * (B.q * v1 + B.p - A.p)+///    = A.q' * B.q * v1 + A.q' * (B.p - A.p)+B2_INLINE b2Transform b2InvMulTransforms( b2Transform A, b2Transform B )+{+	b2Transform C;+	C.q = b2InvMulRot( A.q, B.q );+	C.p = b2InvRotateVector( A.q, b2Sub( B.p, A.p ) );+	return C;+}++/// Convert a vector to a world position. No-op in single precision.+B2_INLINE b2Pos b2ToPos( b2Vec2 v )+{+	return B2_LITERAL( b2Pos ){ v.x, v.y };+}++/// Lossy conversion of a world position to a float vector. No-op in single precision.+B2_INLINE b2Vec2 b2ToVec2( b2Pos p )+{+	return B2_LITERAL( b2Vec2 ){ (float)p.x, (float)p.y };+}++/// Narrow a world coordinate to float, rounding toward negative infinity. Use with b2RoundUpFloat+/// to build a conservative float box that always contains 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.+B2_INLINE float b2RoundDownFloat( double x )+{+#if defined( BOX2D_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.+B2_INLINE float b2RoundUpFloat( double x )+{+#if defined( BOX2D_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.+B2_INLINE b2Vec2 b2SubPos( b2Pos a, b2Pos b )+{+	return B2_LITERAL( b2Vec2 ){ (float)( a.x - b.x ), (float)( a.y - b.y ) };+}++/// p + d+B2_INLINE b2Pos b2OffsetPos( b2Pos p, b2Vec2 d )+{+	return B2_LITERAL( b2Pos ){ p.x + d.x, p.y + d.y };+}++/// World position interpolation for sweeps and sampling.+B2_INLINE b2Pos b2LerpPos( b2Pos a, b2Pos b, float t )+{+	return B2_LITERAL( b2Pos ){ ( 1.0f - t ) * a.x + t * b.x, ( 1.0f - t ) * a.y + t * b.y };+}++/// Transform a local point to a world position. Rotation in float, translation in double.+B2_INLINE b2Pos b2TransformWorldPoint( b2WorldTransform t, b2Vec2 p )+{+	float rx = t.q.c * p.x - t.q.s * p.y;+	float ry = t.q.s * p.x + t.q.c * p.y;+	return B2_LITERAL( b2Pos ){ t.p.x + rx, t.p.y + ry };+}++/// Transform a world position to a local point. One double subtraction, then float.+B2_INLINE b2Vec2 b2InvTransformWorldPoint( b2WorldTransform t, b2Pos p )+{+	float vx = (float)( p.x - t.p.x );+	float vy = (float)( p.y - t.p.y );+	return B2_LITERAL( b2Vec2 ){ t.q.c * vx + t.q.s * vy, -t.q.s * vx + t.q.c * vy };+}++/// Relative transform of frame B in frame A.+B2_INLINE b2Transform b2InvMulWorldTransforms( b2WorldTransform A, b2WorldTransform B )+{+	b2Transform C;+	C.q = b2InvMulRot( A.q, B.q );+	b2Vec2 d = { (float)( B.p.x - A.p.x ), (float)( B.p.y - A.p.y ) };+	C.p = b2InvRotateVector( A.q, d );+	return C;+}++/// Convert a local transform B into world space using world transform A.+B2_INLINE b2WorldTransform b2MulWorldTransforms( b2WorldTransform A, b2Transform B )+{+	b2WorldTransform C;+	C.q = b2MulRot( A.q, B.q );+	C.p = b2OffsetPos( A.p, b2RotateVector( A.q, B.p ) );+	return C;+}++/// Shift a world transform into the frame of a base position.+B2_INLINE b2Transform b2ToRelativeTransform( b2WorldTransform t, b2Pos base )+{+	b2Transform r;+	r.q = t.q;+	r.p = B2_LITERAL( b2Vec2 ){ (float)( t.p.x - base.x ), (float)( t.p.y - base.y ) };+	return r;+}++/// Promote a float transform to a world transform. Lossless.+B2_INLINE b2WorldTransform b2MakeWorldTransform( b2Transform t )+{+	b2WorldTransform w;+	w.p = b2ToPos( t.p );+	w.q = t.q;+	return w;+}++/// Multiply a 2-by-2 matrix times a 2D vector+B2_INLINE b2Vec2 b2MulMV( b2Mat22 A, b2Vec2 v )+{+	b2Vec2 u = {+		A.cx.x * v.x + A.cy.x * v.y,+		A.cx.y * v.x + A.cy.y * v.y,+	};+	return u;+}++/// Get the inverse of a 2-by-2 matrix+B2_INLINE b2Mat22 b2GetInverse22( b2Mat22 A )+{+	float a = A.cx.x, b = A.cy.x, c = A.cx.y, d = A.cy.y;+	float det = a * d - b * c;+	if ( det != 0.0f )+	{+		det = 1.0f / det;+	}++	b2Mat22 B = {+		{ det * d, -det * c },+		{ -det * b, det * a },+	};+	return B;+}++/// Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse+/// in one-shot cases.+B2_INLINE b2Vec2 b2Solve22( b2Mat22 A, b2Vec2 b )+{+	float a11 = A.cx.x, a12 = A.cy.x, a21 = A.cx.y, a22 = A.cy.y;+	float det = a11 * a22 - a12 * a21;+	if ( det != 0.0f )+	{+		det = 1.0f / det;+	}+	b2Vec2 x = { det * ( a22 * b.x - a12 * b.y ), det * ( a11 * b.y - a21 * b.x ) };+	return x;+}++/// Does a fully contain b+B2_INLINE bool b2AABB_Contains( b2AABB a, b2AABB b )+{+	bool s = true;+	s = s && a.lowerBound.x <= b.lowerBound.x;+	s = s && a.lowerBound.y <= b.lowerBound.y;+	s = s && b.upperBound.x <= a.upperBound.x;+	s = s && b.upperBound.y <= a.upperBound.y;+	return s;+}++/// Get the center of the AABB.+B2_INLINE b2Vec2 b2AABB_Center( b2AABB a )+{+	b2Vec2 b = { 0.5f * ( a.lowerBound.x + a.upperBound.x ),+				 0.5f * ( a.lowerBound.y + a.upperBound.y ) };+	return b;+}++/// Get the extents of the AABB (half-widths).+B2_INLINE b2Vec2 b2AABB_Extents( b2AABB a )+{+	b2Vec2 b = { 0.5f * ( a.upperBound.x - a.lowerBound.x ),+				 0.5f * ( a.upperBound.y - a.lowerBound.y ) };+	return b;+}++/// Union of two AABBs+B2_INLINE b2AABB b2AABB_Union( b2AABB a, b2AABB b )+{+	b2AABB c;+	c.lowerBound.x = b2MinFloat( a.lowerBound.x, b.lowerBound.x );+	c.lowerBound.y = b2MinFloat( a.lowerBound.y, b.lowerBound.y );+	c.upperBound.x = b2MaxFloat( a.upperBound.x, b.upperBound.x );+	c.upperBound.y = b2MaxFloat( a.upperBound.y, b.upperBound.y );+	return c;+}++/// Do a and b overlap+B2_INLINE bool b2AABB_Overlaps( b2AABB a, b2AABB b )+{+	return !( b.lowerBound.x > a.upperBound.x || b.lowerBound.y > a.upperBound.y ||+			  a.lowerBound.x > b.upperBound.x || a.lowerBound.y > b.upperBound.y );+}++/// Compute the bounding box of an array of circles+B2_INLINE b2AABB b2MakeAABB( const b2Vec2* points, int count, float radius )+{+	B2_ASSERT( count > 0 );+	b2AABB a = { points[0], points[0] };+	for ( int i = 1; i < count; ++i )+	{+		a.lowerBound = b2Min( a.lowerBound, points[i] );+		a.upperBound = b2Max( a.upperBound, points[i] );+	}++	b2Vec2 r = { radius, radius };+	a.lowerBound = b2Sub( a.lowerBound, r );+	a.upperBound = b2Add( a.upperBound, r );++	return a;+}++/// Signed separation of a point from a plane+B2_INLINE float b2PlaneSeparation( b2Plane plane, b2Vec2 point )+{+	return b2Dot( plane.normal, point ) - plane.offset;+}++/// One-dimensional mass-spring-damper simulation. Returns the new velocity given the position and+/// time step. You can then compute the new position using:+/// position += timeStep * newVelocity+/// This drives towards a zero position. By using implicit integration we get a stable solution that+/// doesn't require transcendental functions.+B2_INLINE float b2SpringDamper( float hertz, float dampingRatio, float position, float velocity,+								float timeStep )+{+	float omega = 2.0f * B2_PI * hertz;+	float omegaH = omega * timeStep;+	return ( velocity - omega * omegaH * position ) /+		   ( 1.0f + 2.0f * dampingRatio * omegaH + omegaH * omegaH );+}++/// Box2D bases all length units on meters, but you may need different units for your game. You can+/// set this value to use different units. This should be done at application startup and only+/// modified once. Default value is 1. For example, if your game uses pixels for units you can use+/// pixels for all length values sent to Box2D. There should be no extra cost. However, Box2D has+/// some internal tolerances and thresholds that have been tuned for meters. By calling this+/// function, Box2D is able to adjust those tolerances and thresholds to improve accuracy. A good+/// rule of thumb is to pass the height of your player character to this function. So if your player+/// character is 32 pixels high, then pass 32 to this function. Then you may confidently use pixels+/// for all the length values sent to Box2D. All length values returned from Box2D will also be+/// pixels because Box2D does not do any scaling internally. However, you are now on the hook for+/// coming up with good values for gravity, density, and forces.+/// @warning This must be modified before any calls to Box2D+B2_API void b2SetLengthUnitsPerMeter( float lengthUnits );++/// Get the current length units per meter.+B2_API float b2GetLengthUnitsPerMeter( void );++#ifdef __cplusplus+/// This value is exported for efficiency. Don't modify directly.+extern "C" BOX2D_EXPORT float b2_lengthUnitsPerMeter;+#else+/// This value is exported for efficiency. Don't modify directly.+BOX2D_EXPORT extern float b2_lengthUnitsPerMeter;+#endif++/**@}*/++#ifdef __cplusplus++/// Unary add one vector to another+inline void operator+=( b2Vec2& a, b2Vec2 b )+{+	a.x += b.x;+	a.y += b.y;+}++/// Unary subtract one vector from another+inline void operator-=( b2Vec2& a, b2Vec2 b )+{+	a.x -= b.x;+	a.y -= b.y;+}++/// Unary multiply a vector by a scalar+inline void operator*=( b2Vec2& a, float b )+{+	a.x *= b;+	a.y *= b;+}++/// Unary negate a vector+inline b2Vec2 operator-( b2Vec2 a )+{+	return { -a.x, -a.y };+}++/// Binary vector addition+inline b2Vec2 operator+( b2Vec2 a, b2Vec2 b )+{+	return { a.x + b.x, a.y + b.y };+}++/// Binary vector subtraction+inline b2Vec2 operator-( b2Vec2 a, b2Vec2 b )+{+	return { a.x - b.x, a.y - b.y };+}++/// Binary scalar and vector multiplication+inline b2Vec2 operator*( float a, b2Vec2 b )+{+	return { a * b.x, a * b.y };+}++/// Binary scalar and vector multiplication+inline b2Vec2 operator*( b2Vec2 a, float b )+{+	return { a.x * b, a.y * b };+}++/// Binary vector equality+inline bool operator==( b2Vec2 a, b2Vec2 b )+{+	return a.x == b.x && a.y == b.y;+}++/// Binary vector inequality+inline bool operator!=( b2Vec2 a, b2Vec2 b )+{+	return a.x != b.x || a.y != b.y;+}++#if defined( BOX2D_DOUBLE_PRECISION )++/// Offset a world position by a vector+inline b2Pos operator+( b2Pos a, b2Vec2 b )+{+	return { a.x + b.x, a.y + b.y };+}++/// Offset a world position by a vector+inline b2Pos operator-( b2Pos a, b2Vec2 b )+{+	return { a.x - b.x, a.y - b.y };+}++/// Delta between two world positions, demoted to float+inline b2Vec2 operator-( b2Pos a, b2Pos b )+{+	return { (float)( a.x - b.x ), (float)( a.y - b.y ) };+}++#endif++#endif+
+ upstream/include/box2d/math_types.h view
@@ -0,0 +1,101 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++/**+ * @defgroup math Math+ * @brief Vector math types and functions+ * @{+ */++/// 2D vector This can be used to represent a point or free vector+typedef struct b2Vec2+{+	/// coordinates+	float x, y;+} b2Vec2;++/// Cosine and sine pair This uses a custom implementation designed for cross-platform determinism+typedef struct b2CosSin+{+	/// cosine and sine+	float cosine;+	float sine;+} b2CosSin;++/// 2D rotation This is similar to using a complex number for rotation+typedef struct b2Rot+{+	/// cosine and sine+	float c, s;+} b2Rot;++/// A 2D rigid transform+typedef struct b2Transform+{+	b2Vec2 p;+	b2Rot q;+} b2Transform;++#if defined( BOX2D_DOUBLE_PRECISION )++/// A world position. Double precision in large world mode so coordinates stay accurate far from the+/// origin.+typedef struct b2Pos+{+	double x, y;+} b2Pos;++/// A world transform with double precision translation and float rotation. Rotation is frame local+/// and never needs the extra range, the same split as Jolt's DMat44.+typedef struct b2WorldTransform+{+	b2Pos p;+	b2Rot q;+} b2WorldTransform;++#else++/// Alias in single precision.+typedef b2Vec2 b2Pos;++/// Alias in single precision.+typedef b2Transform b2WorldTransform;++#endif++/// A 2-by-2 Matrix+typedef struct b2Mat22+{+	/// columns+	b2Vec2 cx, cy;+} b2Mat22;++/// Axis-aligned bounding box+typedef struct b2AABB+{+	b2Vec2 lowerBound;+	b2Vec2 upperBound;+} b2AABB;++/// separation = dot(normal, point) - offset+typedef struct b2Plane+{+	b2Vec2 normal;+	float offset;+} b2Plane;++/// https://en.wikipedia.org/wiki/Pi+#define B2_PI 3.14159265359f++static const b2Vec2 b2Vec2_zero = { 0.0f, 0.0f };+static const b2Rot b2Rot_identity = { 1.0f, 0.0f };+static const b2Transform b2Transform_identity = { { 0.0f, 0.0f }, { 1.0f, 0.0f } };+static const b2Mat22 b2Mat22_zero = { { 0.0f, 0.0f }, { 0.0f, 0.0f } };++// Initializers valid in both modes: 0.0f promotes to double, the identity rotation is float+static const b2Pos b2Pos_zero = { 0.0f, 0.0f };+static const b2WorldTransform b2WorldTransform_identity = { { 0.0f, 0.0f }, { 1.0f, 0.0f } };++/**@}*/
+ upstream/include/box2d/types.h view
@@ -0,0 +1,1655 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "base.h"+#include "collision.h"+#include "id.h"+#include "math_types.h"++#include <stdbool.h>+#include <stdint.h>++/// Default filter category bits.+/// @ingroup shape+#define B2_DEFAULT_CATEGORY_BITS 1++/// Default filter mask bits.+/// @ingroup shape+#define B2_DEFAULT_MASK_BITS UINT64_MAX++/// Task interface This is the prototype for a Box2D 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 b2EnqueueTaskCallbackFcn.+/// @ingroup world+typedef void b2TaskCallbackFcn( void* taskContext );++/// These functions can be provided to Box2D to invoke a task system. Returns a pointer to the+/// user's task object. May be nullptr. A nullptr indicates to Box2D that the work was executed+/// serially within the callback and there is no need to call b2FinishTaskCallbackFcn.+/// @ingroup world+typedef void* b2EnqueueTaskCallbackFcn( b2TaskCallbackFcn* task, void* taskContext,+										void* userContext );++/// Finishes a user task object that wraps a Box2D task.+/// @ingroup world+typedef void b2FinishTaskCallbackFcn( void* userTask, 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 Box2D state or user application state.+/// @ingroup world+typedef float b2FrictionCallbackFcn( 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 Box2D state or user application state.+/// @ingroup world+typedef float b2RestitutionCallbackFcn( float restitutionA, uint64_t userMaterialIdA,+										float restitutionB, uint64_t userMaterialIdB );++/// Result from b2World_CastRayClosest If there is initial overlap the fraction and normal will be+/// zero while the point is an arbitrary point in the overlap region.+/// @ingroup world+typedef struct b2RayResult+{+	/// The shape hit.+	b2ShapeId shapeId;++	/// The point of contact, on the surface of the hit shape.+	b2Pos point;++	/// The unit normal vector at the point of contact.+	b2Vec2 normal;++	/// The fraction of the input ray.+	float fraction;++	/// The number of BVH nodes visited. Diagnostic.+	int nodeVisits;++	/// The number of BVH leaves visited. Diagnostic.+	int leafVisits;++	/// Did the ray hit? If false, all other data is invalid.+	bool hit;+} b2RayResult;++/// Optional world capacities that can be used to avoid run-time allocations.+/// @see b2World_GetMaxCapacity+/// @ingroup world+typedef struct b2Capacity+{+	/// 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;++	/// Reserve arena allocator space, used for temporary allocations.+	int arenaByteCount;+} b2Capacity;++/// World definition used to create a simulation world. Must be initialized using+/// b2DefaultWorldDef().+/// @ingroup world+typedef struct b2WorldDef+{+	/// Gravity vector. Box2D has no up-vector defined.+	b2Vec2 gravity;++	/// Restitution speed threshold, usually in m/s. Collisions above this speed have restitution+	/// applied (will bounce).+	float restitutionThreshold;++	/// Number of iterations of the restitution solver. More iterations can lead to less box+	/// spinning.+	/// @see B2_MAX_RESTITUTION_ITERATIONS+	int restitutionIterations;++	/// Enable full contact propagation in the restitution solver. Expensive. (Experimental)+	bool enableRestitutionPropagation;++	/// Threshold speed for hit events. Usually meters per second.+	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).+	b2FrictionCallbackFcn* frictionCallback;++	/// Optional mixing callback for restitution. The default uses max(restitutionA, restitutionB).+	b2RestitutionCallbackFcn* restitutionCallback;++	/// Can bodies go to sleep to improve performance+	bool enableSleep;++	/// Enable continuous collision+	bool enableContinuous;++	/// Number of workers for multithreading. Box2D performs best when using performance cores and+	/// accessing a single L3 cache (uniform memory). Efficiency cores and SMT provide little+	/// benefit and may even harm performance. This is clamped to the range [1, B2_MAX_WORKERS]. If+	/// task callbacks are provided then Box2D will use the user provided task system. If no task+	/// callbacks are installed then Box2D will run single-threaded with a worker count of 1, or use+	/// the internal scheduler with a worker count greater than 1.+	int workerCount;++	/// Function to spawn tasks+	b2EnqueueTaskCallbackFcn* enqueueTask;++	/// Function to finish a task+	b2FinishTaskCallbackFcn* finishTask;++	/// User context that is provided to enqueueTask and finishTask+	void* userTaskContext;++	/// User data+	void* userData;++	/// Optional initial capacities+	b2Capacity capacity;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2WorldDef;++/// Use this to initialize your world definition+/// @ingroup world+B2_API b2WorldDef b2DefaultWorldDef( 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 b2BodyType+{+	/// zero mass, zero velocity, may be manually moved+	b2_staticBody = 0,++	/// zero mass, velocity set by user, moved by solver+	b2_kinematicBody = 1,++	/// positive mass, velocity determined by forces, moved by solver+	b2_dynamicBody = 2,++	/// number of body types+	b2_bodyTypeCount,+} b2BodyType;++/// Motion locks to restrict the body movement+/// @ingroup body+typedef struct b2MotionLocks+{+	/// Prevent translation along the x-axis+	bool linearX;++	/// Prevent translation along the y-axis+	bool linearY;++	/// Prevent rotation around the z-axis+	bool angularZ;+} b2MotionLocks;++/// 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 b2DefaultBodyDef().+/// @ingroup body+typedef struct b2BodyDef+{+	/// The body type: static, kinematic, or dynamic.+	b2BodyType 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.+	b2Pos position;++	/// The initial world rotation of the body. Use b2MakeRot() if you have an angle.+	b2Rot rotation;++	/// The initial linear velocity of the body's origin. Usually in meters per second.+	b2Vec2 linearVelocity;++	/// The initial angular velocity of the body. Radians per second.+	float 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;++	/// Continuous collision safety factor. The solver only uses continuous collision if there is a+	/// risk of tunneling. If the body is moving fast enough to risk tunneling then it is considered+	/// a "fast body". This improves performance and prevents movement hitches. If a body moving N+	/// meters risks tunneling, then the body will be considered fast if it moves more than a+	/// safetyFactor times N meters over one full time step. Non-dimensional. Recommended range+	/// [0.01, 0.5]. Default is 0.5 for high performance with low tunneling risk.+	float safetyFactor;++	/// Optional body name for debugging. Up to B2_NAME_LENGTH characters+	const char* name;++	/// Use this to store application specific body data.+	void* userData;++	/// Motion locks to restrict linear and angular movement. Caution: may lead to softer+	/// constraints along the locked direction+	b2MotionLocks 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 dynamic 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 a 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 Box2D. 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 collisions that should be avoided on characters.+	bool enableContactRecycling;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2BodyDef;++/// Use this to initialize your body definition+/// @ingroup body+B2_API b2BodyDef b2DefaultBodyDef( void );++/// This is used to filter collision on shapes. It affects shape-vs-shape collision and+/// shape-versus-query collision (such as b2World_CastRay).+/// @ingroup shape+typedef struct b2Filter+{+	/// 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;+} b2Filter;++/// Use this to initialize your filter+/// @ingroup shape+B2_API b2Filter b2DefaultFilter( void );++/// The query filter is used to filter collisions between queries and shapes. For example, you may+/// want a ray-cast representing a projectile to hit players and the static environment but not+/// debris.+/// @ingroup shape+typedef struct b2QueryFilter+{+	/// 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;+} b2QueryFilter;++/// Use this to initialize your query filter+/// @ingroup shape+B2_API b2QueryFilter b2DefaultQueryFilter( void );++/// Shape type+/// @ingroup shape+typedef enum b2ShapeType+{+	/// A circle with an offset+	b2_circleShape,++	/// A capsule is an extruded circle+	b2_capsuleShape,++	/// A line segment+	b2_segmentShape,++	/// A convex polygon+	b2_polygonShape,++	/// A line segment owned by a chain shape+	b2_chainSegmentShape,++	/// The number of shape types+	b2_shapeTypeCount+} b2ShapeType;++/// The surface material groups together parameters related to the shape surface, which is the 2D+/// boundary. Surface materials allow chain shapes to have per segment properties.+/// @ingroup shape+typedef struct b2SurfaceMaterial+{+	/// 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].+	float rollingResistance;++	/// The tangent speed for conveyor belts+	float tangentSpeed;++	/// 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.+	uint32_t customColor;+} b2SurfaceMaterial;++/// Use this to initialize your surface material+/// @ingroup shape+B2_API b2SurfaceMaterial b2DefaultSurfaceMaterial( void );++/// Used to create a shape. This is a temporary object used to bundle shape creation parameters. You+/// may use the same shape definition to create multiple shapes. Must be initialized using+/// b2DefaultShapeDef().+/// @ingroup shape+typedef struct b2ShapeDef+{+	/// Use this to store application specific shape data.+	void* userData;++	/// The surface material for this shape.+	b2SurfaceMaterial material;++	/// The density, usually in kg/m^2. This is not part of the surface material because this is for+	/// the interior, which may have other considerations, such as being hollow. For example a wood+	/// barrel may be hollow or full of water.+	float density;++	/// Collision filtering data.+	b2Filter filter;++	/// Enable custom filtering. Only one of the two shapes needs to enable custom filtering. See+	/// b2World_SetCustomFilterCallback.+	bool enableCustomFiltering;++	/// A sensor shape generates overlap events but never generates a collision response. Fast+	/// moving sensors may miss overlaps. 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. Both shapes+	/// involved must have this flag set to true. False by default, even for sensors.+	bool enableSensorEvents;++	/// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Only one+	/// shape involved needs this flag set to true. Ignored for sensors. False by default.+	bool enableContactEvents;++	/// Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Only one+	/// shape involved needs this flag set to true. 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 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 b2Body_UpdateMassFromShapes or b2Body_SetMassData+	/// before simulating the world.+	bool updateBodyMass;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2ShapeDef;++/// Use this to initialize your shape definition+/// @ingroup shape+B2_API b2ShapeDef b2DefaultShapeDef( void );++/// Used to create a chain of line segments. This is designed to eliminate ghost collisions with+/// some limitations.+/// - chains are one-sided+/// - chains have no mass and should be used on static bodies+/// - chains have a counter-clockwise winding order (normal points right of segment direction)+/// - chains are either a loop or open+/// - the distance between any two points must be greater than B2_LINEAR_SLOP+/// - a chain shape should not self intersect (this is not validated)+/// - an open chain shape needs a leading and trailing ghost point.+/// - you may overlap two open chains using the ghost points to get smooth collision.+/// - a chain shape creates multiple line segment shapes on the body+/// https://en.wikipedia.org/wiki/Polygonal_chain+/// Must be initialized using b2DefaultChainDef().+/// @warning Do not use chain shapes unless you understand the limitations. This is an advanced+/// feature.+/// @ingroup chain+typedef struct b2ChainDef+{+	/// Use this to store application specific shape data.+	void* userData;++	/// Array of chain points. Each consecutive pair forms a segment. For a loop the last and first+	/// point form a segment. Cloned.+	const b2Vec2* points;++	/// The point count. At least 2 for an open chain and at least 3 for a loop.+	/// segmentCount = isLoop ? pointCount : pointCount - 1+	int pointCount;++	/// Leading ghost point for an open chain. Distance from first point must be greater than+	/// B2_LINEAR_SLOP. Ignored for loops.+	b2Vec2 ghost1;++	/// Ending ghost point for an open chain. Distance from last point must be greater than+	/// B2_LINEAR_SLOP. Ignored for loops.+	b2Vec2 ghost2;++	/// One material for the whole chain or one for each segment. Cloned.+	const b2SurfaceMaterial* materials;++	/// The material count. Must be 1 or the segment count.+	int materialCount;++	/// Contact filtering data.+	b2Filter filter;++	/// Indicates a closed chain formed by connecting the first and last point.+	bool isLoop;++	/// Enable sensors to detect this chain. False by default.+	bool enableSensorEvents;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2ChainDef;++/// Use this to initialize your chain definition+/// @ingroup chain+B2_API b2ChainDef b2DefaultChainDef( void );++/// Profiling data. Times are in milliseconds.+/// @ingroup world+typedef struct b2Profile+{+	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 restitution;		   ///<+	float storeImpulses;	   ///<+	float splitIslands;		   ///<+	float transforms;		   ///<+	float sensorHits;		   ///<+	float jointEvents;		   ///<+	float hitEvents;		   ///<+	float refit;			   ///<+	float bullets;			   ///<+	float sleepIslands;		   ///<+	float sensors;			   ///<+} b2Profile;++/// Counters that give details of the simulation size.+/// @ingroup world+typedef struct b2Counters+{+	size_t byteCount;	  ///<+	int bodyCount;		  ///<+	int shapeCount;		  ///<+	int contactCount;	  ///<+	int jointCount;		  ///<+	int islandCount;	  ///<+	int stackUsed;		  ///<+	int staticTreeHeight; ///<+	int treeHeight;		  ///<+	int taskCount;		  ///<+	int colorCounts[24];  ///<++	/// 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;++} b2Counters;++/// Joint type enumeration+///+/// This is useful because all joint types use b2JointId and sometimes you want to get the type of a+/// joint.+/// @ingroup joint+typedef enum b2JointType+{+	b2_distanceJoint,  ///< constrain the distance between two points+	b2_filterJoint,	   ///< filter collision between two bodies+	b2_motorJoint,	   ///< fully control the movement of a body+	b2_moverJoint,	   ///< control the movement of the dynamic character mover+	b2_pogoJoint,	   ///< ground interaction for the dynamic character mover+	b2_prismaticJoint, ///< slide along an axis+	b2_revoluteJoint,  ///< rotate around a pivot+	b2_weldJoint,	   ///< bring two bodies together with some flex+	b2_wheelJoint,	   ///< wheel spins with shock absorber+} b2JointType;++/// 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 b2JointDef+{+	/// User data pointer+	void* userData;++	/// The first attached body+	b2BodyId bodyIdA;++	/// The second attached body+	b2BodyId bodyIdB;++	/// The first local joint frame+	b2Transform localFrameA;++	/// The second local joint frame+	b2Transform 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;++} b2JointDef;++/// 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 b2DistanceJointDef+{+	/// Base joint definition+	b2JointDef 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 for limit. Clamped to a stable minimum value.+	float minLength;++	/// Maximum length for limit. 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;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2DistanceJointDef;++/// Use this to initialize your joint definition+/// @ingroup distance_joint+B2_API b2DistanceJointDef b2DefaultDistanceJointDef( void );++/// A filter joint is used to disable collision between two specific bodies.+///+/// @ingroup filter_joint+typedef struct b2FilterJointDef+{+	/// Base joint definition+	b2JointDef base;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2FilterJointDef;++/// Use this to initialize your joint definition+/// @ingroup filter_joint+B2_API b2FilterJointDef b2DefaultFilterJointDef( void );++/// A motor joint is used to control the relative velocity and/or transform between two bodies. With+/// a velocity of zero this acts like top-down friction.+/// @ingroup motor_joint+typedef struct b2MotorJointDef+{+	/// Base joint definition+	b2JointDef base;++	/// The desired linear velocity+	b2Vec2 linearVelocity;++	/// The maximum motor force in newtons+	float maxVelocityForce;++	/// The desired angular velocity+	float 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;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2MotorJointDef;++/// Use this to initialize your joint definition+/// @ingroup motor_joint+B2_API b2MotorJointDef b2DefaultMotorJointDef( void );++/// A mover joint is used to move a dynamic character mover through velocity commands. The x and y+/// directions are handled separately. Does not affect rotation.+/// @ingroup mover_joint+typedef struct b2MoverJointDef+{+	/// Base joint definition+	b2JointDef base;++	/// The desired linear velocity+	b2Vec2 linearVelocity;++	/// The maximum motor force in newtons.+	b2Vec2 maxVelocityForce;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2MoverJointDef;++/// Use this to initialize your joint definition+/// @ingroup mover_joint+B2_API b2MoverJointDef b2DefaultMoverJointDef( void );++/// Pogo joint definition+/// @ingroup pogo_joint+typedef struct b2PogoJointDef+{+	/// Base joint definition+	b2JointDef base;++	/// Normal vector at the contact+	b2Vec2 normal;++	/// The spring stiffness Hertz, cycles per second+	float hertz;++	/// The spring damping ratio, non-dimensional+	float dampingRatio;++	/// Spring length.+	float restLength;++	/// Maximum tension force. Controls how sticky the mover is to ground.+	float maxTensionForce;++	/// Maximum compression force. How hard the pogo pushes the mover up from the ground.+	float maxCompressionForce;++	/// The initial pogo internal impulse. The pogo is typically recreated every frame. This gives a+	/// way to move the internal state forward for smoother spring behavior.+	float impulse;++	/// The initial pogo internal velocity. The pogo is typically recreated every frame. This gives+	/// a way to move the internal state forward for smoother spring behavior.+	float velocity;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2PogoJointDef;++/// Use this to initialize your joint definition+/// @ingroup pogo_joint+B2_API b2PogoJointDef b2DefaultPogoJointDef( void );++/// Prismatic joint definition Body B may slide along the x-axis in local frame A. Body B cannot+/// rotate relative to body A. The joint translation is zero when the local frame origins coincide+/// in world space.+/// @ingroup prismatic_joint+typedef struct b2PrismaticJointDef+{+	/// Base joint definition+	b2JointDef 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;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2PrismaticJointDef;++/// Use this to initialize your joint definition+/// @ingroup prismatic_joint+B2_API b2PrismaticJointDef b2DefaultPrismaticJointDef( void );++/// Revolute joint definition A point on body B is fixed to a point on body A. Allows relative+/// rotation.+/// @ingroup revolute_joint+typedef struct b2RevoluteJointDef+{+	/// Base joint definition+	b2JointDef base;++	/// The target angle for the joint in radians. The spring-damper will drive to this angle.+	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;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2RevoluteJointDef;++/// Use this to initialize your joint definition.+/// @ingroup revolute_joint+B2_API b2RevoluteJointDef b2DefaultRevoluteJointDef( void );++/// Weld joint definition Connects two bodies together rigidly. This constraint provides springs to+/// mimic soft-body simulation.+/// @note The approximate solver in Box2D cannot hold many bodies together rigidly+/// @ingroup weld_joint+typedef struct b2WeldJointDef+{+	/// Base joint definition+	b2JointDef 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;++	/// Angular damping ratio, non-dimensional. Use 1 for critical damping.+	float angularDampingRatio;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2WeldJointDef;++/// Use this to initialize your joint definition+/// @ingroup weld_joint+B2_API b2WeldJointDef b2DefaultWeldJointDef( void );++/// Wheel joint definition Body B is a wheel that may rotate freely and slide along the local x-axis+/// in frame A. The joint translation is zero when the local frame origins coincide in world space.+/// @ingroup wheel_joint+typedef struct b2WheelJointDef+{+	/// Base joint definition+	b2JointDef base;++	/// Enable a linear spring along the local axis+	bool enableSpring;++	/// Spring stiffness in Hertz+	float hertz;++	/// Spring damping ratio, non-dimensional+	float dampingRatio;++	/// Enable/disable the joint linear limit+	bool enableLimit;++	/// The lower translation limit+	float lowerTranslation;++	/// The upper translation limit+	float upperTranslation;++	/// Enable/disable the joint rotational motor+	bool enableMotor;++	/// The maximum motor torque, typically in newton-meters+	float maxMotorTorque;++	/// The desired motor speed in radians per second+	float motorSpeed;++	/// Used internally to detect a valid definition. DO NOT SET.+	int internalValue;+} b2WheelJointDef;++/// Use this to initialize your joint definition+/// @ingroup wheel_joint+B2_API b2WheelJointDef b2DefaultWheelJointDef( void );++/// The explosion definition is used to configure options for explosions. Explosions consider shape+/// geometry when computing the impulse.+/// @ingroup world+typedef struct b2ExplosionDef+{+	/// Mask bits to filter shapes+	uint64_t maskBits;++	/// The center of the explosion in world space+	b2Pos 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 length. This applies an impulse according to the shape perimeter that is+	/// facing the explosion. Explosions only apply to circles, capsules, and polygons. This may be+	/// negative for implosions.+	float impulsePerLength;+} b2ExplosionDef;++/// Use this to initialize your explosion definition+/// @ingroup world+B2_API b2ExplosionDef b2DefaultExplosionDef( void );++/**+ * @defgroup events Events+ * World event types.+ *+ * Events are used to collect events that occur during the world time step. These events are then+ * available to query after the time step is complete. This is preferable to callbacks because Box2D+ * 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 b2SensorBeginTouchEvent+{+	/// The id of the sensor shape+	b2ShapeId sensorShapeId;++	/// The id of the shape that began touching the sensor shape+	b2ShapeId visitorShapeId;+} b2SensorBeginTouchEvent;++/// An end touch event is generated when a shape stops overlapping a sensor shape. Normally this+/// event is generated from simulation movement. However, this event can also be generated from API+/// interactions: setting the body transform, destroying a body or shape, or changing a filter. You+/// will also get an end event if the sensor is destroyed. Therefore you should always confirm the+/// shape id is valid using b2Shape_IsValid.+typedef struct b2SensorEndTouchEvent+{+	/// The id of the sensor shape+	/// @warning this shape may have been destroyed+	/// @see b2Shape_IsValid+	b2ShapeId sensorShapeId;++	/// The id of the shape that stopped touching the sensor shape+	/// @warning this shape may have been destroyed+	/// @see b2Shape_IsValid+	b2ShapeId visitorShapeId;++} b2SensorEndTouchEvent;++/// 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 b2SensorEvents+{+	/// Array of sensor begin touch events+	b2SensorBeginTouchEvent* beginEvents;++	/// Array of sensor end touch events+	b2SensorEndTouchEvent* endEvents;++	/// The number of begin touch events+	int beginCount;++	/// The number of end touch events+	int endCount;+} b2SensorEvents;++/// A begin touch event is generated when two shapes begin touching.+typedef struct b2ContactBeginTouchEvent+{+	/// Id of the first shape+	b2ShapeId shapeIdA;++	/// Id of the second shape+	b2ShapeId shapeIdB;++	/// The transient contact id. This contact may be destroyed automatically when the world is+	/// modified or simulated. Use b2Contact_IsValid before using this id.+	b2ContactId contactId;+} b2ContactBeginTouchEvent;++/// 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 b2ContactEndTouchEvent+{+	/// Id of the first shape+	/// @warning this shape may have been destroyed+	/// @see b2Shape_IsValid+	b2ShapeId shapeIdA;++	/// Id of the second shape+	/// @warning this shape may have been destroyed+	/// @see b2Shape_IsValid+	b2ShapeId shapeIdB;++	/// Id of the contact.+	/// @warning this contact may have been destroyed+	/// @see b2Contact_IsValid+	b2ContactId contactId;+} b2ContactEndTouchEvent;++/// A hit event is generated when two shapes collide with a speed faster than the hit event+/// threshold. This may be reported for speculative contacts that have a confirmed impulse.+typedef struct b2ContactHitEvent+{+	/// Id of the first shape+	b2ShapeId shapeIdA;++	/// Id of the second shape+	b2ShapeId shapeIdB;++	/// Id of the contact.+	/// @warning this contact may have been destroyed+	/// @see b2Contact_IsValid+	b2ContactId contactId;++	/// Point where the shapes hit. This is a mid-point between the two surfaces. It could be a+	/// speculative point where the shapes were not touching at the beginning of the time step. The+	/// point is reconstructed after the step, so when both bodies are moving it may trail the+	/// impact by up to one step of motion.+	b2Pos point;++	/// Normal vector pointing from shape A to shape B+	b2Vec2 normal;++	/// The speed the shapes are approaching. Always positive. Typically in meters per second.+	float approachSpeed;+} b2ContactHitEvent;++/// Contact events are buffered in the Box2D 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 b2ContactEvents+{+	/// Array of begin touch events+	b2ContactBeginTouchEvent* beginEvents;++	/// Array of end touch events+	b2ContactEndTouchEvent* endEvents;++	/// Array of hit events+	b2ContactHitEvent* hitEvents;++	/// Number of begin touch events+	int beginCount;++	/// Number of end touch events+	int endCount;++	/// Number of hit events+	int hitCount;+} b2ContactEvents;++/// 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+/// b2Body_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 b2BodyMoveEvent+{+	/// The body user data.+	void* userData;++	/// The world transform of the body.+	b2WorldTransform transform;++	/// The id of the body that moved.+	b2BodyId bodyId;++	/// Did the body fall asleep this time step?+	bool fellAsleep;+} b2BodyMoveEvent;++/// Body events are buffered in the Box2D world and are available as event arrays after the time+/// step is complete. Note: this data becomes invalid if bodies are destroyed+typedef struct b2BodyEvents+{+	/// Array of move events+	b2BodyMoveEvent* moveEvents;++	/// Number of move events+	int moveCount;+} b2BodyEvents;++/// 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 b2JointEvent+{+	/// The joint id+	b2JointId jointId;++	/// The user data from the joint for convenience+	void* userData;+} b2JointEvent;++/// 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 b2JointEvents+{+	/// Array of events+	b2JointEvent* jointEvents;++	/// Number of events+	int count;+} b2JointEvents;++/**@}*/++/// The contact data for two shapes. By convention the manifold normal points from shape A to shape+/// B.+/// @see b2Shape_GetContactData() and b2Body_GetContactData()+/// @ingroup contact+typedef struct b2ContactData+{+	/// The contact id. You may hold onto this to track a contact across time steps. This id may+	/// become orphaned. Use b2Contact_IsValid before using it for other functions.+	b2ContactId contactId;++	/// The first shape id.+	b2ShapeId shapeIdA;++	/// The second shape id.+	b2ShapeId shapeIdB;++	/// The manifold copied from the contact.+	b2Manifold manifold;+} b2ContactData;++/// 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. Notes:+/// - this function must be thread-safe+/// - this is only called if one of the two shapes has enabled custom filtering+/// - this may be called for awake dynamic bodies and sensors Return false if you want to disable+///   the collision+/// @see b2ShapeDef+/// @warning Do not attempt to modify the world inside this callback+/// @ingroup world+typedef bool b2CustomFilterCallbackFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, void* context );++/// Prototype for a pre-solve callback. This is called after a contact manifold is updated. This+/// allows you to inspect a manifold before it goes to the solver. If you are careful, you can+/// modify the manifold (e.g. modify the normal). You can set the manifold point count to zero to+/// disable collision. Notes:+/// - this function must be thread-safe+/// - this is only called if the shape has enabled pre-solve events+/// - this is called only for awake dynamic bodies+/// - this is not called for sensors+/// - the supplied manifold impulses are not reliable+/// @warning Do not attempt to modify the world inside this callback+/// @ingroup world+typedef void b2PreSolveCallbackFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold,+									void* context );++/// Prototype for a pre-continuous callback. This is called when a time of impact event is computed+/// in continuous collision. This function can be used to skip the event. Unlike the pre-solve+/// callback, this only supplies the point and normal. At this stage of the solver the manifold may+/// not exist or may be stale. Notes:+/// - this function must be thread-safe+/// - this is only called if the shape has enabled pre-solve events+/// - this is called only for awake dynamic bodies+/// - this is not called for sensors Return false if you want to disable the time of impact event.+/// @warning Do not attempt to modify the world inside this callback+/// @ingroup world+typedef bool b2PreContinuousCallbackFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Pos point,+										 b2Vec2 normal, void* context );++/// Prototype callback for overlap queries. Called for each shape found in the query.+/// @see b2World_OverlapAABB+/// @return false to terminate the query.+/// @ingroup world+typedef bool b2OverlapResultCallbackFcn( b2ShapeId shapeId, void* context );++/// Prototype callback for ray and shape 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 A cast with initial overlap will return a zero+///   fraction and a zero normal.+/// @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, zero for a shape cast with initial+/// overlap+/// @param fraction the fraction along the ray at the point of intersection, zero for a shape cast+/// with initial overlap+/// @param context the user context+/// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue+/// @see b2World_CastRay+/// @ingroup world+typedef float b2CastResultCallbackFcn( b2ShapeId shapeId, b2Pos point, b2Vec2 normal,+									   float fraction, void* context );++/// Used to collect collision planes for character movers. Return true to continue gathering planes.+/// @ingroup character+typedef bool b2PlaneResultCallbackFcn( b2ShapeId shapeId, const b2PlaneResult* plane,+									   void* context );++/**+ * @defgroup debug 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+/// @ingroup debug+typedef enum b2HexColor+{+	b2_colorAliceBlue = 0xF0F8FF,+	b2_colorAntiqueWhite = 0xFAEBD7,+	b2_colorAqua = 0x00FFFF,+	b2_colorAquamarine = 0x7FFFD4,+	b2_colorAzure = 0xF0FFFF,+	b2_colorBeige = 0xF5F5DC,+	b2_colorBisque = 0xFFE4C4,+	b2_colorBlack = 0x000000,+	b2_colorBlanchedAlmond = 0xFFEBCD,+	b2_colorBlue = 0x0000FF,+	b2_colorBlueViolet = 0x8A2BE2,+	b2_colorBrown = 0xA52A2A,+	b2_colorBurlywood = 0xDEB887,+	b2_colorCadetBlue = 0x5F9EA0,+	b2_colorChartreuse = 0x7FFF00,+	b2_colorChocolate = 0xD2691E,+	b2_colorCoral = 0xFF7F50,+	b2_colorCornflowerBlue = 0x6495ED,+	b2_colorCornsilk = 0xFFF8DC,+	b2_colorCrimson = 0xDC143C,+	b2_colorCyan = 0x00FFFF,+	b2_colorDarkBlue = 0x00008B,+	b2_colorDarkCyan = 0x008B8B,+	b2_colorDarkGoldenRod = 0xB8860B,+	b2_colorDarkGray = 0xA9A9A9,+	b2_colorDarkGreen = 0x006400,+	b2_colorDarkKhaki = 0xBDB76B,+	b2_colorDarkMagenta = 0x8B008B,+	b2_colorDarkOliveGreen = 0x556B2F,+	b2_colorDarkOrange = 0xFF8C00,+	b2_colorDarkOrchid = 0x9932CC,+	b2_colorDarkRed = 0x8B0000,+	b2_colorDarkSalmon = 0xE9967A,+	b2_colorDarkSeaGreen = 0x8FBC8F,+	b2_colorDarkSlateBlue = 0x483D8B,+	b2_colorDarkSlateGray = 0x2F4F4F,+	b2_colorDarkTurquoise = 0x00CED1,+	b2_colorDarkViolet = 0x9400D3,+	b2_colorDeepPink = 0xFF1493,+	b2_colorDeepSkyBlue = 0x00BFFF,+	b2_colorDimGray = 0x696969,+	b2_colorDodgerBlue = 0x1E90FF,+	b2_colorFireBrick = 0xB22222,+	b2_colorFloralWhite = 0xFFFAF0,+	b2_colorForestGreen = 0x228B22,+	b2_colorFuchsia = 0xFF00FF,+	b2_colorGainsboro = 0xDCDCDC,+	b2_colorGhostWhite = 0xF8F8FF,+	b2_colorGold = 0xFFD700,+	b2_colorGoldenRod = 0xDAA520,+	b2_colorGray = 0x808080,+	b2_colorGreen = 0x008000,+	b2_colorGreenYellow = 0xADFF2F,+	b2_colorHoneyDew = 0xF0FFF0,+	b2_colorHotPink = 0xFF69B4,+	b2_colorIndianRed = 0xCD5C5C,+	b2_colorIndigo = 0x4B0082,+	b2_colorIvory = 0xFFFFF0,+	b2_colorKhaki = 0xF0E68C,+	b2_colorLavender = 0xE6E6FA,+	b2_colorLavenderBlush = 0xFFF0F5,+	b2_colorLawnGreen = 0x7CFC00,+	b2_colorLemonChiffon = 0xFFFACD,+	b2_colorLightBlue = 0xADD8E6,+	b2_colorLightCoral = 0xF08080,+	b2_colorLightCyan = 0xE0FFFF,+	b2_colorLightGoldenRodYellow = 0xFAFAD2,+	b2_colorLightGray = 0xD3D3D3,+	b2_colorLightGreen = 0x90EE90,+	b2_colorLightPink = 0xFFB6C1,+	b2_colorLightSalmon = 0xFFA07A,+	b2_colorLightSeaGreen = 0x20B2AA,+	b2_colorLightSkyBlue = 0x87CEFA,+	b2_colorLightSlateGray = 0x778899,+	b2_colorLightSteelBlue = 0xB0C4DE,+	b2_colorLightYellow = 0xFFFFE0,+	b2_colorLime = 0x00FF00,+	b2_colorLimeGreen = 0x32CD32,+	b2_colorLinen = 0xFAF0E6,+	b2_colorMagenta = 0xFF00FF,+	b2_colorMaroon = 0x800000,+	b2_colorMediumAquaMarine = 0x66CDAA,+	b2_colorMediumBlue = 0x0000CD,+	b2_colorMediumOrchid = 0xBA55D3,+	b2_colorMediumPurple = 0x9370DB,+	b2_colorMediumSeaGreen = 0x3CB371,+	b2_colorMediumSlateBlue = 0x7B68EE,+	b2_colorMediumSpringGreen = 0x00FA9A,+	b2_colorMediumTurquoise = 0x48D1CC,+	b2_colorMediumVioletRed = 0xC71585,+	b2_colorMidnightBlue = 0x191970,+	b2_colorMintCream = 0xF5FFFA,+	b2_colorMistyRose = 0xFFE4E1,+	b2_colorMoccasin = 0xFFE4B5,+	b2_colorNavajoWhite = 0xFFDEAD,+	b2_colorNavy = 0x000080,+	b2_colorOldLace = 0xFDF5E6,+	b2_colorOlive = 0x808000,+	b2_colorOliveDrab = 0x6B8E23,+	b2_colorOrange = 0xFFA500,+	b2_colorOrangeRed = 0xFF4500,+	b2_colorOrchid = 0xDA70D6,+	b2_colorPaleGoldenRod = 0xEEE8AA,+	b2_colorPaleGreen = 0x98FB98,+	b2_colorPaleTurquoise = 0xAFEEEE,+	b2_colorPaleVioletRed = 0xDB7093,+	b2_colorPapayaWhip = 0xFFEFD5,+	b2_colorPeachPuff = 0xFFDAB9,+	b2_colorPeru = 0xCD853F,+	b2_colorPink = 0xFFC0CB,+	b2_colorPlum = 0xDDA0DD,+	b2_colorPowderBlue = 0xB0E0E6,+	b2_colorPurple = 0x800080,+	b2_colorRebeccaPurple = 0x663399,+	b2_colorRed = 0xFF0000,+	b2_colorRosyBrown = 0xBC8F8F,+	b2_colorRoyalBlue = 0x4169E1,+	b2_colorSaddleBrown = 0x8B4513,+	b2_colorSalmon = 0xFA8072,+	b2_colorSandyBrown = 0xF4A460,+	b2_colorSeaGreen = 0x2E8B57,+	b2_colorSeaShell = 0xFFF5EE,+	b2_colorSienna = 0xA0522D,+	b2_colorSilver = 0xC0C0C0,+	b2_colorSkyBlue = 0x87CEEB,+	b2_colorSlateBlue = 0x6A5ACD,+	b2_colorSlateGray = 0x708090,+	b2_colorSnow = 0xFFFAFA,+	b2_colorSpringGreen = 0x00FF7F,+	b2_colorSteelBlue = 0x4682B4,+	b2_colorTan = 0xD2B48C,+	b2_colorTeal = 0x008080,+	b2_colorThistle = 0xD8BFD8,+	b2_colorTomato = 0xFF6347,+	b2_colorTurquoise = 0x40E0D0,+	b2_colorViolet = 0xEE82EE,+	b2_colorWheat = 0xF5DEB3,+	b2_colorWhite = 0xFFFFFF,+	b2_colorWhiteSmoke = 0xF5F5F5,+	b2_colorYellow = 0xFFFF00,+	b2_colorYellowGreen = 0x9ACD32,++	b2_colorBox2DRed = 0xDC3132,+	b2_colorBox2DBlue = 0x30AEBF,+	b2_colorBox2DGreen = 0x8CC924,+	b2_colorBox2DYellow = 0xFFEE8C+} b2HexColor;++/// Get the visualization color assigned to a constraint graph color slot. The last index+/// (B2_GRAPH_COLOR_COUNT - 1) is the overflow color.+/// @ingroup debug+B2_API b2HexColor b2GetGraphColor( int index );++/// This struct holds callbacks you can implement to draw a Box2D 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. Initialize with+/// b2DefaultDebugDraw.+/// @ingroup debug+typedef struct b2DebugDraw+{+	/// Draw a closed polygon provided in CCW order.+	void ( *DrawPolygonFcn )( b2WorldTransform transform, const b2Vec2* vertices, int vertexCount,+							  b2HexColor color, void* context );++	/// Draw a solid closed polygon provided in CCW order.+	void ( *DrawSolidPolygonFcn )( b2WorldTransform transform, const b2Vec2* vertices,+								   int vertexCount, float radius, b2HexColor color, void* context );++	/// Draw a circle.+	void ( *DrawCircleFcn )( b2Pos center, float radius, b2HexColor color, void* context );++	/// Draw a solid circle.+	void ( *DrawSolidCircleFcn )( b2WorldTransform transform, b2Vec2 center, float radius,+								  b2HexColor color, void* context );++	/// Draw a solid capsule.+	void ( *DrawSolidCapsuleFcn )( b2Pos p1, b2Pos p2, float radius, b2HexColor color,+								   void* context );++	/// Draw a line segment.+	void ( *DrawLineFcn )( b2Pos p1, b2Pos p2, b2HexColor color, void* context );++	/// Draw a transform. Choose your own length scale.+	void ( *DrawTransformFcn )( b2WorldTransform transform, void* context );++	/// Draw a point.+	void ( *DrawPointFcn )( b2Pos p, float size, b2HexColor color, void* context );++	/// Draw a string in world space+	void ( *DrawStringFcn )( b2Pos p, const char* s, b2HexColor color, void* context );++	/// Draw a bounding box. With double precision enabled, the single precision bounding box gets+	/// increasing padding when moving far from the origin.+	void ( *DrawBoundsFcn )( b2AABB aabb, b2HexColor color, void* context );++	/// World bounds used for debug draw. An empty box draws nothing.+	b2AABB drawingBounds;++	/// Scale to use when drawing forces+	float forceScale;++	/// Global scaling for joint drawing+	float jointScale;++	/// Option to draw contact points+	bool drawContacts;++	/// Draw anchor A for contact points (instead of anchorB)+	bool drawAnchorA;++	/// Option to draw shapes+	bool drawShapes;++	/// Option to draw chain shape normals+	bool drawChainNormals;++	/// Option to draw joints+	bool drawJoints;++	/// Option to draw additional information for joints+	bool drawJointExtras;++	/// Option to draw the bounding boxes for shapes+	bool drawBounds;++	/// Option to draw the mass and center of mass of dynamic bodies+	bool drawMass;++	/// Option to draw body names+	bool drawBodyNames;++	/// Option to visualize the graph coloring used for contacts and joints+	bool drawGraphColors;++	/// Option to draw contact feature ids+	bool drawContactFeatures;++	/// Option to draw contact normals+	bool drawContactNormals;++	/// Option to draw contact normal forces+	bool drawContactForces;++	/// Option to draw contact friction forces+	bool drawFrictionForces;++	/// Option to draw islands as bounding boxes+	bool drawIslands;++	/// User context that is passed as an argument to drawing callback functions+	void* context;+} b2DebugDraw;++/// Use this to initialize your drawing interface. This allows you to implement a subset of the+/// drawing functions.+/// @ingroup debug+B2_API b2DebugDraw b2DefaultDebugDraw( void );++/// Opaque handle for incremental playback of a recording.+/// @ingroup replay+typedef struct b2Replay b2Replay;++/// Static metadata describing a recording, resolved once when the player opens the file.+/// @ingroup replay+typedef struct b2ReplayInfo+{+	int frameCount;	   ///< total recorded steps+	int workerCount;   ///< worker count used 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+	b2AABB bounds;	   ///< accumulated world bounds over the recording, zero-extent if unavailable+} b2ReplayInfo;++/// The kind of a recorded spatial query, matching the public query and cast functions.+/// @ingroup replay+typedef enum b2ReplayQueryType+{+	b2_replayQueryOverlapAABB,+	b2_replayQueryOverlapShape,+	b2_replayQueryCastRay,+	b2_replayQueryCastShape,+	b2_replayQueryCollideMover,+	b2_replayQueryCastRayClosest,+	b2_replayQueryCastMover,+} b2ReplayQueryType;++/// A spatial query recorded during a replayed frame, exposed for inspection.+/// @ingroup replay+typedef struct b2ReplayQueryInfo+{+	b2ReplayQueryType type; ///< the query type+	b2QueryFilter filter;	///< the query filter+	b2AABB aabb;			///< overlap AABB, relative to origin+	b2Pos origin;			///< query origin+	b2Vec2 translation;		///< ray and cast translation+	int hitCount;			///< number of recorded results+} b2ReplayQueryInfo;++/// One result of a recorded spatial query.+/// @ingroup replay+typedef struct b2ReplayQueryHit+{+	b2ShapeId shapeId; ///< the shape hit+	b2Pos point;	   ///< the hit point on the shape surface+	b2Vec2 normal;	   ///< the unit normal vector at the hit point+	float fraction;	   ///< the query translation fraction+} b2ReplayQueryHit;
+ upstream/src/aabb.c view
@@ -0,0 +1,137 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "aabb.h"++#include "box2d/math_functions.h"++#include <float.h>++bool b2IsValidAABB( b2AABB a )+{+	b2Vec2 d = b2Sub( a.upperBound, a.lowerBound );+	bool valid = d.x >= 0.0f && d.y >= 0.0f;+	valid = valid && b2IsValidVec2( a.lowerBound ) && b2IsValidVec2( a.upperBound );+	return valid;+}++// From Real-time Collision Detection, p179.+b2CastOutput b2AABB_RayCast( b2AABB a, b2Vec2 p1, b2Vec2 p2 )+{+	// Radius not handled+	b2CastOutput output = { 0 };++	float tMin = -FLT_MAX;+	float tMax = FLT_MAX;++	b2Vec2 p = p1;+	b2Vec2 d = b2Sub( p2, p1 );+	b2Vec2 absD = b2Abs( d );++	b2Vec2 normal = b2Vec2_zero;++	// x-coordinate+	if ( absD.x < FLT_EPSILON )+	{+		// parallel+		if ( p.x < a.lowerBound.x || a.upperBound.x < p.x )+		{+			return output;+		}+	}+	else+	{+		float inv_d = 1.0f / d.x;+		float t1 = ( a.lowerBound.x - p.x ) * inv_d;+		float t2 = ( a.upperBound.x - p.x ) * inv_d;++		// Sign of the normal vector.+		float s = -1.0f;++		if ( t1 > t2 )+		{+			float tmp = t1;+			t1 = t2;+			t2 = tmp;+			s = 1.0f;+		}++		// Push the min up+		if ( t1 > tMin )+		{+			normal.y = 0.0f;+			normal.x = s;+			tMin = t1;+		}++		// Pull the max down+		tMax = b2MinFloat( tMax, t2 );++		if ( tMin > tMax )+		{+			return output;+		}+	}++	// y-coordinate+	if ( absD.y < FLT_EPSILON )+	{+		// parallel+		if ( p.y < a.lowerBound.y || a.upperBound.y < p.y )+		{+			return output;+		}+	}+	else+	{+		float inv_d = 1.0f / d.y;+		float t1 = ( a.lowerBound.y - p.y ) * inv_d;+		float t2 = ( a.upperBound.y - p.y ) * inv_d;++		// Sign of the normal vector.+		float s = -1.0f;++		if ( t1 > t2 )+		{+			float tmp = t1;+			t1 = t2;+			t2 = tmp;+			s = 1.0f;+		}++		// Push the min up+		if ( t1 > tMin )+		{+			normal.x = 0.0f;+			normal.y = s;+			tMin = t1;+		}++		// Pull the max down+		tMax = b2MinFloat( tMax, t2 );++		if ( tMin > tMax )+		{+			return output;+		}+	}++	// Does the ray start inside the box?+	if ( tMin < 0.0f )+	{+		return output;+	}++	// Does the ray intersect beyond the segment length?+	if ( 1.0f < tMin )+	{+		return output;+	}++	// Intersection.+	output.fraction = tMin;+	output.normal = normal;+	output.point = b2Lerp( p1, p2, tMin );+	output.hit = true;+	return output;+}
+ upstream/src/aabb.h view
@@ -0,0 +1,64 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box2d/math_functions.h"+#include "box2d/types.h"++// Ray cast an AABB+b2CastOutput b2AABB_RayCast( b2AABB a, b2Vec2 p1, b2Vec2 p2 );++// Get surface area of an AABB (the perimeter length)+static inline float b2Perimeter( b2AABB a )+{+	float wx = a.upperBound.x - a.lowerBound.x;+	float wy = a.upperBound.y - a.lowerBound.y;+	return 2.0f * ( wx + wy );+}++/// Enlarge a to contain b+/// @return true if the AABB grew+static inline bool b2EnlargeAABB( b2AABB* a, b2AABB 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 ( 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;+	}++	return changed;+}++// Translate a relative AABB into world space, rounding outward so the float box always contains+// the true box far from the origin where a float coordinate cannot resolve the shape extent.+// Float ULP at 1e8 dwarfs the AABB margin, plain truncation could clip a shape out of its own+// box. The broadphase pair order rides on the deterministic directed rounding.+static inline b2AABB b2OffsetAABB( b2AABB box, b2Pos origin )+{+	b2AABB result;+	result.lowerBound.x = b2RoundDownFloat( origin.x + (double)box.lowerBound.x );+	result.lowerBound.y = b2RoundDownFloat( origin.y + (double)box.lowerBound.y );+	result.upperBound.x = b2RoundUpFloat( origin.x + (double)box.upperBound.x );+	result.upperBound.y = b2RoundUpFloat( origin.y + (double)box.upperBound.y );+	return result;+}
+ upstream/src/arena_allocator.c view
@@ -0,0 +1,108 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "arena_allocator.h"++#include "core.h"++#include <stdbool.h>+#include <stddef.h>++b2Stack b2CreateStack( int capacity )+{+	B2_ASSERT( capacity >= 0 );+	b2Stack allocator = { 0 };+	allocator.capacity = capacity;+	allocator.data = b2Alloc( capacity );+	allocator.allocation = 0;+	allocator.maxAllocation = 0;+	allocator.index = 0;+	b2Array_CreateN( allocator.entries, 32 );+	return allocator;+}++void b2DestroyStack( b2Stack* allocator )+{+	b2Array_Destroy( allocator->entries );+	b2Free( allocator->data, allocator->capacity );+}++void* b2StackAlloc( b2Stack* alloc, int size, const char* name )+{+	int alignedSize = ( ( size - 1 ) | ( B2_ALIGNMENT - 1 ) ) + 1;++	b2StackEntry entry;+	entry.size = alignedSize;+	entry.name = name;+	if ( alloc->index + alignedSize > alloc->capacity )+	{+		// fall back to the heap (undesirable)+		entry.data = b2Alloc( alignedSize );+		entry.usedMalloc = true;++		B2_ASSERT( ( (uintptr_t)entry.data & ( B2_ALIGNMENT - 1 ) ) == 0 );+	}+	else+	{+		entry.data = alloc->data + alloc->index;+		entry.usedMalloc = false;+		alloc->index += alignedSize;++		B2_ASSERT( ( (uintptr_t)entry.data & ( B2_ALIGNMENT - 1 ) ) == 0 );+	}++	alloc->allocation += alignedSize;+	if ( alloc->allocation > alloc->maxAllocation )+	{+		alloc->maxAllocation = alloc->allocation;+	}++	b2Array_Push( alloc->entries, entry );+	return entry.data;+}++void b2StackFree( b2Stack* alloc, void* mem )+{+	int entryCount = alloc->entries.count;+	B2_ASSERT( entryCount > 0 );+	b2StackEntry* entry = alloc->entries.data + ( entryCount - 1 );+	B2_ASSERT( mem == entry->data );+	if ( entry->usedMalloc )+	{+		b2Free( mem, entry->size );+	}+	else+	{+		alloc->index -= entry->size;+	}+	alloc->allocation -= entry->size;+	b2Array_Pop( alloc->entries );+}++void b2GrowStack( b2Stack* alloc )+{+	// Stack must not be in use+	B2_ASSERT( alloc->allocation == 0 );++	if ( alloc->maxAllocation > alloc->capacity )+	{+		b2Free( alloc->data, alloc->capacity );+		alloc->capacity = alloc->maxAllocation + alloc->maxAllocation / 2;+		alloc->data = b2Alloc( alloc->capacity );+	}+}++int b2GetStackCapacity( b2Stack* alloc )+{+	return alloc->capacity;+}++int b2GetStackAllocation( b2Stack* alloc )+{+	return alloc->allocation;+}++int b2GetMaxStackAllocation( b2Stack* alloc )+{+	return alloc->maxAllocation;+}
+ upstream/src/arena_allocator.h view
@@ -0,0 +1,49 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"++#include <stdbool.h>++b2DeclareArray( b2StackEntry );++typedef struct b2StackEntry+{+	char* data;+	const char* name;+	int size;+	bool usedMalloc;+} b2StackEntry;++// This is a stack-like arena allocator used for fast per step allocations.+// You must nest allocate/free pairs. The code will B2_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 b2Stack+{+	char* data;+	int capacity;+	int index;++	int allocation;+	int maxAllocation;++	b2Array( b2StackEntry ) entries;+} b2Stack;++b2Stack b2CreateStack( int capacity );+void b2DestroyStack( b2Stack* allocator );++void* b2StackAlloc( b2Stack* alloc, int size, const char* name );+void b2StackFree( b2Stack* alloc, void* mem );++// Grow the stack based on usage+void b2GrowStack( b2Stack* alloc );++int b2GetStackCapacity( b2Stack* alloc );+int b2GetStackAllocation( b2Stack* alloc );+int b2GetMaxStackAllocation( b2Stack* alloc );+
+ upstream/src/bitset.c view
@@ -0,0 +1,67 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "bitset.h"++#include <string.h>++b2BitSet b2CreateBitSet( uint32_t bitCapacity )+{+	b2BitSet bitSet = { 0 };++	bitSet.blockCapacity = ( bitCapacity + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 );+	bitSet.blockCount = 0;+	bitSet.bits = b2Alloc( bitSet.blockCapacity * sizeof( uint64_t ) );+	memset( bitSet.bits, 0, bitSet.blockCapacity * sizeof( uint64_t ) );+	return bitSet;+}++void b2DestroyBitSet( b2BitSet* bitSet )+{+	b2Free( bitSet->bits, bitSet->blockCapacity * sizeof( uint64_t ) );+	bitSet->blockCapacity = 0;+	bitSet->blockCount = 0;+	bitSet->bits = NULL;+}++void b2SetBitCountAndClear( b2BitSet* bitSet, uint32_t bitCount )+{+	uint32_t blockCount = ( bitCount + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 );+	if ( bitSet->blockCapacity < blockCount )+	{+		b2DestroyBitSet( bitSet );+		uint32_t newBitCapacity = bitCount + ( bitCount >> 1 );+		*bitSet = b2CreateBitSet( newBitCapacity );+	}++	bitSet->blockCount = blockCount;+	memset( bitSet->bits, 0, bitSet->blockCount * sizeof( uint64_t ) );+}++void b2GrowBitSet( b2BitSet* bitSet, uint32_t blockCount )+{+	B2_ASSERT( blockCount > bitSet->blockCount );+	if ( blockCount > bitSet->blockCapacity )+	{+		uint32_t oldCapacity = bitSet->blockCapacity;+		bitSet->blockCapacity = blockCount + blockCount / 2;+		uint64_t* newBits = b2Alloc( bitSet->blockCapacity * sizeof( uint64_t ) );+		memset( newBits, 0, bitSet->blockCapacity * sizeof( uint64_t ) );+		B2_ASSERT( bitSet->bits != NULL );+		memcpy( newBits, bitSet->bits, oldCapacity * sizeof( uint64_t ) );+		b2Free( bitSet->bits, oldCapacity * sizeof( uint64_t ) );+		bitSet->bits = newBits;+	}++	bitSet->blockCount = blockCount;+}++void b2InPlaceUnion( b2BitSet* B2_RESTRICT setA, const b2BitSet* B2_RESTRICT setB )+{+	B2_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,66 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "core.h"++#include <stdbool.h>+#include <stdint.h>++// Bit set provides fast operations on large arrays of bits.+typedef struct b2BitSet+{+	uint64_t* bits;+	uint32_t blockCapacity;+	uint32_t blockCount;+} b2BitSet;++b2BitSet b2CreateBitSet( uint32_t bitCapacity );+void b2DestroyBitSet( b2BitSet* bitSet );+void b2SetBitCountAndClear( b2BitSet* bitSet, uint32_t bitCount );+void b2InPlaceUnion( b2BitSet* setA, const b2BitSet* setB );+void b2GrowBitSet( b2BitSet* bitSet, uint32_t blockCount );+int b2CountSetBits( b2BitSet* bitSet );++static inline void b2SetBit( b2BitSet* bitSet, uint32_t bitIndex )+{+	uint32_t blockIndex = bitIndex / 64;+	B2_ASSERT( blockIndex < bitSet->blockCount );+	bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 );+}++static inline void b2SetBitGrow( b2BitSet* bitSet, uint32_t bitIndex )+{+	uint32_t blockIndex = bitIndex / 64;+	if ( blockIndex >= bitSet->blockCount )+	{+		b2GrowBitSet( bitSet, blockIndex + 1 );+	}+	bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 );+}++static inline void b2ClearBit( b2BitSet* 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 b2GetBit( const b2BitSet* 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 b2GetBitSetBytes( b2BitSet* bitSet )+{+	return bitSet->blockCapacity * sizeof( uint64_t );+}
+ upstream/src/body.c view
@@ -0,0 +1,2199 @@+// SPDX-FileCopyrightText: 2023 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"++#include "box2d/box2d.h"+#include "box2d/id.h"++#include <string.h>++_Static_assert( B2_NAME_LENGTH >= 0, "minimum name length" );++static void b2LimitVelocity( b2BodyState* state, float maxLinearSpeed )+{+	float v2 = b2LengthSquared( state->linearVelocity );+	if ( v2 > maxLinearSpeed * maxLinearSpeed )+	{+		state->linearVelocity = b2MulSV( maxLinearSpeed / sqrtf( v2 ), state->linearVelocity );+	}+}++// Refresh the body sim index on associated contact sims.+void b2RefreshBodyContactIndices( b2World* world, b2Body* body )+{+	int encodedIndex = b2EncodeBodySimIndex( body );++	int contactKey = body->headContactKey;+	while ( contactKey != B2_NULL_INDEX )+	{+		int contactId = contactKey >> 1;+		int edgeIndex = contactKey & 1;++		b2Contact* contact = b2Array_Get( world->contacts, contactId );+		b2ContactSim* contactSim = b2GetContactSim( world, contact );++		if ( edgeIndex == 0 )+		{+			contactSim->encodedBodySimA = encodedIndex;+		}+		else+		{+			contactSim->encodedBodySimB = encodedIndex;+		}++		contactKey = contact->edges[edgeIndex].nextKey;+	}+}++// Get a validated body from a world using an id.+b2Body* b2GetBodyFullId( b2World* world, b2BodyId bodyId )+{+	B2_ASSERT( b2Body_IsValid( bodyId ) );++	// id index starts at one so that zero can represent null+	return b2Array_Get( world->bodies, bodyId.index1 - 1 );+}++b2WorldTransform b2GetBodyTransformQuick( b2World* world, b2Body* body )+{+	b2SolverSet* set = b2Array_Get( world->solverSets, body->setIndex );+	b2BodySim* bodySim = b2Array_Get( set->bodySims, body->localIndex );+	return bodySim->transform;+}++b2WorldTransform b2GetBodyTransform( b2World* world, int bodyId )+{+	b2Body* body = b2Array_Get( world->bodies, bodyId );+	return b2GetBodyTransformQuick( world, body );+}++// Create a b2BodyId from a raw id.+b2BodyId b2MakeBodyId( b2World* world, int bodyId )+{+	b2Body* body = b2Array_Get( world->bodies, bodyId );+	return (b2BodyId){ bodyId + 1, world->worldId, body->generation };+}++// Sync the body flags when something external changes them,+// such as the user changing the motion locks.+void b2SyncBodyFlags( b2World* world, b2Body* body )+{+	b2BodySim* bodySim = b2GetBodySim( world, body );++	// Preserve the sim only flags: fast for contact recycling, time of impact for debug draw.+	bodySim->flags = ( bodySim->flags & ( b2_isFast | b2_hadTimeOfImpact ) ) | ( body->flags & ~b2_bodyTransientFlags );++	b2BodyState* bodyState = b2GetBodyState( world, body );+	if ( bodyState != NULL )+	{+		bodyState->flags = body->flags & ~b2_bodyTransientFlags;+	}+}++static void b2CreateIslandForBody( b2World* world, int setIndex, b2Body* body )+{+	B2_ASSERT( body->islandId == B2_NULL_INDEX );+	B2_ASSERT( setIndex != b2_disabledSet );++	b2Island* island = b2CreateIsland( world, setIndex );+	b2Array_Push( island->bodies, body->id );+	body->islandId = island->islandId;+	body->islandIndex = 0;++	b2ValidateIsland( world, island->islandId );+}++static void b2RemoveBodyFromIsland( b2World* world, b2Body* body )+{+	if ( body->islandId == B2_NULL_INDEX )+	{+		B2_ASSERT( body->islandIndex == B2_NULL_INDEX );+		return;+	}++	int islandId = body->islandId;+	b2Island* island = b2Array_Get( world->islands, islandId );+	{+		int localIndex = body->islandIndex;+		int movedBodyId = island->bodies.data[island->bodies.count - 1];+		island->bodies.data[localIndex] = movedBodyId;+		B2_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+		B2_ASSERT( island->contacts.count == 0 );+		B2_ASSERT( island->joints.count == 0 );++		// Free the island+		b2DestroyIsland( world, island->islandId );+	}+	else+	{+		b2ValidateIsland( world, islandId );+	}++	body->islandId = B2_NULL_INDEX;+	body->islandIndex = B2_NULL_INDEX;+}++static void b2DestroyBodyContacts( b2World* world, b2Body* body )+{+	// Destroy the attached contacts+	int edgeKey = body->headContactKey;+	while ( edgeKey != B2_NULL_INDEX )+	{+		int contactId = edgeKey >> 1;+		int edgeIndex = edgeKey & 1;++		b2Contact* contact = b2Array_Get( world->contacts, contactId );+		edgeKey = contact->edges[edgeIndex].nextKey;+		b2DestroyContact( world, contact );+	}++	b2ValidateSolverSets( world );+}++b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidPos( def->position ), b2_nullBodyId );+	B2_CHECK_INPUT_RETURN( b2IsValidRot( def->rotation ), b2_nullBodyId );+	B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->linearVelocity ), b2_nullBodyId );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->angularVelocity ), b2_nullBodyId );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->linearDamping ) && def->linearDamping >= 0.0f, b2_nullBodyId );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->angularDamping ) && def->angularDamping >= 0.0f, b2_nullBodyId );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->sleepThreshold ) && def->sleepThreshold >= 0.0f, b2_nullBodyId );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->safetyFactor ) && def->safetyFactor >= 0.0f, b2_nullBodyId );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->gravityScale ), b2_nullBodyId );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return b2_nullBodyId;+	}++	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 = b2_disabledSet;+	}+	else if ( def->type == b2_staticBody )+	{+		setId = b2_staticSet;+	}+	else if ( isAwake == true )+	{+		setId = b2_awakeSet;+	}+	else+	{+		// new set for a sleeping body in its own island+		setId = b2AllocId( &world->solverSetIdPool );+		if ( setId == world->solverSets.count )+		{+			// Create a zero initialized solver set. All sub-arrays are also zero initialized.+			b2Array_Push( world->solverSets, (b2SolverSet){ 0 } );+		}+		else+		{+			B2_ASSERT( world->solverSets.data[setId].setIndex == B2_NULL_INDEX );+		}++		world->solverSets.data[setId].setIndex = setId;+	}++	B2_ASSERT( 0 <= setId && setId < world->solverSets.count );++	int bodyId = b2AllocId( &world->bodyIdPool );++	uint32_t lockFlags = 0;+	lockFlags |= def->motionLocks.linearX ? b2_lockLinearX : 0;+	lockFlags |= def->motionLocks.linearY ? b2_lockLinearY : 0;+	lockFlags |= def->motionLocks.angularZ ? b2_lockAngularZ : 0;++	b2SolverSet* set = b2Array_Get( world->solverSets, setId );+	b2BodySim* bodySim = b2Array_Emplace( set->bodySims );+	*bodySim = (b2BodySim){ 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->minExtent = B2_HUGE;+	bodySim->maxExtent = 0.0f;+	bodySim->linearDamping = def->linearDamping;+	bodySim->angularDamping = def->angularDamping;+	bodySim->gravityScale = def->gravityScale;+	bodySim->bodyId = bodyId;+	bodySim->flags = lockFlags;+	bodySim->flags |= def->isBullet ? b2_isBullet : 0;+	bodySim->flags |= def->allowFastRotation ? b2_allowFastRotation : 0;+	bodySim->flags |= def->type == b2_dynamicBody ? b2_dynamicFlag : 0;+	bodySim->flags |= def->enableSleep ? b2_enableSleep : 0;+	bodySim->flags |= def->enableContactRecycling ? b2_bodyEnableContactRecycling : 0;++	if ( setId == b2_awakeSet )+	{+		b2BodyState* bodyState = b2Array_Emplace( set->bodyStates );+		B2_ASSERT( ( (uintptr_t)bodyState & 0x1F ) == 0 );++		*bodyState = (b2BodyState){ 0 };+		bodyState->linearVelocity = def->linearVelocity;+		bodyState->angularVelocity = def->angularVelocity;+		bodyState->deltaRotation = b2Rot_identity;+		bodyState->flags = bodySim->flags;+	}++	if ( bodyId == world->bodies.count )+	{+		b2Array_Push( world->bodies, (b2Body){ 0 } );+	}+	else+	{+		B2_ASSERT( world->bodies.data[bodyId].id == B2_NULL_INDEX );+	}++	b2Body* body = b2Array_Get( world->bodies, bodyId );++	if ( def->name )+	{+#if defined( _MSC_VER )+		strncpy_s( body->name, B2_NAME_LENGTH + 1, def->name, B2_NAME_LENGTH );+#else+		strncpy( body->name, def->name, B2_NAME_LENGTH );+		body->name[B2_NAME_LENGTH] = 0;+#endif+	}+	else+	{+		memset( body->name, 0, sizeof( body->name ) );+	}++	body->userData = def->userData;+	body->setIndex = setId;+	body->localIndex = set->bodySims.count - 1;+	body->generation += 1;+	body->headShapeId = B2_NULL_INDEX;+	body->shapeCount = 0;+	body->headChainId = B2_NULL_INDEX;+	body->headContactKey = B2_NULL_INDEX;+	body->contactCount = 0;+	body->headJointKey = B2_NULL_INDEX;+	body->jointCount = 0;+	body->islandId = B2_NULL_INDEX;+	body->islandIndex = B2_NULL_INDEX;+	body->bodyMoveIndex = B2_NULL_INDEX;+	body->id = bodyId;+	body->mass = 0.0f;+	body->inertia = 0.0f;+	body->sleepThreshold = def->sleepThreshold;+	body->sleepTime = 0.0f;+	body->safetyFactor = def->safetyFactor;+	body->type = def->type;+	body->flags = bodySim->flags;++	// dynamic and kinematic bodies that are enabled need a island+	if ( setId >= b2_awakeSet )+	{+		b2CreateIslandForBody( world, setId, body );+	}++	b2ValidateSolverSets( world );++	b2BodyId id = { bodyId + 1, world->worldId, body->generation };++	B2_REC_CREATE( world, CreateBody, id, worldId, *def );++	return id;+}++bool b2WakeBody( b2World* world, b2Body* body )+{+	if ( body->setIndex >= b2_firstSleepingSet )+	{+		b2WakeSolverSet( world, body->setIndex );+		b2ValidateSolverSets( world );+		return true;+	}++	return false;+}++void b2DestroyBody( b2BodyId bodyId )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	// Record before destroying (body must still be valid)+	B2_REC( world, DestroyBody, bodyId );++	b2Body* body = b2GetBodyFullId( world, bodyId );++	// Destroy the attached joints+	int edgeKey = body->headJointKey;+	while ( edgeKey != B2_NULL_INDEX )+	{+		int jointId = edgeKey >> 1;+		int edgeIndex = edgeKey & 1;++		b2Joint* joint = b2Array_Get( world->joints, jointId );+		edgeKey = joint->edges[edgeIndex].nextKey;++		// Careful because this modifies the list being traversed+		b2DestroyJointInternal( world, joint );+	}++	// Destroy all contacts attached to this body.+	b2DestroyBodyContacts( world, body );++	// Destroy the attached shapes and their broad-phase proxies.+	int shapeId = body->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );++		if ( shape->sensorIndex != B2_NULL_INDEX )+		{+			b2DestroySensor( world, shape );+		}++		b2DestroyShapeProxy( shape, &world->broadPhase );++		// Return shape to free list.+		b2FreeId( &world->shapeIdPool, shapeId );+		shape->id = B2_NULL_INDEX;++		shapeId = shape->nextShapeId;+	}++	// Destroy the attached chains. The associated shapes have already been destroyed above.+	int chainId = body->headChainId;+	while ( chainId != B2_NULL_INDEX )+	{+		b2ChainShape* chain = b2Array_Get( world->chainShapes, chainId );++		b2FreeChainData( chain );++		// Return chain to free list.+		b2FreeId( &world->chainIdPool, chainId );+		chain->id = B2_NULL_INDEX;++		chainId = chain->nextChainId;+	}++	b2RemoveBodyFromIsland( world, body );++	// Remove body sim from solver set that owns it+	b2SolverSet* set = b2Array_Get( world->solverSets, body->setIndex );+	b2Body* movedBody = b2RemoveBodySim( world, set, body->localIndex );+	if ( movedBody != NULL )+	{+		b2RefreshBodyContactIndices( world, movedBody );+	}++	// Remove body state from awake set+	if ( body->setIndex == b2_awakeSet )+	{+		(void)b2Array_RemoveSwap( set->bodyStates, body->localIndex );+	}+	else if ( set->setIndex >= b2_firstSleepingSet && set->bodySims.count == 0 )+	{+		// Remove solver set if it is empty+		b2DestroySolverSet( world, set->setIndex );+	}++	// Free body and id (preserve body generation)+	b2FreeId( &world->bodyIdPool, body->id );++	body->setIndex = B2_NULL_INDEX;+	body->localIndex = B2_NULL_INDEX;+	body->id = B2_NULL_INDEX;++	b2ValidateSolverSets( world );+}++int b2Body_GetContactCapacity( b2BodyId bodyId )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return 0;+	}++	b2Body* body = b2GetBodyFullId( world, bodyId );++	// Conservative and fast+	return body->contactCount;+}++int b2Body_GetContactData( b2BodyId bodyId, b2ContactData* contactData, int capacity )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return 0;+	}++	b2Body* body = b2GetBodyFullId( world, bodyId );++	int contactKey = body->headContactKey;+	int index = 0;+	while ( contactKey != B2_NULL_INDEX && index < capacity )+	{+		int contactId = contactKey >> 1;+		int edgeIndex = contactKey & 1;++		b2Contact* contact = b2Array_Get( world->contacts, contactId );++		// Is contact touching?+		if ( contact->flags & b2_contactTouchingFlag )+		{+			b2Shape* shapeA = b2Array_Get( world->shapes, contact->shapeIdA );+			b2Shape* shapeB = b2Array_Get( world->shapes, contact->shapeIdB );++			contactData[index].contactId = (b2ContactId){ contact->contactId + 1, bodyId.world0, 0, contact->generation };+			contactData[index].shapeIdA = (b2ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation };+			contactData[index].shapeIdB = (b2ShapeId){ shapeB->id + 1, bodyId.world0, shapeB->generation };++			b2ContactSim* contactSim = b2GetContactSim( world, contact );+			contactData[index].manifold = contactSim->manifold;++			index += 1;+		}++		contactKey = contact->edges[edgeIndex].nextKey;+	}++	B2_ASSERT( index <= capacity );++	return index;+}++b2AABB b2Body_ComputeBounds( b2BodyId bodyId )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return (b2AABB){ 0 };+	}++	b2Body* body = b2GetBodyFullId( world, bodyId );+	if ( body->headShapeId == B2_NULL_INDEX )+	{+		// No shapes, bracket the body origin so the box still contains the true position far away+		b2WorldTransform transform = b2GetBodyTransform( world, body->id );+		b2Vec2 lower = { b2RoundDownFloat( transform.p.x ), b2RoundDownFloat( transform.p.y ) };+		b2Vec2 upper = { b2RoundUpFloat( transform.p.x ), b2RoundUpFloat( transform.p.y ) };+		return (b2AABB){ lower, upper };+	}++	b2Shape* shape = b2Array_Get( world->shapes, body->headShapeId );+	b2AABB aabb = shape->aabb;+	while ( shape->nextShapeId != B2_NULL_INDEX )+	{+		shape = b2Array_Get( world->shapes, shape->nextShapeId );+		aabb = b2AABB_Union( aabb, shape->aabb );+	}++	return aabb;+}++void b2UpdateBodyMassData( b2World* world, b2Body* body )+{+	b2BodySim* bodySim = b2GetBodySim( world, body );++	// Mass is no longer dirty+	body->flags &= ~b2_dirtyMass;++	// Compute mass data from shapes. Each shape has its own density.+	body->mass = 0.0f;+	body->inertia = 0.0f;++	bodySim->invMass = 0.0f;+	bodySim->invInertia = 0.0f;+	bodySim->localCenter = b2Vec2_zero;+	bodySim->minExtent = B2_HUGE;+	bodySim->maxExtent = 0.0f;++	// Static and kinematic sims have zero mass.+	if ( body->type != b2_dynamicBody )+	{+		bodySim->center = bodySim->transform.p;+		bodySim->center0 = bodySim->center;++		// Need extents for kinematic bodies for sleeping to work correctly.+		if ( body->type == b2_kinematicBody )+		{+			int shapeId = body->headShapeId;+			while ( shapeId != B2_NULL_INDEX )+			{+				const b2Shape* s = b2Array_Get( world->shapes, shapeId );++				b2ShapeExtent extent = b2ComputeShapeExtent( s, b2Vec2_zero );+				bodySim->minExtent = b2MinFloat( bodySim->minExtent, extent.minExtent );+				bodySim->maxExtent = b2MaxFloat( bodySim->maxExtent, extent.maxExtent );++				shapeId = s->nextShapeId;+			}+		}++		return;+	}++	int shapeCount = body->shapeCount;+	b2MassData* masses = b2StackAlloc( &world->stack, shapeCount * sizeof( b2MassData ), "mass data" );++	// Accumulate mass over all shapes.+	b2Vec2 localCenter = b2Vec2_zero;+	int shapeId = body->headShapeId;+	int shapeIndex = 0;+	while ( shapeId != B2_NULL_INDEX )+	{+		const b2Shape* s = b2Array_Get( world->shapes, shapeId );+		shapeId = s->nextShapeId;++		if ( s->density == 0.0f )+		{+			masses[shapeIndex] = (b2MassData){ 0 };+			shapeIndex += 1;+			continue;+		}++		b2MassData massData = b2ComputeShapeMass( s );+		body->mass += massData.mass;+		localCenter = b2MulAdd( 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 = b2MulSV( bodySim->invMass, localCenter );+	}++	// Second loop to accumulate the rotational inertia about the center of mass+	for ( shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex )+	{+		b2MassData massData = masses[shapeIndex];+		if ( massData.mass == 0.0f )+		{+			continue;+		}++		// Shift to center of mass. This is safe because it can only increase.+		b2Vec2 offset = b2Sub( localCenter, massData.center );+		float inertia = massData.rotationalInertia + massData.mass * b2Dot( offset, offset );+		body->inertia += inertia;+	}++	b2StackFree( &world->stack, masses );+	masses = NULL;++	B2_ASSERT( body->inertia >= 0.0f );++	if ( body->inertia > 0.0f && ( body->flags & b2_fixedRotation ) == 0 )+	{+		bodySim->invInertia = 1.0f / body->inertia;+	}+	else+	{+		body->inertia = 0.0f;+		bodySim->invInertia = 0.0f;+	}++	// Move center of mass.+	b2Pos oldCenter = bodySim->center;+	bodySim->localCenter = localCenter;+	bodySim->center = b2TransformWorldPoint( bodySim->transform, bodySim->localCenter );+	bodySim->center0 = bodySim->center;++	// Update center of mass velocity+	b2BodyState* state = b2GetBodyState( world, body );+	if ( state != NULL )+	{+		b2Vec2 deltaLinear = b2CrossSV( state->angularVelocity, b2SubPos( bodySim->center, oldCenter ) );+		state->linearVelocity = b2Add( state->linearVelocity, deltaLinear );+	}++	// Compute body extents relative to center of mass+	shapeId = body->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		const b2Shape* s = b2Array_Get( world->shapes, shapeId );++		b2ShapeExtent extent = b2ComputeShapeExtent( s, localCenter );+		bodySim->minExtent = b2MinFloat( bodySim->minExtent, extent.minExtent );+		bodySim->maxExtent = b2MaxFloat( bodySim->maxExtent, extent.maxExtent );++		shapeId = s->nextShapeId;+	}++	// When the center of mass changes, any cached contact manifold becomes invalid.+	int edgeKey = body->headContactKey;+	while ( edgeKey != B2_NULL_INDEX )+	{+		int contactId = edgeKey >> 1;+		int edgeIndex = edgeKey & 1;++		b2Contact* contact = b2Array_Get( world->contacts, contactId );+		b2ContactSim* contactSim = b2GetContactSim( world, contact );+		contactSim->simFlags &= ~b2_simRelativeTransformValid;++		edgeKey = contact->edges[edgeIndex].nextKey;+	}+}++b2Pos b2Body_GetPosition( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );+	return transform.p;+}++b2Rot b2Body_GetRotation( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );+	return transform.q;+}++b2WorldTransform b2Body_GetTransform( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return b2GetBodyTransformQuick( world, body );+}++b2Vec2 b2Body_GetLocalPoint( b2BodyId bodyId, b2Pos worldPoint )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );+	return b2InvTransformWorldPoint( transform, worldPoint );+}++b2Pos b2Body_GetWorldPoint( b2BodyId bodyId, b2Vec2 localPoint )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );+	return b2TransformWorldPoint( transform, localPoint );+}++b2Vec2 b2Body_GetLocalVector( b2BodyId bodyId, b2Vec2 worldVector )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );+	return b2InvRotateVector( transform.q, worldVector );+}++b2Vec2 b2Body_GetWorldVector( b2BodyId bodyId, b2Vec2 localVector )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );+	return b2RotateVector( transform.q, localVector );+}++void b2Body_SetTransform( b2BodyId bodyId, b2Pos position, b2Rot rotation )+{+	B2_CHECK_INPUT( b2IsValidPos( position ) );+	B2_CHECK_INPUT( b2IsValidRot( rotation ) );+	B2_ASSERT( b2Body_IsValid( bodyId ) );+	b2World* world = b2GetWorld( bodyId.world0 );+	B2_ASSERT( world->locked == false );++	B2_REC( world, BodySetTransform, bodyId, position, rotation );++	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );++	bodySim->transform.p = position;+	bodySim->transform.q = rotation;+	bodySim->center = b2TransformWorldPoint( bodySim->transform, bodySim->localCenter );++	bodySim->rotation0 = bodySim->transform.q;+	bodySim->center0 = bodySim->center;++	b2BroadPhase* broadPhase = &world->broadPhase;++	b2WorldTransform transform = bodySim->transform;+	const float speculativeDistance = B2_SPECULATIVE_DISTANCE;++	int shapeId = body->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		b2AABB aabb = b2ComputeFatShapeAABB( shape, transform, speculativeDistance );+		shape->aabb = aabb;++		b2AABB* shapeFatAABB = world->fatAABBs.data + shapeId;+		if ( b2AABB_Contains( *shapeFatAABB, aabb ) == false )+		{+			float margin = shape->aabbMargin;+			b2AABB fatAABB;+			fatAABB.lowerBound.x = aabb.lowerBound.x - margin;+			fatAABB.lowerBound.y = aabb.lowerBound.y - margin;+			fatAABB.upperBound.x = aabb.upperBound.x + margin;+			fatAABB.upperBound.y = aabb.upperBound.y + margin;+			*shapeFatAABB = fatAABB;++			// They body could be disabled+			if ( shape->proxyKey != B2_NULL_INDEX )+			{+				b2BroadPhase_MoveProxy( broadPhase, shape->proxyKey, fatAABB );+			}+		}++		shapeId = shape->nextShapeId;+	}+}++b2Vec2 b2Body_GetLinearVelocity( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodyState* state = b2GetBodyState( world, body );+	if ( state != NULL )+	{+		return state->linearVelocity;+	}+	return b2Vec2_zero;+}++float b2Body_GetAngularVelocity( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodyState* state = b2GetBodyState( world, body );+	if ( state != NULL )+	{+		return state->angularVelocity;+	}+	return 0.0;+}++void b2Body_SetLinearVelocity( b2BodyId bodyId, b2Vec2 linearVelocity )+{+	B2_CHECK_INPUT( b2IsValidVec2( linearVelocity ) );++	b2World* world = b2GetWorld( bodyId.world0 );++	B2_REC( world, BodySetLinearVelocity, bodyId, linearVelocity );++	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( body->type == b2_staticBody )+	{+		return;+	}++	if ( b2LengthSquared( linearVelocity ) > 0.0f )+	{+		b2WakeBody( world, body );+	}++	b2BodyState* state = b2GetBodyState( world, body );+	if ( state == NULL )+	{+		return;+	}++	state->linearVelocity = linearVelocity;+}++void b2Body_SetAngularVelocity( b2BodyId bodyId, float angularVelocity )+{+	B2_CHECK_INPUT( b2IsValidFloat( angularVelocity ) );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodySetAngularVelocity, bodyId, angularVelocity );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( body->type == b2_staticBody || ( body->flags & b2_lockAngularZ ) )+	{+		return;+	}++	if ( angularVelocity != 0.0f )+	{+		b2WakeBody( world, body );+	}++	b2BodyState* state = b2GetBodyState( world, body );+	if ( state == NULL )+	{+		return;+	}++	state->angularVelocity = angularVelocity;+}++void b2Body_SetTargetTransform( b2BodyId bodyId, b2WorldTransform target, float timeStep, bool wake )+{+	B2_CHECK_INPUT( b2IsValidWorldTransform( target ) );+	B2_CHECK_INPUT( b2IsValidFloat( timeStep ) );++	b2World* world = b2GetWorld( bodyId.world0 );++	B2_REC( world, BodySetTargetTransform, bodyId, target, timeStep, wake );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( body->setIndex == b2_disabledSet )+	{+		return;+	}++	if ( body->type == b2_staticBody || timeStep <= 0.0f )+	{+		return;+	}++	if ( body->setIndex != b2_awakeSet && wake == false )+	{+		return;+	}++	b2BodySim* sim = b2GetBodySim( world, body );++	// Compute linear velocity. The center difference is taken in world precision then demoted+	b2Vec2 delta = b2SubPos( b2TransformWorldPoint( target, sim->localCenter ), sim->center );+	float invTimeStep = 1.0f / timeStep;+	b2Vec2 linearVelocity = b2MulSV( invTimeStep, delta );++	// Compute angular velocity+	b2Rot q1 = sim->transform.q;+	b2Rot q2 = target.q;+	float deltaAngle = b2RelativeAngle( q1, q2 );+	float angularVelocity = invTimeStep * deltaAngle;++	// Early out if the body is asleep already and the desired movement is small+	if ( body->setIndex != b2_awakeSet )+	{+		float maxVelocity = b2Length( linearVelocity ) + b2AbsFloat( angularVelocity ) * sim->maxExtent;++		// Return if velocity would be sleepy+		if ( maxVelocity < body->sleepThreshold )+		{+			return;+		}++		// Must wake for state to exist+		b2WakeBody( world, body );+	}++	B2_ASSERT( body->setIndex == b2_awakeSet );++	b2BodyState* state = b2GetBodyState( world, body );+	state->linearVelocity = linearVelocity;+	state->angularVelocity = angularVelocity;+}++b2Vec2 b2Body_GetLocalPointVelocity( b2BodyId bodyId, b2Vec2 localPoint )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodyState* state = b2GetBodyState( world, body );+	if ( state == NULL )+	{+		return b2Vec2_zero;+	}++	b2SolverSet* set = b2Array_Get( world->solverSets, body->setIndex );+	b2BodySim* bodySim = b2Array_Get( set->bodySims, body->localIndex );++	b2Vec2 r = b2RotateVector( bodySim->transform.q, b2Sub( localPoint, bodySim->localCenter ) );+	b2Vec2 v = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, r ) );+	return v;+}++b2Vec2 b2Body_GetWorldPointVelocity( b2BodyId bodyId, b2Pos worldPoint )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodyState* state = b2GetBodyState( world, body );+	if ( state == NULL )+	{+		return b2Vec2_zero;+	}++	b2SolverSet* set = b2Array_Get( world->solverSets, body->setIndex );+	b2BodySim* bodySim = b2Array_Get( set->bodySims, body->localIndex );++	b2Vec2 r = b2SubPos( worldPoint, bodySim->center );+	b2Vec2 v = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, r ) );+	return v;+}++void b2Body_ApplyForce( b2BodyId bodyId, b2Vec2 force, b2Pos point, bool wake )+{+	B2_CHECK_INPUT( b2IsValidVec2( force ) );+	B2_CHECK_INPUT( b2IsValidPos( point ) );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodyApplyForce, bodyId, force, point, wake );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( body->type != b2_dynamicBody || body->setIndex == b2_disabledSet )+	{+		return;+	}++	if ( wake && body->setIndex >= b2_firstSleepingSet )+	{+		b2WakeBody( world, body );+	}++	if ( body->setIndex == b2_awakeSet )+	{+		b2BodySim* bodySim = b2GetBodySim( world, body );+		bodySim->force = b2Add( bodySim->force, force );+		bodySim->torque += b2Cross( b2SubPos( point, bodySim->center ), force );+	}+}++void b2Body_ApplyForceToCenter( b2BodyId bodyId, b2Vec2 force, bool wake )+{+	B2_CHECK_INPUT( b2IsValidVec2( force ) );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodyApplyForceToCenter, bodyId, force, wake );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( body->type != b2_dynamicBody || body->setIndex == b2_disabledSet )+	{+		return;+	}++	if ( wake && body->setIndex >= b2_firstSleepingSet )+	{+		b2WakeBody( world, body );+	}++	if ( body->setIndex == b2_awakeSet )+	{+		b2BodySim* bodySim = b2GetBodySim( world, body );+		bodySim->force = b2Add( bodySim->force, force );+	}+}++void b2Body_ApplyTorque( b2BodyId bodyId, float torque, bool wake )+{+	B2_CHECK_INPUT( b2IsValidFloat( torque ) );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodyApplyTorque, bodyId, torque, wake );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( body->type != b2_dynamicBody || body->setIndex == b2_disabledSet )+	{+		return;+	}++	if ( wake && body->setIndex >= b2_firstSleepingSet )+	{+		b2WakeBody( world, body );+	}++	if ( body->setIndex == b2_awakeSet )+	{+		b2BodySim* bodySim = b2GetBodySim( world, body );+		bodySim->torque += torque;+	}+}++void b2Body_ClearForces( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodyClearForces, bodyId );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	bodySim->force = b2Vec2_zero;+	bodySim->torque = 0.0f;+}++void b2Body_ApplyLinearImpulse( b2BodyId bodyId, b2Vec2 impulse, b2Pos point, bool wake )+{+	B2_CHECK_INPUT( b2IsValidVec2( impulse ) );+	B2_CHECK_INPUT( b2IsValidPos( point ) );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodyApplyLinearImpulse, bodyId, impulse, point, wake );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( body->type != b2_dynamicBody || body->setIndex == b2_disabledSet )+	{+		return;+	}++	if ( wake && body->setIndex >= b2_firstSleepingSet )+	{+		b2WakeBody( world, body );+	}++	if ( body->setIndex == b2_awakeSet )+	{+		int localIndex = body->localIndex;+		b2SolverSet* set = b2Array_Get( world->solverSets, b2_awakeSet );+		b2BodyState* state = b2Array_Get( set->bodyStates, localIndex );+		b2BodySim* bodySim = b2Array_Get( set->bodySims, localIndex );+		state->linearVelocity = b2MulAdd( state->linearVelocity, bodySim->invMass, impulse );+		state->angularVelocity += bodySim->invInertia * b2Cross( b2SubPos( point, bodySim->center ), impulse );++		b2LimitVelocity( state, world->maxLinearSpeed );+	}+}++void b2Body_ApplyLinearImpulseToCenter( b2BodyId bodyId, b2Vec2 impulse, bool wake )+{+	B2_CHECK_INPUT( b2IsValidVec2( impulse ) );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodyApplyLinearImpulseToCenter, bodyId, impulse, wake );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( body->type != b2_dynamicBody || body->setIndex == b2_disabledSet )+	{+		return;+	}++	if ( wake && body->setIndex >= b2_firstSleepingSet )+	{+		b2WakeBody( world, body );+	}++	if ( body->setIndex == b2_awakeSet )+	{+		int localIndex = body->localIndex;+		b2SolverSet* set = b2Array_Get( world->solverSets, b2_awakeSet );+		b2BodyState* state = b2Array_Get( set->bodyStates, localIndex );+		b2BodySim* bodySim = b2Array_Get( set->bodySims, localIndex );+		state->linearVelocity = b2MulAdd( state->linearVelocity, bodySim->invMass, impulse );++		b2LimitVelocity( state, world->maxLinearSpeed );+	}+}++void b2Body_ApplyAngularImpulse( b2BodyId bodyId, float impulse, bool wake )+{+	B2_CHECK_INPUT( b2IsValidFloat( impulse ) );+	B2_ASSERT( b2Body_IsValid( bodyId ) );+	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodyApplyAngularImpulse, bodyId, impulse, wake );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( body->type != b2_dynamicBody || body->setIndex == b2_disabledSet )+	{+		return;+	}++	if ( wake && body->setIndex >= b2_firstSleepingSet )+	{+		// this will not invalidate body pointer+		b2WakeBody( world, body );+	}++	if ( body->setIndex == b2_awakeSet )+	{+		int localIndex = body->localIndex;+		b2SolverSet* set = b2Array_Get( world->solverSets, b2_awakeSet );+		b2BodyState* state = b2Array_Get( set->bodyStates, localIndex );+		b2BodySim* bodySim = b2Array_Get( set->bodySims, localIndex );+		state->angularVelocity += bodySim->invInertia * impulse;+	}+}++b2BodyType b2Body_GetType( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( 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 b2Body_SetType( b2BodyId bodyId, b2BodyType type )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodySetType, bodyId, (int32_t)type );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	b2BodyType originalType = body->type;+	if ( originalType == type )+	{+		return;+	}++	// Stage 1: skip disabled bodies+	if ( body->setIndex == b2_disabledSet )+	{+		// Disabled bodies don't change solver sets or islands when they change type.+		body->type = type;++		if ( type == b2_dynamicBody )+		{+			body->flags |= b2_dynamicFlag;+		}+		else+		{+			body->flags &= ~b2_dynamicFlag;+		}++		b2SyncBodyFlags( world, body );++		// Body type affects the mass properties+		b2UpdateBodyMassData( world, body );+		return;+	}++	// Stage 2: destroy all contacts+	b2DestroyBodyContacts( world, body );++	// Stage 3: wake this body (does nothing if body is static), otherwise it will also wake+	// all bodies in the same sleeping solver set.+	b2WakeBody( world, body );++	// Stage 4: move joints to temporary storage+	b2SolverSet* staticSet = b2Array_Get( world->solverSets, b2_staticSet );++	int jointKey = body->headJointKey;+	while ( jointKey != B2_NULL_INDEX )+	{+		int jointId = jointKey >> 1;+		int edgeIndex = jointKey & 1;++		b2Joint* joint = b2Array_Get( world->joints, jointId );+		jointKey = joint->edges[edgeIndex].nextKey;++		// Joint may be disabled by other body+		if ( joint->setIndex == b2_disabledSet )+		{+			continue;+		}++		// Wake attached bodies. The b2WakeBody call above does not wake bodies+		// attached to a static body. But it is necessary because the body may have+		// no joints.+		b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId );+		b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId );+		b2WakeBody( world, bodyA );+		b2WakeBody( world, bodyB );++		// Remove joint from island+		b2UnlinkJoint( 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.+		b2SolverSet* jointSourceSet = b2Array_Get( world->solverSets, joint->setIndex );+		b2TransferJoint( world, staticSet, jointSourceSet, joint );+	}++	// Stage 5: change the body type and transfer body+	body->type = type;++	if ( type == b2_dynamicBody )+	{+		body->flags |= b2_dynamicFlag;+	}+	else+	{+		body->flags &= ~b2_dynamicFlag;+	}++	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	b2SolverSet* sourceSet = b2Array_Get( world->solverSets, body->setIndex );+	b2SolverSet* targetSet = type == b2_staticBody ? staticSet : awakeSet;++	// Transfer body+	b2TransferBody( world, targetSet, sourceSet, body );++	// Stage 6: update island participation for the body+	if ( originalType == b2_staticBody )+	{+		// Create island for body+		b2CreateIslandForBody( world, b2_awakeSet, body );+	}+	else if ( type == b2_staticBody )+	{+		// Remove body from island.+		b2RemoveBodyFromIsland( world, body );+	}++	// Stage 7: Transfer joints to the target set+	jointKey = body->headJointKey;+	while ( jointKey != B2_NULL_INDEX )+	{+		int jointId = jointKey >> 1;+		int edgeIndex = jointKey & 1;++		b2Joint* joint = b2Array_Get( world->joints, jointId );++		jointKey = joint->edges[edgeIndex].nextKey;++		// Joint may be disabled by other body+		if ( joint->setIndex == b2_disabledSet )+		{+			continue;+		}++		// All joints were transferred to the static set in an earlier stage+		B2_ASSERT( joint->setIndex == b2_staticSet );++		b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId );+		b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId );+		B2_ASSERT( bodyA->setIndex == b2_staticSet || bodyA->setIndex == b2_awakeSet );+		B2_ASSERT( bodyB->setIndex == b2_staticSet || bodyB->setIndex == b2_awakeSet );++		if ( bodyA->type == b2_dynamicBody || bodyB->type == b2_dynamicBody )+		{+			b2TransferJoint( world, awakeSet, staticSet, joint );+		}+	}++	// Recreate shape proxies in broadphase+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );+	int shapeId = body->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		shapeId = shape->nextShapeId;+		b2DestroyShapeProxy( shape, &world->broadPhase );+		bool forcePairCreation = true;+		b2CreateShapeProxy( world, shape, type, transform, forcePairCreation );+	}++	// Relink all joints+	jointKey = body->headJointKey;+	while ( jointKey != B2_NULL_INDEX )+	{+		int jointId = jointKey >> 1;+		int edgeIndex = jointKey & 1;++		b2Joint* joint = b2Array_Get( world->joints, jointId );+		jointKey = joint->edges[edgeIndex].nextKey;++		int otherEdgeIndex = edgeIndex ^ 1;+		int otherBodyId = joint->edges[otherEdgeIndex].bodyId;+		b2Body* otherBody = b2Array_Get( world->bodies, otherBodyId );++		if ( otherBody->setIndex == b2_disabledSet )+		{+			continue;+		}++		if ( body->type != b2_dynamicBody && otherBody->type != b2_dynamicBody )+		{+			continue;+		}++		b2LinkJoint( world, joint );+	}++	b2SyncBodyFlags( world, body );++	// Body type affects the mass+	b2UpdateBodyMassData( world, body );++	b2ValidateSolverSets( world );+	b2ValidateIsland( world, body->islandId );+}++void b2Body_SetName( b2BodyId bodyId, const char* name )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodySetName, bodyId, name );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( name )+	{+#if defined( _MSC_VER )+		strncpy_s( body->name, B2_NAME_LENGTH + 1, name, B2_NAME_LENGTH );+#else+		strncpy( body->name, name, B2_NAME_LENGTH );+		body->name[B2_NAME_LENGTH] = 0;+#endif+	}+	else+	{+		memset( body->name, 0, sizeof( body->name ) );+	}+}++const char* b2Body_GetName( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->name;+}++void b2Body_SetUserData( b2BodyId bodyId, void* userData )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	body->userData = userData;+}++void* b2Body_GetUserData( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->userData;+}++float b2Body_GetMass( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->mass;+}++float b2Body_GetRotationalInertia( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->inertia;+}++b2Vec2 b2Body_GetLocalCenter( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	return bodySim->localCenter;+}++b2Pos b2Body_GetWorldCenter( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	return bodySim->center;+}++void b2Body_SetMassData( b2BodyId bodyId, b2MassData massData )+{+	B2_CHECK_INPUT( b2IsValidFloat( massData.mass ) && massData.mass >= 0.0f );+	B2_CHECK_INPUT( b2IsValidFloat( massData.rotationalInertia ) && massData.rotationalInertia >= 0.0f );+	B2_CHECK_INPUT( b2IsValidVec2( massData.center ) );++	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodySetMassData, bodyId, massData );++	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );++	body->mass = massData.mass;+	body->inertia = massData.rotationalInertia;+	bodySim->localCenter = massData.center;++	b2Pos oldCenter = bodySim->center;+	b2Pos center = b2TransformWorldPoint( bodySim->transform, massData.center );+	bodySim->center = center;+	bodySim->center0 = center;++	// Update center of mass velocity+	b2BodyState* state = b2GetBodyState( world, body );+	if ( state != NULL )+	{+		b2Vec2 deltaLinear = b2CrossSV( state->angularVelocity, b2SubPos( bodySim->center, oldCenter ) );+		state->linearVelocity = b2Add( state->linearVelocity, deltaLinear );+	}++	bodySim->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f;+	bodySim->invInertia = body->inertia > 0.0f ? 1.0f / body->inertia : 0.0f;++	// Update extents using supplied mass center.+	bodySim->minExtent = B2_HUGE;+	bodySim->maxExtent = 0.0f;+	int shapeId = body->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		const b2Shape* s = b2Array_Get( world->shapes, shapeId );+		b2ShapeExtent extent = b2ComputeShapeExtent( s, massData.center );+		bodySim->minExtent = b2MinFloat( bodySim->minExtent, extent.minExtent );+		bodySim->maxExtent = b2MaxFloat( bodySim->maxExtent, extent.maxExtent );+		shapeId = s->nextShapeId;+	}++	// When the center of mass changes, any cached contact manifold becomes invalid.+	int edgeKey = body->headContactKey;+	while ( edgeKey != B2_NULL_INDEX )+	{+		int contactId = edgeKey >> 1;+		int edgeIndex = edgeKey & 1;++		b2Contact* contact = b2Array_Get( world->contacts, contactId );+		b2ContactSim* contactSim = b2GetContactSim( world, contact );+		contactSim->simFlags &= ~b2_simRelativeTransformValid;++		edgeKey = contact->edges[edgeIndex].nextKey;+	}++	// Motion locks take priority over mass data.+	if ( body->flags & b2_fixedRotation )+	{+		body->inertia = 0.0f;+		bodySim->invInertia = 0.0f;+	}++	body->flags &= ~b2_dirtyMass;+}++b2MassData b2Body_GetMassData( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	b2MassData massData = { body->mass, bodySim->localCenter, body->inertia };+	return massData;+}++void b2Body_UpdateMassFromShapes( b2BodyId bodyId )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodyUpdateMassFromShapes, bodyId );++	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2UpdateBodyMassData( world, body );+}++void b2Body_SetLinearDamping( b2BodyId bodyId, float linearDamping )+{+	B2_CHECK_INPUT( b2IsValidFloat( linearDamping ) && linearDamping >= 0.0f );++	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodySetLinearDamping, bodyId, linearDamping );++	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	bodySim->linearDamping = linearDamping;+}++float b2Body_GetLinearDamping( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	return bodySim->linearDamping;+}++void b2Body_SetAngularDamping( b2BodyId bodyId, float angularDamping )+{+	B2_CHECK_INPUT( b2IsValidFloat( angularDamping ) && angularDamping >= 0.0f );++	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodySetAngularDamping, bodyId, angularDamping );++	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	bodySim->angularDamping = angularDamping;+}++float b2Body_GetAngularDamping( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	return bodySim->angularDamping;+}++void b2Body_SetGravityScale( b2BodyId bodyId, float gravityScale )+{+	B2_ASSERT( b2Body_IsValid( bodyId ) );+	B2_CHECK_INPUT( b2IsValidFloat( gravityScale ) );++	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodySetGravityScale, bodyId, gravityScale );++	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	bodySim->gravityScale = gravityScale;+}++float b2Body_GetGravityScale( b2BodyId bodyId )+{+	B2_ASSERT( b2Body_IsValid( bodyId ) );+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	return bodySim->gravityScale;+}++bool b2Body_IsAwake( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->setIndex == b2_awakeSet;+}++void b2Body_SetAwake( b2BodyId bodyId, bool awake )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodySetAwake, bodyId, awake );++	b2Body* body = b2GetBodyFullId( world, bodyId );++	if ( awake && body->setIndex >= b2_firstSleepingSet )+	{+		b2WakeBody( world, body );+	}+	else if ( awake == false && body->setIndex == b2_awakeSet )+	{+		b2Island* island = b2Array_Get( world->islands, body->islandId );+		if ( island->constraintRemoveCount > 0 )+		{+			// Must split the island before sleeping. This is expensive.+			b2SplitIsland( world, body->islandId );+		}++		b2TrySleepIsland( world, body->islandId );+	}+}++void b2Body_WakeTouching( b2BodyId bodyId )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodyWakeTouching, bodyId );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	int contactKey = body->headContactKey;+	while ( contactKey != B2_NULL_INDEX )+	{+		int contactId = contactKey >> 1;+		int edgeIndex = contactKey & 1;++		b2Contact* contact = b2Array_Get( world->contacts, contactId );+		b2Shape* shapeA = b2Array_Get( world->shapes, contact->shapeIdA );+		b2Shape* shapeB = b2Array_Get( world->shapes, contact->shapeIdB );++		if ( shapeA->bodyId == bodyId.index1 - 1 )+		{+			b2Body* otherBody = b2Array_Get( world->bodies, shapeB->bodyId );+			b2WakeBody( world, otherBody );+		}+		else+		{+			b2Body* otherBody = b2Array_Get( world->bodies, shapeA->bodyId );+			b2WakeBody( world, otherBody );+		}++		contactKey = contact->edges[edgeIndex].nextKey;+	}+}++bool b2Body_IsEnabled( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->setIndex != b2_disabledSet;+}++bool b2Body_IsSleepingEnabled( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return ( body->flags & b2_enableSleep ) == b2_enableSleep;+}++void b2Body_SetSleepThreshold( b2BodyId bodyId, float sleepThreshold )+{+	B2_CHECK_INPUT( b2IsValidFloat( sleepThreshold ) && sleepThreshold >= 0.0f );++	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodySetSleepThreshold, bodyId, sleepThreshold );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	body->sleepThreshold = sleepThreshold;+}++float b2Body_GetSleepThreshold( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->sleepThreshold;+}++void b2Body_SetSafetyFactor( b2BodyId bodyId, float safetyFactor )+{+	B2_CHECK_INPUT( b2IsValidFloat( safetyFactor ) && safetyFactor >= 0.0f );+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodySetSafetyFactor, bodyId, safetyFactor );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	body->safetyFactor = safetyFactor;+}++float b2Body_GetSafetyFactor( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->safetyFactor;+}++void b2Body_EnableSleeping( b2BodyId bodyId, bool enableSleep )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodyEnableSleeping, bodyId, enableSleep );++	b2Body* body = b2GetBodyFullId( world, bodyId );++	bool flag = ( body->flags & b2_enableSleep ) == b2_enableSleep;+	if ( enableSleep == flag )+	{+		return;+	}++	body->flags = enableSleep ? body->flags | b2_enableSleep : body->flags & ~b2_enableSleep;+	b2SyncBodyFlags( world, body );++	if ( enableSleep == false )+	{+		b2WakeBody( world, body );+	}+}++// Disabling a body requires a lot of detailed bookkeeping, but it is a valuable feature.+// The most challenging aspect is that joints may connect to bodies that are not disabled.+void b2Body_Disable( b2BodyId bodyId )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodyDisable, bodyId );++	b2Body* body = b2GetBodyFullId( world, bodyId );+	if ( body->setIndex == b2_disabledSet )+	{+		return;+	}++	// Destroy contacts and wake bodies touching this body. This avoid floating bodies.+	// This is necessary even for static bodies.+	b2DestroyBodyContacts( world, body );++	// The current solver set of the body+	b2SolverSet* set = b2Array_Get( world->solverSets, body->setIndex );++	// Disabled bodies and connected joints are moved to the disabled set+	b2SolverSet* disabledSet = b2Array_Get( world->solverSets, b2_disabledSet );++	// Unlink joints and transfer them to the disabled set+	int jointKey = body->headJointKey;+	while ( jointKey != B2_NULL_INDEX )+	{+		int jointId = jointKey >> 1;+		int edgeIndex = jointKey & 1;++		b2Joint* joint = b2Array_Get( world->joints, jointId );+		jointKey = joint->edges[edgeIndex].nextKey;++		// joint may already be disabled by other body+		if ( joint->setIndex == b2_disabledSet )+		{+			continue;+		}++		B2_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b2_staticSet || joint->setIndex == b2_staticSet );++		// Remove joint from island+		b2UnlinkJoint( world, joint );++		// Transfer joint to disabled set+		b2SolverSet* jointSet = b2Array_Get( world->solverSets, joint->setIndex );+		b2TransferJoint( world, disabledSet, jointSet, joint );+	}++	// Remove shapes from broad-phase+	int shapeId = body->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		shapeId = shape->nextShapeId;+		b2DestroyShapeProxy( shape, &world->broadPhase );+	}++	// Disabled bodies are not in an island. If the island becomes empty it will be destroyed.+	b2RemoveBodyFromIsland( world, body );++	// Transfer body sim+	b2TransferBody( world, disabledSet, set, body );++	b2ValidateConnectivity( world );+	b2ValidateSolverSets( world );+}++void b2Body_Enable( b2BodyId bodyId )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodyEnable, bodyId );++	b2Body* body = b2GetBodyFullId( world, bodyId );+	if ( body->setIndex != b2_disabledSet )+	{+		return;+	}++	b2SolverSet* disabledSet = b2Array_Get( world->solverSets, b2_disabledSet );+	int setId = body->type == b2_staticBody ? b2_staticSet : b2_awakeSet;+	b2SolverSet* targetSet = b2Array_Get( world->solverSets, setId );++	b2TransferBody( world, targetSet, disabledSet, body );++	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );++	// Add shapes to broad-phase+	b2BodyType proxyType = body->type;+	bool forcePairCreation = true;+	int shapeId = body->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		shapeId = shape->nextShapeId;++		b2CreateShapeProxy( world, shape, proxyType, transform, forcePairCreation );+	}++	if ( setId != b2_staticSet )+	{+		b2CreateIslandForBody( 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 != B2_NULL_INDEX )+	{+		int jointId = jointKey >> 1;+		int edgeIndex = jointKey & 1;++		b2Joint* joint = b2Array_Get( world->joints, jointId );+		B2_ASSERT( joint->setIndex == b2_disabledSet );+		B2_ASSERT( joint->islandId == B2_NULL_INDEX );++		jointKey = joint->edges[edgeIndex].nextKey;++		b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId );+		b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId );++		if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet )+		{+			// one body is still disabled+			continue;+		}++		// Transfer joint first+		int jointSetId;+		if ( bodyA->type != b2_dynamicBody && bodyB->type != b2_dynamicBody )+		{+			jointSetId = b2_staticSet;+		}+		else if ( bodyA->setIndex == b2_staticSet )+		{+			jointSetId = bodyB->setIndex;+		}+		else+		{+			jointSetId = bodyA->setIndex;+		}++		b2SolverSet* jointSet = b2Array_Get( world->solverSets, jointSetId );+		b2TransferJoint( world, jointSet, disabledSet, joint );++		// Now that the joint is in the correct set, I can link the joint in the island.+		if ( jointSetId != b2_staticSet )+		{+			b2LinkJoint( world, joint );+		}+	}++	b2ValidateSolverSets( world );+}++void b2Body_SetMotionLocks( b2BodyId bodyId, b2MotionLocks locks )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodySetMotionLocks, bodyId, locks );++	uint32_t newFlags = 0;+	newFlags |= locks.linearX ? b2_lockLinearX : 0;+	newFlags |= locks.linearY ? b2_lockLinearY : 0;+	newFlags |= locks.angularZ ? b2_lockAngularZ : 0;++	b2Body* body = b2GetBodyFullId( world, bodyId );+	if ( ( body->flags & b2_allLocks ) != newFlags )+	{+		body->flags &= ~b2_allLocks;+		body->flags |= newFlags;++		b2SyncBodyFlags( world, body );++		b2BodyState* state = b2GetBodyState( world, body );++		if ( state != NULL )+		{+			if ( locks.linearX )+			{+				state->linearVelocity.x = 0.0f;+			}++			if ( locks.linearY )+			{+				state->linearVelocity.y = 0.0f;+			}++			if ( locks.angularZ )+			{+				state->angularVelocity = 0.0f;+			}+		}++		// Motion locks can affect mass properties.+		b2UpdateBodyMassData( world, body );+	}+}++b2MotionLocks b2Body_GetMotionLocks( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );++	b2MotionLocks locks;+	locks.linearX = ( body->flags & b2_lockLinearX );+	locks.linearY = ( body->flags & b2_lockLinearY );+	locks.angularZ = ( body->flags & b2_lockAngularZ );+	return locks;+}++void b2Body_SetBullet( b2BodyId bodyId, bool flag )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodySetBullet, bodyId, flag );++	uint32_t newFlag = flag ? b2_isBullet : 0;++	b2Body* body = b2GetBodyFullId( world, bodyId );+	if ( ( body->flags & b2_isBullet ) == newFlag )+	{+		return;+	}++	body->flags &= ~b2_isBullet;+	body->flags |= newFlag;++	b2SyncBodyFlags( world, body );+}++bool b2Body_IsBullet( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	return ( bodySim->flags & b2_isBullet ) != 0;+}++void b2Body_EnableContactRecycling( b2BodyId bodyId, bool flag )+{+	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, BodyEnableContactRecycling, bodyId, flag );++	uint32_t newFlag = flag ? b2_bodyEnableContactRecycling : 0;++	b2Body* body = b2GetBodyFullId( world, bodyId );+	if ( ( body->flags & b2_bodyEnableContactRecycling ) == newFlag )+	{+		return;+	}++	body->flags &= ~b2_bodyEnableContactRecycling;+	body->flags |= newFlag;++	b2SyncBodyFlags( world, body );+}++bool b2Body_IsContactRecyclingEnabled( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return ( body->flags & b2_bodyEnableContactRecycling ) != 0;+}++void b2Body_EnableContactEvents( b2BodyId bodyId, bool flag )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodyEnableContactEvents, bodyId, flag );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	int shapeId = body->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		shape->enableContactEvents = flag;+		shapeId = shape->nextShapeId;+	}+}++void b2Body_EnableHitEvents( b2BodyId bodyId, bool flag )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC( world, BodyEnableHitEvents, bodyId, flag );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	int shapeId = body->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		shape->enableHitEvents = flag;+		shapeId = shape->nextShapeId;+	}+}++b2WorldId b2Body_GetWorld( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	return (b2WorldId){ bodyId.world0 + 1, world->generation };+}++int b2Body_GetShapeCount( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->shapeCount;+}++int b2Body_GetShapes( b2BodyId bodyId, b2ShapeId* shapeArray, int capacity )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	int shapeId = body->headShapeId;+	int shapeCount = 0;+	while ( shapeId != B2_NULL_INDEX && shapeCount < capacity )+	{+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		b2ShapeId id = { shape->id + 1, bodyId.world0, shape->generation };+		shapeArray[shapeCount] = id;+		shapeCount += 1;++		shapeId = shape->nextShapeId;+	}++	return shapeCount;+}++int b2Body_GetJointCount( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	return body->jointCount;+}++int b2Body_GetJoints( b2BodyId bodyId, b2JointId* jointArray, int capacity )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	int jointKey = body->headJointKey;++	int jointCount = 0;+	while ( jointKey != B2_NULL_INDEX && jointCount < capacity )+	{+		int jointId = jointKey >> 1;+		int edgeIndex = jointKey & 1;++		b2Joint* joint = b2Array_Get( world->joints, jointId );++		b2JointId id = { jointId + 1, bodyId.world0, joint->generation };+		jointArray[jointCount] = id;+		jointCount += 1;++		jointKey = joint->edges[edgeIndex].nextKey;+	}++	return jointCount;+}++bool b2ShouldBodiesCollide( b2World* world, b2Body* bodyA, b2Body* bodyB )+{+	if ( bodyA->type != b2_dynamicBody && bodyB->type != b2_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 != B2_NULL_INDEX )+	{+		int jointId = jointKey >> 1;+		int edgeIndex = jointKey & 1;+		int otherEdgeIndex = edgeIndex ^ 1;++		b2Joint* joint = b2Array_Get( world->joints, jointId );+		if ( joint->collideConnected == false && joint->edges[otherEdgeIndex].bodyId == otherBodyId )+		{+			return false;+		}++		jointKey = joint->edges[edgeIndex].nextKey;+	}++	return true;+}++float b2Body_GetMinExtent( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	return bodySim->minExtent;+}++float b2Body_GetMaxExtent( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	return bodySim->maxExtent;+}++float b2Body_GetMaxExtentFromOrigin( b2BodyId bodyId )+{+	b2World* world = b2GetWorld( bodyId.world0 );+	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );+	return bodySim->maxExtent + b2Length( bodySim->localCenter );+}
+ upstream/src/body.h view
@@ -0,0 +1,295 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"+#include "physics_world.h"+#include "solver_set.h"++#include "box2d/constants.h"+#include "box2d/math_functions.h"+#include "box2d/types.h"++enum b2BodyFlags+{+	// This body has fixed translation along the x-axis+	b2_lockLinearX = 0x00000001,++	// This body has fixed translation along the y-axis+	b2_lockLinearY = 0x00000002,++	// This body has fixed rotation+	b2_lockAngularZ = 0x00000004,++	// This flag is used for debug draw and contact recycling. Only on b2BodySim.+	b2_isFast = 0x00000008,++	// This dynamic body does a final CCD pass against all body types, but not other bullets+	b2_isBullet = 0x00000010,++	// This body was speed capped in the current time step+	b2_isSpeedCapped = 0x00000020,++	// This body had a time of impact event in the current time step (body sim only).+	b2_hadTimeOfImpact = 0x00000040,++	// This body has no limit on angular velocity+	b2_allowFastRotation = 0x00000080,++	// This bullet body needs to have its AABB increased. Needed because bullets don't follow+	// the standard broad-phase update.+	b2_enlargeBulletBounds = 0x00000100,++	// 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 b2BodyState flags.+	b2_dynamicFlag = 0x00000200,++	// Flag to indicate the user has used the updateBodyMass option to defer mass+	// computation but b2Body_UpdateMassFromShapes was not called before the world step.+	b2_dirtyMass = 0x00000400,++	b2_enableSleep = 0x00000800,++	b2_bodyEnableContactRecycling = 0x00001000,++	// All lock flags+	b2_allLocks = b2_lockAngularZ | b2_lockLinearX | b2_lockLinearY,++	// If this flag is set then the body has fixed rotation+	b2_fixedRotation = b2_lockAngularZ,++	// These flags are transient per time step. These may be different across b2Body, b2BodySim, and b2BodyState.+	b2_bodyTransientFlags = b2_isSpeedCapped | b2_hadTimeOfImpact,+};++// Body organizational details that are not used in the solver.+typedef struct b2Body+{+	void* userData;++	// index of solver set stored in b2World+	// may be B2_NULL_INDEX+	int setIndex;++	// body sim and state index within set+	// may be B2_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;++	// Need this island index for faster union-find+	int islandIndex;++	float mass;++	// Rotational inertia about the center of mass.+	float inertia;++	float sleepThreshold;+	float sleepTime;+	float safetyFactor;++	// this is used to adjust the fellAsleep flag in the body move array+	int bodyMoveIndex;++	int id;++	// b2BodyFlags+	uint32_t flags;++	b2BodyType type;++	// This is monotonically advanced when a body is allocated in this slot+	// Used to check for invalid b2BodyId+	uint16_t generation;++	char name[B2_NAME_LENGTH + 1];+} b2Body;++// 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.++// 32 bytes+typedef struct b2BodyState+{+	b2Vec2 linearVelocity; // 8+	float angularVelocity; // 4++	// b2BodyFlags+	// Important flags: locking, dynamic+	uint32_t flags; // 4++	// Using delta position reduces round-off error far from the origin+	b2Vec2 deltaPosition; // 8++	// 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)+	b2Rot deltaRotation; // 8+} b2BodyState;++// Identity body state, notice the deltaRotation is {1, 0}+static const b2BodyState b2_identityBodyState = { { 0.0f, 0.0f }, 0.0f, 0, { 0.0f, 0.0f }, { 1.0f, 0.0f } };++// Body simulation data used for integration of position and velocity+// Transform data used for collision and solver preparation.+typedef struct b2BodySim+{+	// transform for body origin, double translation in large world mode+	b2WorldTransform transform;++	// center of mass position in world space+	b2Pos center;++	// previous rotation and COM for TOI+	b2Rot rotation0;+	b2Pos center0;++	// location of center of mass relative to the body origin+	b2Vec2 localCenter;++	b2Vec2 force;+	float torque;++	// inverse inertia+	float invMass;+	float invInertia;++	float minExtent;+	float maxExtent;+	float linearDamping;+	float angularDamping;+	float gravityScale;++	// Index of b2Body+	int bodyId;++	// b2BodyFlags+	uint32_t flags;+} b2BodySim;++// Get a validated body from a world using an id.+b2Body* b2GetBodyFullId( b2World* world, b2BodyId bodyId );++b2WorldTransform b2GetBodyTransformQuick( b2World* world, b2Body* body );+b2WorldTransform b2GetBodyTransform( b2World* world, int bodyId );++// Create a b2BodyId from a raw id.+b2BodyId b2MakeBodyId( b2World* world, int bodyId );++bool b2ShouldBodiesCollide( b2World* world, b2Body* bodyA, b2Body* bodyB );++void b2RefreshBodyContactIndices( b2World* world, b2Body* body );++// Careful calling this because it can invalidate body, state, joint, and contact pointers.+bool b2WakeBody( b2World* world, b2Body* body );++void b2UpdateBodyMassData( b2World* world, b2Body* body );+void b2SyncBodyFlags( b2World* world, b2Body* body );++// Build a sweep relative to a base position so continuous collision keeps float precision far+// from the origin. The base cancels out of the relative motion the TOI actually solves.+static inline b2Sweep b2MakeRelativeSweep( const b2BodySim* bodySim, b2Pos base )+{+	b2Sweep s;+	s.c1 = b2SubPos( bodySim->center0, base );+	s.c2 = b2SubPos( bodySim->center, base );+	s.q1 = bodySim->rotation0;+	s.q2 = bodySim->transform.q;+	s.localCenter = bodySim->localCenter;+	return s;+}++static inline b2BodySim* b2GetBodySim( b2World* world, b2Body* body )+{+	b2SolverSet* set = b2Array_Get( world->solverSets, body->setIndex );+	b2BodySim* bodySim = b2Array_Get( set->bodySims, body->localIndex );+	return bodySim;+}++static inline b2BodyState* b2GetBodyState( b2World* world, b2Body* body )+{+	if ( body->setIndex == b2_awakeSet )+	{+		b2SolverSet* set = b2Array_Get( world->solverSets, b2_awakeSet );+		return b2Array_Get( set->bodyStates, body->localIndex );+	}++	return NULL;+}++// This encodes the body sim index for storage in the contact sim. This helps+// avoid a lookup and cache miss in the narrow phase.+static inline int b2EncodeBodySimIndex( const b2Body* body )+{+	if ( body->setIndex == b2_awakeSet )+	{+		return body->localIndex;+	}++	if ( body->setIndex == b2_staticSet )+	{+		return -( body->localIndex + 2 );+	}++	return B2_NULL_INDEX;+}++static inline bool b2IsStaticSimIndex( int encodedBodySimIndex )+{+	return encodedBodySimIndex < B2_NULL_INDEX;+}++// In the contact solver a static body sim is expected to have null index.+static inline int b2DecodeAwakeIndex( int encodedBodySimIndex )+{+	return encodedBodySimIndex >= 0 ? encodedBodySimIndex : B2_NULL_INDEX;+}++// When a body goes to sleep the contact sim has a null index for it. This+// also handles the case where the body is static and has an encoded+// sim index of -2 or less.+static inline int b2SleepBodySimIndex( int encodedIndex )+{+	return encodedIndex >= 0 ? B2_NULL_INDEX : encodedIndex;+}
+ upstream/src/broad_phase.c view
@@ -0,0 +1,721 @@+// 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 "broad_phase.h"++#include "aabb.h"+#include "arena_allocator.h"+#include "body.h"+#include "contact.h"+#include "core.h"+#include "dynamic_tree.h"+#include "parallel_for.h"+#include "physics_world.h"+#include "platform.h"+#include "shape.h"+#include "simd.h"++#include <stdbool.h>+#include <string.h>++void b2CreateBroadPhase( b2BroadPhase* bp, const b2Capacity* capacity )+{+	_Static_assert( b2_bodyTypeCount == 3, "must be three body types" );++	bp->pairSet = b2CreateSet( b2MaxInt( 32, 2 * capacity->contactCount ) );++	int staticCapacity = b2MaxInt( 16, capacity->staticShapeCount );+	b2CreateDynamicTreeInternal( bp->trees + b2_staticBody, staticCapacity );++	int kinematicCapacity = 16;+	b2CreateDynamicTreeInternal( bp->trees + b2_kinematicBody, kinematicCapacity );++	int dynamicCapacity = b2MaxInt( 16, capacity->dynamicShapeCount );+	b2CreateDynamicTreeInternal( bp->trees + b2_dynamicBody, dynamicCapacity );++	bp->movedSiblings = NULL;+}++void b2DestroyBroadPhase( b2BroadPhase* bp )+{+	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2DestroyDynamicTreeInternal( bp->trees + i );+	}++	b2DestroySet( &bp->pairSet );++	memset( bp, 0, sizeof( b2BroadPhase ) );+}++int b2BroadPhase_CreateProxy( b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint64_t categoryBits, int shapeIndex,+							  bool forcePairCreation )+{+	B2_ASSERT( 0 <= proxyType && proxyType < b2_bodyTypeCount );++	bool mark = ( proxyType != b2_staticBody || forcePairCreation );++	int proxyId = b2CreateTreeProxyInternal( bp->trees + proxyType, aabb, categoryBits, shapeIndex, mark );+	int proxyKey = B2_PROXY_KEY( proxyId, proxyType );+	return proxyKey;+}++void b2BroadPhase_DestroyProxy( b2BroadPhase* bp, int proxyKey )+{+	b2BodyType proxyType = B2_PROXY_TYPE( proxyKey );+	int proxyId = B2_PROXY_ID( proxyKey );++	B2_ASSERT( 0 <= proxyType && proxyType < b2_bodyTypeCount );+	b2DestroyTreeProxy( bp->trees + proxyType, proxyId );+}++void b2BroadPhase_MoveProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb )+{+	b2BodyType proxyType = B2_PROXY_TYPE( proxyKey );+	int proxyId = B2_PROXY_ID( proxyKey );++	bool mark = true;+	b2DynamicTree_MoveProxyInternal( bp->trees + proxyType, proxyId, aabb, mark );+}++// Gather the sibling pairs with a moved node. This is done serially it is cache friendly.+static int b2GatherMovedSiblings( const b2DynamicTree* tree, int* pairIndices )+{+	const b2TreeNode* nodes = tree->nodes;+	int nodeEnd = tree->nodeEnd;++	int count = 0;++	// Skip the root.+	for ( int pair = 2; pair < nodeEnd; pair += 2 )+	{+		// Push when either sibling moved.+		if ( ( nodes[pair].flagIndex | nodes[pair + 1].flagIndex ) & B2_MOVED_NODE )+		{+			pairIndices[count++] = pair;+		}+	}++	return count;+}++#define B2_CANDIDATE_BATCH 32++typedef struct b2Candidate+{+	int shapeIdA;+	int shapeIdB;+} b2CandidatePair;++typedef struct b2PairContext+{+	b2World* world;+	b2Array( uint64_t ) * pairKeys;+	b2CandidatePair batch[B2_CANDIDATE_BATCH];+	int batchCount;+} b2PairContext;++typedef struct b2NodePair+{+	b2TreeNode a;+	b2TreeNode b;+} b2NodePair;++typedef struct b2IndexPair+{+	int a, b;+} b2IndexPair;++static inline void b2PrefetchHash( b2HashSet* set, uint64_t hash )+{+	uint32_t capacity = set->capacity;+	uint32_t index = (uint32_t)hash & ( capacity - 1 );+	b2Prefetch( set->items + index );+}++static void b2FlushCandidatePairs( b2PairContext* context )+{+	b2World* world = context->world;+	b2BroadPhase* bp = &world->broadPhase;++	int count1 = context->batchCount;+	context->batchCount = 0;++	// Prefetch hash set entries. Less than 1% gain from this but it might+	// matter more for Box3D. Also, if I switch to verstable then this might+	// not be possible.+	uint64_t keys[B2_CANDIDATE_BATCH];+	uint64_t hashes[B2_CANDIDATE_BATCH];+	for ( int i = 0; i < count1; ++i )+	{+		b2CandidatePair* candidate = context->batch + i;+		keys[i] = B2_SHAPE_PAIR_KEY( candidate->shapeIdA, candidate->shapeIdB );+		hashes[i] = b2KeyHash( keys[i] );+		b2PrefetchHash( &bp->pairSet, hashes[i] );+	}++	// Cull existing pairs.+	b2CandidatePair candidates[B2_CANDIDATE_BATCH];+	int count2 = 0;+	for ( int i = 0; i < count1; ++i )+	{+		bool pairExists = b2ContainsHashedKey( &bp->pairSet, keys[i], hashes[i] );+		if ( pairExists == false )+		{+			candidates[count2] = context->batch[i];+			count2 += 1;+		}+	}++	// Prefetch shapes. Again, less than a 1% gain from this. Might matter more for Box3D.+	const b2Shape* shapes = world->shapes.data;+	for ( int i = 0; i < count2; ++i )+	{+		b2Prefetch( shapes + candidates[i].shapeIdA );+		b2Prefetch( shapes + candidates[i].shapeIdB );+	}++	// Filter candidates.+	b2Array( uint64_t )* pairKeys = context->pairKeys;+	for ( int i = 0; i < count2; ++i )+	{+		int shapeIdA = candidates[i].shapeIdA;+		int shapeIdB = candidates[i].shapeIdB;++		b2Shape* shapeA = b2Array_Get( world->shapes, shapeIdA );+		b2Shape* shapeB = b2Array_Get( world->shapes, shapeIdB );++		int bodyIdA = shapeA->bodyId;+		int bodyIdB = shapeB->bodyId;++		// Are the shapes on the same body?+		if ( bodyIdA == bodyIdB )+		{+			continue;+		}++		// Sensors are handled elsewhere+		if ( shapeA->sensorIndex != B2_NULL_INDEX || shapeB->sensorIndex != B2_NULL_INDEX )+		{+			continue;+		}++		if ( b2ShouldShapesCollide( shapeA->filter, shapeB->filter ) == false )+		{+			continue;+		}++		if ( b2CanCollide( shapeA->type, shapeB->type ) == false )+		{+			// For example, no segment vs segment collision+			continue;+		}++		// Does a joint override collision?+		b2Body* bodyA = b2Array_Get( world->bodies, bodyIdA );+		b2Body* bodyB = b2Array_Get( world->bodies, bodyIdB );+		if ( b2ShouldBodiesCollide( world, bodyA, bodyB ) == false )+		{+			continue;+		}++		// Custom user filter+		if ( shapeA->enableCustomFiltering || shapeB->enableCustomFiltering )+		{+			b2CustomFilterCallbackFcn* customFilterFcn = world->customFilterFcn;+			if ( customFilterFcn != NULL )+			{+				b2ShapeId idA = { shapeIdA + 1, world->worldId, shapeA->generation };+				b2ShapeId idB = { shapeIdB + 1, world->worldId, shapeB->generation };+				bool shouldCollide = customFilterFcn( idA, idB, world->customFilterContext );+				if ( shouldCollide == false )+				{+					continue;+				}+			}+		}++		// The pair passed the gauntlet. A new contact will be created.+		b2Array_Push( *pairKeys, B2_SHAPE_PAIR_KEY( shapeIdA, shapeIdB ) );+	}+}++B2_FORCE_INLINE void b2AddCandidatePair( int shapeIdA, int shapeIdB, b2PairContext* context )+{+	// Follow shape index order.+	b2CandidatePair* candidate = context->batch + context->batchCount;+	candidate->shapeIdA = b2MinInt( shapeIdA, shapeIdB );+	candidate->shapeIdB = b2MaxInt( shapeIdA, shapeIdB );+	context->batchCount += 1;+	if ( context->batchCount == B2_CANDIDATE_BATCH )+	{+		b2FlushCandidatePairs( context );+	}+}++// Did either move and if so do they overlap?+B2_FORCE_INLINE bool b2TestPair( const b2TreeNode* a, const b2TreeNode* b )+{+	if ( ( ( a->flagIndex | b->flagIndex ) & B2_MOVED_NODE ) == 0 )+	{+		return false;+	}++	return b2AABB_Overlaps( a->aabb, b->aabb );+}++static void b2CollideProxyAndSubtree( const b2TreeNode* proxy, const b2TreeNode* nodes, int pair, b2PairContext* context )+{+	uint32_t proxyMark = proxy->flagIndex & B2_MOVED_NODE;+	b2AABBV boxv = b2LoadAABBV( &proxy->aabb );+	int shapeId = proxy->shapeIndex;++	int stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;+	stack[stackCount++] = pair;++	while ( stackCount > 0 )+	{+		pair = stack[--stackCount];+		const b2TreeNode* children = nodes + pair;++		int movedMask = 3;+		if ( proxyMark == 0 )+		{+			movedMask = ( b2IsNodeMoved( children + 0 ) ? 1 : 0 ) | ( b2IsNodeMoved( children + 1 ) ? 2 : 0 );+		}++		int mask = b2OverlapNodePairMask( boxv, children ) & movedMask;+		while ( mask != 0 )+		{+			int i = (int)b2CTZ32( (uint32_t)mask );+			mask &= mask - 1;++			const b2TreeNode* node = children + i;+			if ( b2IsLeaf( node ) )+			{+				b2AddCandidatePair( shapeId, node->shapeIndex, context );+			}+			else+			{+				if ( stackCount < B2_TREE_STACK_SIZE )+				{+					stack[stackCount++] = b2GetLeftChild( node );+				}+				else+				{+					B2_ASSERT( stackCount < B2_TREE_STACK_SIZE );+				}+			}+		}+	}+}++B2_FORCE_INLINE void b2ExpandPair( const b2TreeNode* arrayA, const b2TreeNode* arrayB, const b2TreeNode* nodeA,+								   const b2TreeNode* nodeB, b2IndexPair* stack, int* stackCount, b2PairContext* context )+{+	bool leafA = b2IsLeaf( nodeA );+	bool leafB = b2IsLeaf( nodeB );+	if ( leafA && leafB )+	{+		b2AddCandidatePair( nodeA->shapeIndex, nodeB->shapeIndex, context );+	}+	else if ( leafA )+	{+		b2CollideProxyAndSubtree( nodeA, arrayB, b2GetLeftChild( nodeB ), context );+	}+	else if ( leafB )+	{+		b2CollideProxyAndSubtree( nodeB, arrayA, b2GetLeftChild( nodeA ), context );+	}+	else+	{+		if ( *stackCount < B2_TREE_STACK_SIZE )+		{+			stack[*stackCount] = (b2IndexPair){ .a = b2GetLeftChild( nodeA ), .b = b2GetLeftChild( nodeB ) };+			*stackCount += 1;+		}+		else+		{+			B2_ASSERT( *stackCount < B2_TREE_STACK_SIZE );+		}+	}+}++// This collides two sub-trees against each other. They can live in the same dynamic tree.+// This can only generate pairs cross sub-tree, but not within a sub-tree. This fact means+// this does not generate duplicate pairs.+// For example consider the full binary tree A (B (D  E) C (F G))+// Colliding children of A (B and C) can give pairs (D,F) (D,G) (E,F) and (E,G).+// Then colliding children of B can give the pair (D,E) and for C (F,G).+// So no duplicates even when used for self-collision.+// Whenever a proxy is moved, the flag is propagated up the hierarchy to the root. So+// self collision gathers all those moved internal nodes and collides their subtrees together.+// See Real-time collision detection section 6.3.2. This is faster than querying every moved+// proxy against the whole tree. Scaling is linear instead of linear * log.+// Many other physics engines do this (Bepu, Rapier, etc). So nothing new here.+static void b2CollideCrossPairs( const b2TreeNode* arrayA, const b2TreeNode* arrayB, const b2TreeNode* subtreeA,+								 const b2TreeNode* subtreeB, b2PairContext* context )+{+	b2IndexPair stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;++	// Seed the stack.+	if ( b2TestPair( subtreeA, subtreeB ) )+	{+		b2ExpandPair( arrayA, arrayB, subtreeA, subtreeB, stack, &stackCount, context );+	}++	while ( stackCount > 0 )+	{+		b2IndexPair pair = stack[--stackCount];+		const b2TreeNode* pairA = arrayA + pair.a;+		const b2TreeNode* pairB = arrayB + pair.b;++		int movedA = ( b2IsNodeMoved( pairA + 0 ) ? 0x3 : 0 ) | ( b2IsNodeMoved( pairA + 1 ) ? 0xC : 0 );+		int movedB = ( b2IsNodeMoved( pairB + 0 ) ? 0x5 : 0 ) | ( b2IsNodeMoved( pairB + 1 ) ? 0xA : 0 );++		int mask = b2OverlapPairMask( pairA, pairB ) & ( movedA | movedB );+		while ( mask != 0 )+		{+			int lane = (int)b2CTZ32( (uint32_t)mask );+			mask &= mask - 1;+			b2ExpandPair( arrayA, arrayB, pairA + ( lane >> 1 ), pairB + ( lane & 1 ), stack, &stackCount, context );+		}+	}+}++// This takes moved internal nodes and collides their sub-trees against each other.+static void b2SelfPairsTask( int startIndex, int endIndex, int workerIndex, void* context )+{+	b2TracyCZoneNC( self_pairs, "Self", b2_colorCoral, true );++	b2World* world = context;+	b2BroadPhase* bp = &world->broadPhase;+	const b2DynamicTree* tree = bp->trees + b2_dynamicBody;+	const b2TreeNode* nodes = tree->nodes;+	const int* siblingIndices = bp->movedSiblings;++	b2PairContext pairContext = { .world = world, .pairKeys = &world->taskContexts.data[workerIndex].pairKeys };++	for ( int i = startIndex; i < endIndex; ++i )+	{+		int nodeIndex = siblingIndices[i];+		b2CollideCrossPairs( nodes, nodes, nodes + nodeIndex, nodes + nodeIndex + 1, &pairContext );+	}++	b2FlushCandidatePairs( &pairContext );++	b2TracyCZoneEnd( self_pairs );+}++#define B2_CROSS_SEED_COUNT 64+_Static_assert( ( B2_CROSS_SEED_COUNT & ( B2_CROSS_SEED_COUNT - 1 ) ) == 0, "must be power of 2" );++// This does a serial cross-tree breadth first search until the queue is full. Then it returns+// the queue pairs as seeds for a parallel search.+static int b2GatherCrossSeeds( const b2DynamicTree* treeA, const b2DynamicTree* treeB, b2NodePair* seeds )+{+	const b2TreeNode* nodesA = treeA->nodes;+	const b2TreeNode* nodesB = treeB->nodes;++	// Breadth-first search from the two roots as one pair. An empty tree has a sentinel root that+	// survives nothing.+	b2NodePair queue[2 * B2_CROSS_SEED_COUNT];+	int mask = 2 * B2_CROSS_SEED_COUNT - 1;+	int head = 0;+	int tail = 0;++	const b2TreeNode* rootA = nodesA + B2_ROOT_NODE;+	const b2TreeNode* rootB = nodesB + B2_ROOT_NODE;+	if ( b2TestPair( rootA, rootB ) )+	{+		queue[tail & mask] = (b2NodePair){ .a = *rootA, .b = *rootB };+		tail += 1;+	}++	int seedCount = 0;+	while ( head < tail && seedCount + ( tail - head ) + 3 < B2_CROSS_SEED_COUNT )+	{+		b2NodePair pair = queue[head & mask];+		head += 1;++		if ( b2IsLeaf( &pair.a ) || b2IsLeaf( &pair.b ) )+		{+			seeds[seedCount++] = pair;+			continue;+		}++		const b2TreeNode* a = nodesA + b2GetLeftChild( &pair.a );+		const b2TreeNode* b = nodesB + b2GetLeftChild( &pair.b );++		// Nodes have two children each, so four combinations.+		for ( int i = 0; i < 2; ++i )+		{+			for ( int j = 0; j < 2; ++j )+			{+				if ( b2TestPair( a + i, b + j ) )+				{+					queue[tail & mask] = (b2NodePair){ a[i], b[j] };+					tail += 1;+				}+			}+		}+	}++	while ( head < tail )+	{+		seeds[seedCount] = queue[head & mask];+		seedCount += 1;+		head += 1;+	}++	return seedCount;+}++typedef struct b2CrossContext+{+	b2World* world;+	const b2NodePair* seeds;+	int staticSeedCount;+} b2CrossContext;++static void b2CrossPairsTask( int startIndex, int endIndex, int workerIndex, void* context )+{+	b2TracyCZoneNC( cross_pairs, "Cross", b2_colorCoral, true );++	b2CrossContext* crossContext = context;+	b2World* world = crossContext->world;+	b2BroadPhase* bp = &world->broadPhase;+	const b2TreeNode* staticNodes = bp->trees[b2_staticBody].nodes;+	const b2TreeNode* kinematicNodes = bp->trees[b2_kinematicBody].nodes;+	const b2TreeNode* dynamicNodes = bp->trees[b2_dynamicBody].nodes;++	b2PairContext pairContext = { .world = world, .pairKeys = &world->taskContexts.data[workerIndex].pairKeys };++	for ( int i = startIndex; i < endIndex; ++i )+	{+		const b2TreeNode* nodesB = i < crossContext->staticSeedCount ? staticNodes : kinematicNodes;+		b2NodePair seed = crossContext->seeds[i];+		b2CollideCrossPairs( dynamicNodes, nodesB, &seed.a, &seed.b, &pairContext );+	}++	b2FlushCandidatePairs( &pairContext );++	b2TracyCZoneEnd( cross_pairs );+}++// LSD radix sort. Faster than qsort when count is large and fast enough otherwise.+// https://en.wikipedia.org/wiki/Radix_sort+void b2RadixSortKeys( uint64_t* keys, uint64_t* tempKeys, int count )+{+	int digitCounts[8][256] = { 0 };+	for ( int i = 0; i < count; ++i )+	{+		uint64_t key = keys[i];+		for ( int digit = 0; digit < 8; ++digit )+		{+			digitCounts[digit][( key >> ( 8 * digit ) ) & 0xFF] += 1;+		}+	}++	uint64_t* source = keys;+	uint64_t* target = tempKeys;++	for ( int digit = 0; digit < 8; ++digit )+	{+		int shift = 8 * digit;+		int* offsets = digitCounts[digit];++		if ( offsets[( source[0] >> shift ) & 0xFF] == count )+		{+			// All keys have the same value for this digit. Common for shape pair keys.+			continue;+		}++		int sum = 0;+		for ( int i = 0; i < 256; ++i )+		{+			int digitCount = offsets[i];+			offsets[i] = sum;+			sum += digitCount;+		}++		for ( int i = 0; i < count; ++i )+		{+			uint64_t key = source[i];+			target[offsets[( key >> shift ) & 0xFF]++] = key;+		}++		// Swap for the next pass+		uint64_t* swap = source;+		source = target;+		target = swap;+	}++	if ( source != keys )+	{+		memcpy( keys, source, count * sizeof( uint64_t ) );+	}+}++static void b2UpdateTreesTask( void* context )+{+	b2TracyCZoneNC( tree_task, "Rebuild BVH", b2_colorFireBrick, true );++	b2World* world = context;+	b2DynamicTree_Rebuild( world->broadPhase.trees + b2_dynamicBody, false );+	b2DynamicTree_Rebuild( world->broadPhase.trees + b2_kinematicBody, false );++	b2TracyCZoneEnd( tree_task );+}++// Task that can be done in parallel with the narrow-phase+// - rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good+static void b2EnqueueTreeUpdate( b2World* world )+{+	if ( world->taskCount < B2_MAX_TASKS )+	{+		world->userTreeTask = world->enqueueTaskFcn( &b2UpdateTreesTask, world, world->userTaskContext );+		world->taskCount += 1;+		world->activeTaskCount += world->userTreeTask == NULL ? 0 : 1;+	}+	else+	{+		world->userTreeTask = NULL;+		b2UpdateTreesTask( world );+	}+}++void b2UpdateBroadPhasePairs( b2World* world )+{+	b2BroadPhase* bp = &world->broadPhase;++	bool needUpdate = b2HasTreeMoved( bp->trees + b2_staticBody );+	needUpdate = needUpdate || b2NeedsRebuild( bp->trees + b2_kinematicBody );+	needUpdate = needUpdate || b2NeedsRebuild( bp->trees + b2_dynamicBody );++	if ( needUpdate == false )+	{+		return;+	}++	b2TracyCZoneNC( update_pairs, "Find Pairs", b2_colorMediumSlateBlue, true );++	b2Stack* alloc = &world->stack;++	for ( int i = 0; i < world->workerCount; ++i )+	{+		b2Array_Clear( world->taskContexts.data[i].pairKeys );+	}++	// Generate pairs by querying the dynamic body tree against itself and against+	// the kinematic and static trees.+	{+		// Get the sibling pairs of the dynamic body tree that have moved.+		const b2DynamicTree* dynamicTree = bp->trees + b2_dynamicBody;+		int pairCapacity = dynamicTree->nodeEnd / 2;+		bp->movedSiblings = b2StackAlloc( alloc, pairCapacity * sizeof( int ), "moved pairs" );+		int dynamicMoveCount = b2GatherMovedSiblings( dynamicTree, bp->movedSiblings );++		// Get seeds for colliding against the static and kinematic trees.+		b2NodePair crossSeeds[2 * B2_CROSS_SEED_COUNT];+		int staticSeedCount = b2GatherCrossSeeds( dynamicTree, bp->trees + b2_staticBody, crossSeeds );+		B2_ASSERT( staticSeedCount <= B2_CROSS_SEED_COUNT );+		int kinematicSeedCount = b2GatherCrossSeeds( dynamicTree, bp->trees + b2_kinematicBody, crossSeeds + staticSeedCount );+		B2_ASSERT( kinematicSeedCount <= B2_CROSS_SEED_COUNT );+		int crossMoveCount = staticSeedCount + kinematicSeedCount;++		// Collide the dynamic body tree against the static and kinematic trees.+		b2CrossContext crossContext = {+			.world = world,+			.seeds = crossSeeds,+			.staticSeedCount = staticSeedCount,+		};+		b2ParallelFor( world, &b2CrossPairsTask, crossMoveCount, 1, &crossContext );++		// Collide the dynamic body tree against itself.+		b2ParallelFor( world, &b2SelfPairsTask, dynamicMoveCount, 64, world );+	}++	b2DynamicTree_ClearMoved( bp->trees + b2_staticBody );++	b2TracyCZoneEnd( update_pairs );++	b2TracyCZoneNC( create_contacts, "Create Contacts", b2_colorCoral, true );++	// Update stale trees.+	b2EnqueueTreeUpdate( world );++	// Pairs arrive in deterministic order but scrambled relative to body and shape order+	// sorting them here improves solver performance.+	int pairCount = 0;+	for ( int i = 0; i < world->workerCount; ++i )+	{+		pairCount += world->taskContexts.data[i].pairKeys.count;+	}++	uint64_t* pairKeys = b2StackAlloc( alloc, b2MaxInt( pairCount, 1 ) * sizeof( uint64_t ), "pair keys" );+	int keyCount = 0;+	for ( int i = 0; i < world->workerCount; ++i )+	{+		const b2Array( uint64_t )* workerKeys = &world->taskContexts.data[i].pairKeys;+		if ( workerKeys->count > 0 )+		{+			memcpy( pairKeys + keyCount, workerKeys->data, workerKeys->count * sizeof( uint64_t ) );+			keyCount += workerKeys->count;+		}+	}++	B2_ASSERT( keyCount == pairCount );++	if ( pairCount > 1 )+	{+		uint64_t* tempKeys = b2StackAlloc( alloc, pairCount * sizeof( uint64_t ), "temp keys" );+		b2RadixSortKeys( pairKeys, tempKeys, pairCount );+		b2StackFree( alloc, tempKeys );+	}++	for ( int i = 0; i < keyCount; ++i )+	{+		int shapeIdA = (int)( pairKeys[i] >> 32 );+		int shapeIdB = (int)( pairKeys[i] & 0xFFFFFFFF );+		b2Shape* shapeA = b2Array_Get( world->shapes, shapeIdA );+		b2Shape* shapeB = b2Array_Get( world->shapes, shapeIdB );+		b2CreateContact( world, shapeA, shapeB );+	}++	b2StackFree( alloc, pairKeys );++	b2StackFree( alloc, bp->movedSiblings );+	bp->movedSiblings = NULL;++	b2ValidateSolverSets( world );++	b2TracyCZoneEnd( create_contacts );+}++void b2ValidateBroadphase( const b2BroadPhase* bp )+{+	b2DynamicTree_Validate( bp->trees + b2_dynamicBody );+	b2DynamicTree_Validate( bp->trees + b2_kinematicBody );++	// TODO_ERIN validate every shape AABB is contained in tree AABB+}++void b2ValidateNoEnlarged( const b2BroadPhase* bp )+{+#if B2_ENABLE_VALIDATION == 1+	for ( int j = 0; j < b2_bodyTypeCount; ++j )+	{+		const b2DynamicTree* tree = bp->trees + j;+		b2DynamicTree_ValidateNoMoved( tree );+	}+#else+	B2_UNUSED( bp );+#endif+}
+ upstream/src/broad_phase.h view
@@ -0,0 +1,64 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "dynamic_tree.h"+#include "table.h"++#include "box2d/collision.h"+#include "box2d/types.h"++typedef struct b2Shape b2Shape;+typedef struct b2Stack b2Stack;+typedef struct b2World b2World;++// Store the proxy type in the lower 2 bits of the proxy key. This leaves 30 bits for the id.+#define B2_PROXY_TYPE( KEY ) ( (b2BodyType)( ( KEY ) & 3 ) )+#define B2_PROXY_ID( KEY ) ( ( KEY ) >> 2 )+#define B2_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 b2BroadPhase+{+	// One tree for each body type.+	b2DynamicTree trees[b2_bodyTypeCount];++	// The moved siblings gathered from the dynamic body tree.+	int* movedSiblings;++	// Tracks shape pairs that have a b2Contact+	b2HashSet pairSet;+} b2BroadPhase;++void b2CreateBroadPhase( b2BroadPhase* bp, const b2Capacity* capacity );+void b2DestroyBroadPhase( b2BroadPhase* bp );++int b2BroadPhase_CreateProxy( b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint64_t categoryBits, int shapeIndex,+							  bool forcePairCreation );+void b2BroadPhase_DestroyProxy( b2BroadPhase* bp, int proxyKey );++void b2BroadPhase_MoveProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb );++void b2UpdateBroadPhasePairs( b2World* world );++void b2RadixSortKeys( uint64_t* keys, uint64_t* tempKeys, int count );++void b2ValidateBroadphase( const b2BroadPhase* bp );+void b2ValidateNoEnlarged( const b2BroadPhase* bp );++static inline void b2BroadPhase_MarkProxyMovedSerial( b2BroadPhase* bp, int proxyKey )+{+	b2BodyType proxyType = B2_PROXY_TYPE( proxyKey );+	int proxyId = B2_PROXY_ID( proxyKey );+	b2DynamicTree_MarkProxyMovedSerial( bp->trees + proxyType, proxyId );+}++static inline void b2BroadPhase_MarkProxyMoved( b2BroadPhase* bp, int proxyKey, b2AABB aabb )+{+	b2BodyType proxyType = B2_PROXY_TYPE( proxyKey );+	int proxyId = B2_PROXY_ID( proxyKey );+	b2DynamicTree_MarkProxyMoved( bp->trees + proxyType, proxyId, aabb );+}
+ upstream/src/constraint_graph.c view
@@ -0,0 +1,365 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "constraint_graph.h"++#include "bitset.h"+#include "body.h"+#include "contact.h"+#include "joint.h"+#include "physics_world.h"+#include "solver_set.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.+// todo should be possible to branch on the scatters to avoid writing to kinematic bodies++// This is used for debugging by making all constraints be assigned to overflow.+// Don't expect the same result for overflow and graph colored. They have a different solve order.+#define B2_FORCE_OVERFLOW 0++static int b2GetLinearShare( int total, int rampCount, int rampIndex )+{+	int64_t weight = rampCount - rampIndex;+	int64_t weightSum = (int64_t)rampCount * ( rampCount + 1 ) / 2;+	return (int)( total * weight / weightSum );+}++void b2CreateGraph( b2ConstraintGraph* graph, const b2Capacity* capacity )+{+	_Static_assert( B2_GRAPH_COLOR_COUNT >= 2, "must have at least two constraint graph colors" );+	_Static_assert( B2_OVERFLOW_INDEX == B2_GRAPH_COLOR_COUNT - 1, "bad over flow index" );+	_Static_assert( B2_DYNAMIC_COLOR_COUNT >= 2, "need more dynamic colors" );++	*graph = (b2ConstraintGraph){ 0 };++	int bodyCapacity = b2MaxInt( capacity->staticBodyCount + capacity->dynamicBodyCount, 16 );++	int staticContactCount = capacity->contactCount / 8;+	int dynamicContactCount = capacity->contactCount - staticContactCount;+	int dynamicRampCount = B2_DYNAMIC_COLOR_COUNT / 2;+	int staticRampCount = B2_GRAPH_COLOR_COUNT - B2_DYNAMIC_COLOR_COUNT;++	// Initialize graph color bit set.+	// No bitset for overflow color.+	for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i )+	{+		b2GraphColor* color = graph->colors + i;+		color->bodySet = b2CreateBitSet( bodyCapacity );+		b2SetBitCountAndClear( &color->bodySet, bodyCapacity );++		int contactCapacity = 0;+		if ( i < dynamicRampCount )+		{+			contactCapacity += b2GetLinearShare( dynamicContactCount, dynamicRampCount, i );+		}++		int staticRampIndex = B2_OVERFLOW_INDEX - 1 - i;+		if ( staticRampIndex < staticRampCount )+		{+			contactCapacity += b2GetLinearShare( staticContactCount, staticRampCount, staticRampIndex );+		}++		b2Array_Reserve( color->contactSims, b2MaxInt( 16, contactCapacity ) );+	}+}++void b2DestroyGraph( b2ConstraintGraph* graph )+{+	for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i )+	{+		b2GraphColor* color = graph->colors + i;++		// The bit set should never be used on the overflow color+		B2_ASSERT( i != B2_OVERFLOW_INDEX || color->bodySet.bits == NULL );++		b2DestroyBitSet( &color->bodySet );+		b2Array_Destroy( color->contactSims );+		b2Array_Destroy( color->jointSims );+	}+}++// Contacts are always created as non-touching. They get moved into the constraint+// graph once they are found to be touching.+void b2AddContactToGraph( b2World* world, b2ContactSim* contactSim, b2Contact* contact )+{+	B2_ASSERT( contactSim->manifold.pointCount > 0 );+	B2_ASSERT( contactSim->simFlags & b2_simTouchingFlag );+	B2_ASSERT( contact->flags & b2_contactTouchingFlag );++	b2ConstraintGraph* graph = &world->constraintGraph;+	int colorIndex = B2_OVERFLOW_INDEX;++	int bodyIdA = contact->edges[0].bodyId;+	int bodyIdB = contact->edges[1].bodyId;+	b2Body* bodyA = b2Array_Get( world->bodies, bodyIdA );+	b2Body* bodyB = b2Array_Get( world->bodies, bodyIdB );++	b2BodyType typeA = bodyA->type;+	b2BodyType typeB = bodyB->type;+	B2_ASSERT( typeA == b2_dynamicBody || typeB == b2_dynamicBody );++#if B2_FORCE_OVERFLOW == 0+	if ( typeA == b2_dynamicBody && typeB == b2_dynamicBody )+	{+		// Dynamic constraint colors cannot encroach on colors reserved for static constraints+		for ( int i = 0; i < B2_DYNAMIC_COLOR_COUNT; ++i )+		{+			b2GraphColor* color = graph->colors + i;+			if ( b2GetBit( &color->bodySet, bodyIdA ) || b2GetBit( &color->bodySet, bodyIdB ) )+			{+				continue;+			}++			b2SetBitGrow( &color->bodySet, bodyIdA );+			b2SetBitGrow( &color->bodySet, bodyIdB );+			colorIndex = i;+			break;+		}+	}+	else if ( typeA == b2_dynamicBody )+	{+		// Static constraint colors build from the end to get higher priority than dyn-dyn constraints+		for ( int i = B2_OVERFLOW_INDEX - 1; i >= 1; --i )+		{+			b2GraphColor* color = graph->colors + i;+			if ( b2GetBit( &color->bodySet, bodyIdA ) )+			{+				continue;+			}++			b2SetBitGrow( &color->bodySet, bodyIdA );+			colorIndex = i;+			break;+		}+	}+	else if ( typeB == b2_dynamicBody )+	{+		// Static constraint colors build from the end to get higher priority than dyn-dyn constraints+		for ( int i = B2_OVERFLOW_INDEX - 1; i >= 1; --i )+		{+			b2GraphColor* color = graph->colors + i;+			if ( b2GetBit( &color->bodySet, bodyIdB ) )+			{+				continue;+			}++			b2SetBitGrow( &color->bodySet, bodyIdB );+			colorIndex = i;+			break;+		}+	}+#endif++	b2GraphColor* color = graph->colors + colorIndex;+	contact->colorIndex = colorIndex;+	contact->localIndex = color->contactSims.count;++	b2ContactSim* newContact = b2Array_Emplace( color->contactSims );+	memcpy( newContact, contactSim, sizeof( b2ContactSim ) );++	// todo perhaps skip this if the contact is already awake++	if ( typeA == b2_staticBody )+	{+		newContact->encodedBodySimA = b2EncodeBodySimIndex( bodyA );+		newContact->invMassA = 0.0f;+		newContact->invIA = 0.0f;+	}+	else+	{+		B2_ASSERT( bodyA->setIndex == b2_awakeSet );+		b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );++		int localIndex = bodyA->localIndex;+		newContact->encodedBodySimA = localIndex;++		b2BodySim* bodySimA = b2Array_Get( awakeSet->bodySims, localIndex );+		newContact->invMassA = bodySimA->invMass;+		newContact->invIA = bodySimA->invInertia;+	}++	if ( typeB == b2_staticBody )+	{+		newContact->encodedBodySimB = b2EncodeBodySimIndex( bodyB );+		newContact->invMassB = 0.0f;+		newContact->invIB = 0.0f;+	}+	else+	{+		B2_ASSERT( bodyB->setIndex == b2_awakeSet );+		b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );++		int localIndex = bodyB->localIndex;+		newContact->encodedBodySimB = localIndex;++		b2BodySim* bodySimB = b2Array_Get( awakeSet->bodySims, localIndex );+		newContact->invMassB = bodySimB->invMass;+		newContact->invIB = bodySimB->invInertia;+	}+}++void b2RemoveContactFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex )+{+	b2ConstraintGraph* graph = &world->constraintGraph;++	B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );+	b2GraphColor* color = graph->colors + colorIndex;++	if ( colorIndex != B2_OVERFLOW_INDEX )+	{+		// This might clear a bit for a kinematic or static body, but this has no effect+		b2ClearBit( &color->bodySet, bodyIdA );+		b2ClearBit( &color->bodySet, bodyIdB );+	}++	int movedIndex = b2Array_RemoveSwap( color->contactSims, localIndex );+	if ( movedIndex != B2_NULL_INDEX )+	{+		// Fix index on swapped contact+		b2ContactSim* movedContactSim = color->contactSims.data + localIndex;++		// Fix moved contact+		int movedId = movedContactSim->contactId;+		b2Contact* movedContact = b2Array_Get( world->contacts, movedId );+		B2_ASSERT( movedContact->setIndex == b2_awakeSet );+		B2_ASSERT( movedContact->colorIndex == colorIndex );+		B2_ASSERT( movedContact->localIndex == movedIndex );+		movedContact->localIndex = localIndex;+	}+}++// Notice that a joint cannot share the same color as a contact between the same two bodies. This means I can solve contacts and+// joints in parallel with each other within each color.+static int b2AssignJointColor( b2ConstraintGraph* graph, int bodyIdA, int bodyIdB, b2BodyType typeA, b2BodyType typeB )+{+	B2_ASSERT( typeA == b2_dynamicBody || typeB == b2_dynamicBody );++#if B2_FORCE_OVERFLOW == 0+	if ( typeA == b2_dynamicBody && typeB == b2_dynamicBody )+	{+		// Dynamic constraint colors cannot encroach on colors reserved for static constraints+		for ( int i = 0; i < B2_DYNAMIC_COLOR_COUNT; ++i )+		{+			b2GraphColor* color = graph->colors + i;+			if ( b2GetBit( &color->bodySet, bodyIdA ) || b2GetBit( &color->bodySet, bodyIdB ) )+			{+				continue;+			}++			b2SetBitGrow( &color->bodySet, bodyIdA );+			b2SetBitGrow( &color->bodySet, bodyIdB );+			return i;+		}+	}+	else if ( typeA == b2_dynamicBody )+	{+		// Static constraint colors build from the end to get higher priority than dyn-dyn constraints+		for ( int i = B2_OVERFLOW_INDEX - 1; i >= 1; --i )+		{+			b2GraphColor* color = graph->colors + i;+			if ( b2GetBit( &color->bodySet, bodyIdA ) )+			{+				continue;+			}++			b2SetBitGrow( &color->bodySet, bodyIdA );+			return i;+		}+	}+	else if ( typeB == b2_dynamicBody )+	{+		// Static constraint colors build from the end to get higher priority than dyn-dyn constraints+		for ( int i = B2_OVERFLOW_INDEX - 1; i >= 1; --i )+		{+			b2GraphColor* color = graph->colors + i;+			if ( b2GetBit( &color->bodySet, bodyIdB ) )+			{+				continue;+			}++			b2SetBitGrow( &color->bodySet, bodyIdB );+			return i;+		}+	}+#else+	B2_UNUSED( graph, bodyIdA, bodyIdB );+#endif++	return B2_OVERFLOW_INDEX;+}++b2JointSim* b2CreateJointInGraph( b2World* world, b2Joint* joint )+{+	b2ConstraintGraph* graph = &world->constraintGraph;++	int bodyIdA = joint->edges[0].bodyId;+	int bodyIdB = joint->edges[1].bodyId;+	b2Body* bodyA = b2Array_Get( world->bodies, bodyIdA );+	b2Body* bodyB = b2Array_Get( world->bodies, bodyIdB );++	int colorIndex = b2AssignJointColor( graph, bodyIdA, bodyIdB, bodyA->type, bodyB->type );++	b2JointSim* jointSim = b2Array_Emplace( graph->colors[colorIndex].jointSims );+	memset( jointSim, 0, sizeof( b2JointSim ) );++	joint->colorIndex = colorIndex;+	joint->localIndex = graph->colors[colorIndex].jointSims.count - 1;+	return jointSim;+}++void b2AddJointToGraph( b2World* world, b2JointSim* jointSim, b2Joint* joint )+{+	b2JointSim* jointDst = b2CreateJointInGraph( world, joint );+	memcpy( jointDst, jointSim, sizeof( b2JointSim ) );+}++void b2RemoveJointFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex )+{+	b2ConstraintGraph* graph = &world->constraintGraph;++	B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );+	b2GraphColor* color = graph->colors + colorIndex;++	if ( colorIndex != B2_OVERFLOW_INDEX )+	{+		// May clear static bodies, no effect+		b2ClearBit( &color->bodySet, bodyIdA );+		b2ClearBit( &color->bodySet, bodyIdB );+	}++	int movedIndex = b2Array_RemoveSwap( color->jointSims, localIndex );+	if ( movedIndex != B2_NULL_INDEX )+	{+		// Fix moved joint+		b2JointSim* movedJointSim = color->jointSims.data + localIndex;+		int movedId = movedJointSim->jointId;+		b2Joint* movedJoint = b2Array_Get( world->joints, movedId );+		B2_ASSERT( movedJoint->setIndex == b2_awakeSet );+		B2_ASSERT( movedJoint->colorIndex == colorIndex );+		B2_ASSERT( movedJoint->localIndex == movedIndex );+		movedJoint->localIndex = localIndex;+	}+}++static const b2HexColor b2_graphColors[B2_GRAPH_COLOR_COUNT] = {+	b2_colorRed,	   b2_colorOrange,		b2_colorYellow,			b2_colorLimeGreen,		 b2_colorSpringGreen,+	b2_colorAqua,	   b2_colorDodgerBlue,	b2_colorBlueViolet,		b2_colorMagenta,		 b2_colorDeepPink,+	b2_colorCrimson,   b2_colorCoral,		b2_colorGold,			b2_colorGreenYellow,	 b2_colorMediumSeaGreen,+	b2_colorTurquoise, b2_colorDeepSkyBlue, b2_colorCornflowerBlue, b2_colorMediumSlateBlue, b2_colorMediumOrchid,+	b2_colorHotPink,   b2_colorTomato,		b2_colorKhaki,			b2_colorSilver,+};++b2HexColor b2GetGraphColor( int index )+{+	B2_ASSERT( 0 <= index && index < B2_GRAPH_COLOR_COUNT );+	return b2_graphColors[index];+}
+ upstream/src/constraint_graph.h view
@@ -0,0 +1,67 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "bitset.h"+#include "contact.h"+#include "joint.h"+#include "box2d/constants.h"++#include "box2d/types.h"++typedef struct b2Body b2Body;+typedef struct b2Contact b2Contact;+typedef struct b2StepContext b2StepContext;+typedef struct b2World b2World;++// This holds constraints that cannot fit the graph color limit. This happens when a single dynamic body+// is touching many other bodies.+#define B2_OVERFLOW_INDEX ( B2_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 B2_DYNAMIC_COLOR_COUNT ( B2_GRAPH_COLOR_COUNT - 4 )++typedef struct b2GraphColor+{+	// 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.+	//+	// Dirk suggested having a uint64_t per body that tracks the graph color membership+	// but I think this would make debugging harder and be less flexible. With the bitset+	// I can trivially increase the number of graph colors beyond 64. See usage of b2CountSetBits+	// for validation.+	b2BitSet bodySet;++	// cache friendly arrays+	b2Array( b2ContactSim ) contactSims;+	b2Array( b2JointSim ) jointSims;++	// transient+	union+	{+		void* wideConstraints;+		struct b2ContactConstraint* overflowConstraints;+	};++	int wideConstraintCount;++} b2GraphColor;++typedef struct b2ConstraintGraph+{+	// including overflow at the end+	b2GraphColor colors[B2_GRAPH_COLOR_COUNT];+} b2ConstraintGraph;++void b2CreateGraph( b2ConstraintGraph* graph, const b2Capacity* capacity );+void b2DestroyGraph( b2ConstraintGraph* graph );++void b2AddContactToGraph( b2World* world, b2ContactSim* contactSim, b2Contact* contact );+void b2RemoveContactFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex );++b2JointSim* b2CreateJointInGraph( b2World* world, b2Joint* joint );+void b2AddJointToGraph( b2World* world, b2JointSim* jointSim, b2Joint* joint );+void b2RemoveJointFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex );
+ upstream/src/contact.c view
@@ -0,0 +1,723 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "contact.h"++#include "body.h"+#include "core.h"+#include "island.h"+#include "physics_world.h"+#include "shape.h"+#include "solver_set.h"+#include "table.h"++// needed for dll export+#include "box2d/box2d.h"++#include <stddef.h>++// Contacts and determinism+// A deterministic simulation requires contacts to exist in the same order in b2Island 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 b2UpdateBroadPhasePairs+// - 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:+// b2ManifoldPoint::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.++static b2Contact* b2GetContactFullId( b2World* world, b2ContactId contactId )+{+	int id = contactId.index1 - 1;+	b2Contact* contact = b2Array_Get( world->contacts, id );+	B2_ASSERT( contact->contactId == id && contact->generation == contactId.generation );+	return contact;+}++b2ContactData b2Contact_GetData( b2ContactId contactId )+{+	b2World* world = b2GetWorld( contactId.world0 );+	b2Contact* contact = b2GetContactFullId( world, contactId );+	b2ContactSim* contactSim = b2GetContactSim( world, contact );+	const b2Shape* shapeA = b2Array_Get( world->shapes, contact->shapeIdA );+	const b2Shape* shapeB = b2Array_Get( world->shapes, contact->shapeIdB );++	b2ContactData data = {+		.contactId = contactId,+		.shapeIdA =+			{+				.index1 = shapeA->id + 1,+				.world0 = (uint16_t)contactId.world0,+				.generation = shapeA->generation,+			},+		.shapeIdB =+			{+				.index1 = shapeB->id + 1,+				.world0 = (uint16_t)contactId.world0,+				.generation = shapeB->generation,+			},+		.manifold = contactSim->manifold,+	};++	return data;+}++typedef b2LocalManifold b2ManifoldFcn( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf, b2SimplexCache* cache );++struct b2ContactRegister+{+	b2ManifoldFcn* fcn;+	bool primary;+};++static b2LocalManifold b2CircleManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf, b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollideCircles( &shapeA->circle, &shapeB->circle, xf );+}++static b2LocalManifold b2CapsuleAndCircleManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf,+												   b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollideCapsuleAndCircle( &shapeA->capsule, &shapeB->circle, xf );+}++static b2LocalManifold b2CapsuleManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf, b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollideCapsules( &shapeA->capsule, &shapeB->capsule, xf );+}++static b2LocalManifold b2PolygonAndCircleManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf,+												   b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollidePolygonAndCircle( &shapeA->polygon, &shapeB->circle, xf );+}++static b2LocalManifold b2PolygonAndCapsuleManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf,+													b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollidePolygonAndCapsule( &shapeA->polygon, &shapeB->capsule, xf );+}++static b2LocalManifold b2PolygonManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf, b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollidePolygons( &shapeA->polygon, &shapeB->polygon, xf );+}++static b2LocalManifold b2SegmentAndCircleManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf,+												   b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollideSegmentAndCircle( &shapeA->segment, &shapeB->circle, xf );+}++static b2LocalManifold b2SegmentAndCapsuleManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf,+													b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollideSegmentAndCapsule( &shapeA->segment, &shapeB->capsule, xf );+}++static b2LocalManifold b2SegmentAndPolygonManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf,+													b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollideSegmentAndPolygon( &shapeA->segment, &shapeB->polygon, xf );+}++static b2LocalManifold b2ChainSegmentAndCircleManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf,+														b2SimplexCache* cache )+{+	B2_UNUSED( cache );+	return b2CollideChainSegmentAndCircle( &shapeA->chainSegment, &shapeB->circle, xf );+}++static b2LocalManifold b2ChainSegmentAndCapsuleManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf,+														 b2SimplexCache* cache )+{+	return b2CollideChainSegmentAndCapsule( &shapeA->chainSegment, &shapeB->capsule, xf, cache );+}++static b2LocalManifold b2ChainSegmentAndPolygonManifold( const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xf,+														 b2SimplexCache* cache )+{+	return b2CollideChainSegmentAndPolygon( &shapeA->chainSegment, &shapeB->polygon, xf, cache );+}++// This works with DLL hot reloading.+static const struct b2ContactRegister b2_contactRegistry[b2_shapeTypeCount][b2_shapeTypeCount] = {+	[b2_circleShape] =+		{+			[b2_circleShape] = { .fcn = b2CircleManifold, .primary = true },+			[b2_capsuleShape] = { .fcn = b2CapsuleAndCircleManifold, .primary = false },+			[b2_segmentShape] = { .fcn = b2SegmentAndCircleManifold, .primary = false },+			[b2_polygonShape] = { .fcn = b2PolygonAndCircleManifold, .primary = false },+			[b2_chainSegmentShape] = { .fcn = b2ChainSegmentAndCircleManifold, .primary = false },+		},+	[b2_capsuleShape] =+		{+			[b2_circleShape] = { .fcn = b2CapsuleAndCircleManifold, .primary = true },+			[b2_capsuleShape] = { .fcn = b2CapsuleManifold, .primary = true },+			[b2_segmentShape] = { .fcn = b2SegmentAndCapsuleManifold, .primary = false },+			[b2_polygonShape] = { .fcn = b2PolygonAndCapsuleManifold, .primary = false },+			[b2_chainSegmentShape] = { .fcn = b2ChainSegmentAndCapsuleManifold, .primary = false },+		},+	[b2_segmentShape] =+		{+			[b2_circleShape] = { .fcn = b2SegmentAndCircleManifold, .primary = true },+			[b2_capsuleShape] = { .fcn = b2SegmentAndCapsuleManifold, .primary = true },+			[b2_polygonShape] = { .fcn = b2SegmentAndPolygonManifold, .primary = true },+		},+	[b2_polygonShape] =+		{+			[b2_circleShape] = { .fcn = b2PolygonAndCircleManifold, .primary = true },+			[b2_capsuleShape] = { .fcn = b2PolygonAndCapsuleManifold, .primary = true },+			[b2_segmentShape] = { .fcn = b2SegmentAndPolygonManifold, .primary = false },+			[b2_polygonShape] = { .fcn = b2PolygonManifold, .primary = true },+			[b2_chainSegmentShape] = { .fcn = b2ChainSegmentAndPolygonManifold, .primary = false },+		},+	[b2_chainSegmentShape] =+		{+			[b2_circleShape] = { .fcn = b2ChainSegmentAndCircleManifold, .primary = true },+			[b2_capsuleShape] = { .fcn = b2ChainSegmentAndCapsuleManifold, .primary = true },+			[b2_polygonShape] = { .fcn = b2ChainSegmentAndPolygonManifold, .primary = true },+		},+};++bool b2CanCollide( b2ShapeType typeA, b2ShapeType typeB )+{+	return b2_contactRegistry[typeA][typeB].fcn != NULL;+}++// WARNING: this should never fail to create a contact because the pair already exists in the pairSet.+void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB )+{+	b2ShapeType type1 = shapeA->type;+	b2ShapeType type2 = shapeB->type;++	B2_ASSERT( 0 <= type1 && type1 < b2_shapeTypeCount );+	B2_ASSERT( 0 <= type2 && type2 < b2_shapeTypeCount );++	if ( b2_contactRegistry[type1][type2].fcn == NULL )+	{+		// For example, no segment vs segment collision+		return;+	}++	if ( b2_contactRegistry[type1][type2].primary == false )+	{+		// flip order+		b2CreateContact( world, shapeB, shapeA );+		return;+	}++	b2Body* bodyA = b2Array_Get( world->bodies, shapeA->bodyId );+	b2Body* bodyB = b2Array_Get( world->bodies, shapeB->bodyId );++	B2_ASSERT( bodyA->setIndex != b2_disabledSet && bodyB->setIndex != b2_disabledSet );+	B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet );++	int setIndex;+	if ( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet )+	{+		setIndex = b2_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+		setIndex = b2_disabledSet;+	}++	b2SolverSet* set = b2Array_Get( world->solverSets, setIndex );++	// Create contact key and contact+	int contactId = b2AllocId( &world->contactIdPool );+	if ( contactId == world->contacts.count )+	{+		b2Array_Push( world->contacts, (b2Contact){ 0 } );+	}++	int shapeIdA = shapeA->id;+	int shapeIdB = shapeB->id;++	b2Contact* contact = b2Array_Get( world->contacts, contactId );+	contact->contactId = contactId;+	contact->generation += 1;+	contact->setIndex = setIndex;+	contact->colorIndex = B2_NULL_INDEX;+	contact->localIndex = set->contactSims.count;+	contact->islandId = B2_NULL_INDEX;+	contact->islandIndex = B2_NULL_INDEX;+	contact->shapeIdA = shapeIdA;+	contact->shapeIdB = shapeIdB;+	contact->flags = 0;++	// Both bodies must enable recycling+	if ( ( bodyA->flags & b2_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b2_bodyEnableContactRecycling ) != 0 )+	{+		contact->flags |= b2_contactRecycleFlag;+	}++	B2_ASSERT( shapeA->sensorIndex == B2_NULL_INDEX && shapeB->sensorIndex == B2_NULL_INDEX );++	if ( shapeA->enableContactEvents || shapeB->enableContactEvents )+	{+		contact->flags |= b2_contactEnableContactEvents;+	}++	// Connect to body A+	{+		contact->edges[0].bodyId = shapeA->bodyId;+		contact->edges[0].prevKey = B2_NULL_INDEX;+		contact->edges[0].nextKey = bodyA->headContactKey;++		int keyA = ( contactId << 1 ) | 0;+		int headContactKey = bodyA->headContactKey;+		if ( headContactKey != B2_NULL_INDEX )+		{+			b2Contact* headContact = b2Array_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 = B2_NULL_INDEX;+		contact->edges[1].nextKey = bodyB->headContactKey;++		int keyB = ( contactId << 1 ) | 1;+		int headContactKey = bodyB->headContactKey;+		if ( bodyB->headContactKey != B2_NULL_INDEX )+		{+			b2Contact* headContact = b2Array_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 = B2_SHAPE_PAIR_KEY( shapeIdA, shapeIdB );+	b2AddKey( &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.+	b2ContactSim* contactSim = b2Array_Emplace( set->contactSims );+	contactSim->contactId = contactId;++#if B2_ENABLE_VALIDATION+	contactSim->bodyIdA = shapeA->bodyId;+	contactSim->bodyIdB = shapeB->bodyId;+#endif++	contactSim->encodedBodySimA = b2EncodeBodySimIndex( bodyA );+	contactSim->encodedBodySimB = b2EncodeBodySimIndex( bodyB );+	contactSim->invMassA = 0.0f;+	contactSim->invIA = 0.0f;+	contactSim->invMassB = 0.0f;+	contactSim->invIB = 0.0f;+	contactSim->shapeIdA = shapeIdA;+	contactSim->shapeIdB = shapeIdB;+	contactSim->cache = b2_emptySimplexCache;+	contactSim->manifold = (b2Manifold){ 0 };++	// These get updated in the narrow phase, but these are needed for first touch+	if ( world->frictionCallback == NULL )+	{+		contactSim->friction = b2MixFriction( shapeA->material.friction, shapeB->material.friction );+	}+	else+	{+		contactSim->friction = world->frictionCallback( shapeA->material.friction, shapeA->material.userMaterialId,+														shapeB->material.friction, shapeB->material.userMaterialId );+	}++	if ( world->restitutionCallback == NULL )+	{+		contactSim->restitution = b2MixRestitution( shapeA->material.restitution, shapeB->material.restitution );+	}+	else+	{+		contactSim->restitution = world->restitutionCallback( shapeA->material.restitution, shapeA->material.userMaterialId,+															  shapeB->material.restitution, shapeB->material.userMaterialId );+	}++	contactSim->tangentSpeed = 0.0f;+	contactSim->simFlags = contact->flags;++	if ( shapeA->enablePreSolveEvents || shapeB->enablePreSolveEvents )+	{+		contactSim->simFlags |= b2_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 b2DestroyContact( b2World* world, b2Contact* contact )+{+	// Remove pair from set+	uint64_t pairKey = B2_SHAPE_PAIR_KEY( contact->shapeIdA, contact->shapeIdB );+	b2RemoveKey( &world->broadPhase.pairSet, pairKey );++	b2ContactEdge* edgeA = contact->edges + 0;+	b2ContactEdge* edgeB = contact->edges + 1;++	int bodyIdA = edgeA->bodyId;+	int bodyIdB = edgeB->bodyId;+	b2Body* bodyA = b2Array_Get( world->bodies, bodyIdA );+	b2Body* bodyB = b2Array_Get( world->bodies, bodyIdB );++	uint32_t flags = contact->flags;+	bool touching = ( flags & b2_contactTouchingFlag ) != 0;++	// End touch event+	if ( touching && ( flags & b2_contactEnableContactEvents ) != 0 )+	{+		uint16_t worldId = world->worldId;+		const b2Shape* shapeA = b2Array_Get( world->shapes, contact->shapeIdA );+		const b2Shape* shapeB = b2Array_Get( world->shapes, contact->shapeIdB );+		b2ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation };+		b2ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation };++		b2ContactId contactId = {+			.index1 = contact->contactId + 1,+			.world0 = world->worldId,+			.padding = 0,+			.generation = contact->generation,+		};++		b2ContactEndTouchEvent event = {+			.shapeIdA = shapeIdA,+			.shapeIdB = shapeIdB,+			.contactId = contactId,+		};++		b2Array_Push( world->contactEndEvents[world->endEventArrayIndex], event );+	}++	// Remove from body A+	if ( edgeA->prevKey != B2_NULL_INDEX )+	{+		b2Contact* prevContact = b2Array_Get( world->contacts, edgeA->prevKey >> 1 );+		b2ContactEdge* prevEdge = prevContact->edges + ( edgeA->prevKey & 1 );+		prevEdge->nextKey = edgeA->nextKey;+	}++	if ( edgeA->nextKey != B2_NULL_INDEX )+	{+		b2Contact* nextContact = b2Array_Get( world->contacts, edgeA->nextKey >> 1 );+		b2ContactEdge* 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 != B2_NULL_INDEX )+	{+		b2Contact* prevContact = b2Array_Get( world->contacts, edgeB->prevKey >> 1 );+		b2ContactEdge* prevEdge = prevContact->edges + ( edgeB->prevKey & 1 );+		prevEdge->nextKey = edgeB->nextKey;+	}++	if ( edgeB->nextKey != B2_NULL_INDEX )+	{+		b2Contact* nextContact = b2Array_Get( world->contacts, edgeB->nextKey >> 1 );+		b2ContactEdge* 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;++	// Remove contact from island.+	if ( contact->islandId != B2_NULL_INDEX )+	{+		b2UnlinkContact( world, contact );+	}++	if ( contact->colorIndex != B2_NULL_INDEX )+	{+		// contact is an active constraint+		B2_ASSERT( contact->setIndex == b2_awakeSet );+		b2RemoveContactFromGraph( world, bodyIdA, bodyIdB, contact->colorIndex, contact->localIndex );+	}+	else+	{+		// contact is non-touching or is sleeping+		B2_ASSERT( contact->setIndex != b2_awakeSet || ( contact->flags & b2_contactTouchingFlag ) == 0 );+		b2SolverSet* set = b2Array_Get( world->solverSets, contact->setIndex );++		int movedIndex = b2Array_RemoveSwap( set->contactSims, contact->localIndex );+		if ( movedIndex != B2_NULL_INDEX )+		{+			b2ContactSim* movedContactSim = set->contactSims.data + contact->localIndex;+			b2Contact* movedContact = b2Array_Get( world->contacts, movedContactSim->contactId );+			movedContact->localIndex = contact->localIndex;+		}+	}++	// Free contact and id (preserve generation)+	contact->contactId = B2_NULL_INDEX;+	contact->setIndex = B2_NULL_INDEX;+	contact->colorIndex = B2_NULL_INDEX;+	contact->localIndex = B2_NULL_INDEX;+	b2FreeId( &world->contactIdPool, contactId );++	if ( touching )+	{+		b2WakeBody( world, bodyA );+		b2WakeBody( world, bodyB );+	}+}++b2ContactSim* b2GetContactSim( b2World* world, b2Contact* contact )+{+	if ( contact->setIndex == b2_awakeSet && contact->colorIndex != B2_NULL_INDEX )+	{+		// contact lives in constraint graph+		B2_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B2_GRAPH_COLOR_COUNT );+		b2GraphColor* color = world->constraintGraph.colors + contact->colorIndex;+		return b2Array_Get( color->contactSims, contact->localIndex );+	}++	b2SolverSet* set = b2Array_Get( world->solverSets, contact->setIndex );+	return b2Array_Get( set->contactSims, contact->localIndex );+}++// Update the contact manifold and touching status.+// Note: do not assume the shape AABBs are overlapping or are valid.+bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2WorldTransform transformA,+					  b2Vec2 centerOffsetA, b2Shape* shapeB, b2WorldTransform transformB, b2Vec2 centerOffsetB )+{+	// Save old manifold+	b2Manifold oldManifold = contactSim->manifold;++	// Compute the manifold in frame A, then marshal it to world anchors relative to each shape origin.+	// The relative pose differences the two world positions before the narrow phase runs in frame A,+	// so precision is retained far from the origin.+	// anchorB = worldPoint - pB = rot(qA, localAnchorA) + pA - pB = anchorA + (pA - pB)+	b2Transform relativeTransform = b2InvMulWorldTransforms( transformA, transformB );+	b2ManifoldFcn* fcn = b2_contactRegistry[shapeA->type][shapeB->type].fcn;+	b2LocalManifold local = fcn( shapeA, shapeB, relativeTransform, &contactSim->cache );++	contactSim->manifold = (b2Manifold){ 0 };+	contactSim->manifold.normal = b2RotateVector( transformA.q, local.normal );+	contactSim->manifold.pointCount = local.pointCount;++	b2Vec2 originDelta = b2SubPos( transformA.p, transformB.p );+	for ( int i = 0; i < local.pointCount; ++i )+	{+		b2ManifoldPoint* mp = contactSim->manifold.points + i;+		mp->anchorA = b2RotateVector( transformA.q, local.points[i].point );+		mp->anchorB = b2Add( mp->anchorA, originDelta );+		mp->separation = local.points[i].separation;+		mp->id = local.points[i].id;+	}++	// Keep these updated in case the values on the shapes are modified+	if ( world->frictionCallback == NULL )+	{+		contactSim->friction = b2MixFriction( shapeA->material.friction, shapeB->material.friction );+	}+	else+	{+		contactSim->friction = world->frictionCallback( shapeA->material.friction, shapeA->material.userMaterialId,+														shapeB->material.friction, shapeB->material.userMaterialId );+	}++	if ( world->restitutionCallback == NULL )+	{+		contactSim->restitution = b2MixRestitution( shapeA->material.restitution, shapeB->material.restitution );+	}+	else+	{+		contactSim->restitution = world->restitutionCallback( shapeA->material.restitution, shapeA->material.userMaterialId,+															  shapeB->material.restitution, shapeB->material.userMaterialId );+	}++	if ( shapeA->material.rollingResistance > 0.0f || shapeB->material.rollingResistance > 0.0f )+	{+		float radiusA = b2GetShapeRadius( shapeA );+		float radiusB = b2GetShapeRadius( shapeB );+		float maxRadius = b2MaxFloat( radiusA, radiusB );+		contactSim->rollingResistance =+			b2MaxFloat( shapeA->material.rollingResistance, shapeB->material.rollingResistance ) * maxRadius;+	}+	else+	{+		contactSim->rollingResistance = 0.0f;+	}++	contactSim->tangentSpeed = shapeA->material.tangentSpeed + shapeB->material.tangentSpeed;++	int pointCount = contactSim->manifold.pointCount;+	bool touching = pointCount > 0;++	if ( touching && world->preSolveFcn != NULL && ( contactSim->simFlags & b2_simEnablePreSolveEvents ) != 0 )+	{+		b2ShapeId shapeIdA = { shapeA->id + 1, world->worldId, shapeA->generation };+		b2ShapeId shapeIdB = { shapeB->id + 1, world->worldId, shapeB->generation };++		// This call assumes thread safety.+		world->preSolveFcn( shapeIdA, shapeIdB, &contactSim->manifold, world->preSolveContext );++		// Keep these current.+		pointCount = contactSim->manifold.pointCount;+		touching = pointCount > 0;++		// Mark recycling dirty.+		if ( touching == false )+		{+			contactSim->simFlags &= ~b2_simRelativeTransformValid;+		}+	}++	if ( touching && ( shapeA->enableHitEvents || shapeB->enableHitEvents ) )+	{+		contactSim->simFlags |= b2_simEnableHitEvent;+	}+	else+	{+		contactSim->simFlags &= ~b2_simEnableHitEvent;+	}++	if ( pointCount > 0 )+	{+		contactSim->manifold.rollingImpulse = oldManifold.rollingImpulse;++		// Match old contact ids to new contact ids and copy the+		// stored impulses to warm start the solver.+		int unmatchedCount = 0;+		for ( int i = 0; i < pointCount; ++i )+		{+			b2ManifoldPoint* mp2 = contactSim->manifold.points + i;++			// shift anchors to be center of mass relative+			mp2->anchorA = b2Sub( mp2->anchorA, centerOffsetA );+			mp2->anchorB = b2Sub( mp2->anchorB, centerOffsetB );++			mp2->normalImpulse = 0.0f;+			mp2->tangentImpulse = 0.0f;+			mp2->totalNormalImpulse = 0.0f;+			mp2->normalVelocity = 0.0f;+			mp2->persisted = false;++			uint16_t id2 = mp2->id;++			for ( int j = 0; j < oldManifold.pointCount; ++j )+			{+				b2ManifoldPoint* mp1 = oldManifold.points + j;++				if ( mp1->id == id2 )+				{+					mp2->normalImpulse = mp1->normalImpulse;+					mp2->tangentImpulse = mp1->tangentImpulse;+					mp2->persisted = true;++					break;+				}+			}++			unmatchedCount += mp2->persisted ? 0 : 1;+		}++		B2_UNUSED( unmatchedCount );+	}++#if 0+		// todo I haven't found an improvement from this yet+		// If there are unmatched new contact points, apply any left over old impulse.+		if (unmatchedCount > 0)+		{+			float unmatchedNormalImpulse = 0.0f;+			float unmatchedTangentImpulse = 0.0f;+			for (int i = 0; i < oldManifold.pointCount; ++i)+			{+				b2ManifoldPoint* mp = oldManifold.points + i;+				unmatchedNormalImpulse += mp->normalImpulse;+				unmatchedTangentImpulse += mp->tangentImpulse;+			}++			float inverse = 1.0f / unmatchedCount;+			unmatchedNormalImpulse *= inverse;+			unmatchedTangentImpulse *= inverse;++			for ( int i = 0; i < pointCount; ++i )+			{+				b2ManifoldPoint* mp2 = contactSim->manifold.points + i;++				if (mp2->persisted)+				{+					continue;+				}++				mp2->normalImpulse = unmatchedNormalImpulse;+				mp2->tangentImpulse = unmatchedTangentImpulse;+			}+		}+#endif++	if ( touching )+	{+		contactSim->simFlags |= b2_simTouchingFlag;+	}+	else+	{+		contactSim->simFlags &= ~b2_simTouchingFlag;+	}++	return touching;+}
+ upstream/src/contact.h view
@@ -0,0 +1,165 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"+#include "core.h"++#include "box2d/collision.h"+#include "box2d/math_functions.h"+#include "box2d/types.h"++typedef struct b2Shape b2Shape;+typedef struct b2World b2World;++enum b2ContactFlags+{+	// Set when the solid shapes are touching.+	b2_contactTouchingFlag = 0x00000001,++	// Contact has a hit event+	b2_contactHitEventFlag = 0x00000002,++	// This contact wants contact events+	b2_contactEnableContactEvents = 0x00000004,++	b2_contactRecycleFlag = 0x00000008,++	// Set when the shapes are touching+	b2_simTouchingFlag = 0x00010000,++	// This contact no longer has overlapping AABBs+	b2_simDisjoint = 0x00020000,++	// This contact started touching+	b2_simStartedTouching = 0x00040000,++	// This contact stopped touching+	b2_simStoppedTouching = 0x00080000,++	// This contact has a hit event+	b2_simEnableHitEvent = 0x00100000,++	// This contact wants pre-solve events+	b2_simEnablePreSolveEvents = 0x00200000,++	// This contact has a cached relative transform+	b2_simRelativeTransformValid = 0x00400000,+};++// 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 b2ContactEdge+{+	int bodyId;+	int prevKey;+	int nextKey;+} b2ContactEdge;++// Cold contact data. Used as a persistent handle and for persistent island+// connectivity.+typedef struct b2Contact+{+	b2ContactEdge edges[2];++	// A contact only belongs to an island if touching, otherwise B2_NULL_INDEX.+	int islandId;++	// Index into the island's contacts array for O(1) swap-removal.+	// B2_NULL_INDEX when not in an island.+	int islandIndex;++	// index of simulation set stored in b2World+	// B2_NULL_INDEX when slot is free+	int setIndex;++	// index into the constraint graph color array+	// B2_NULL_INDEX for non-touching or sleeping contacts+	// B2_NULL_INDEX when slot is free+	int colorIndex;++	// contact index within set or graph color+	// B2_NULL_INDEX when slot is free+	int localIndex;++	int shapeIdA;+	int shapeIdB;+	int contactId;++	// b2ContactFlags+	uint32_t flags;++	// This is monotonically advanced when a contact is allocated in this slot+	// Used to check for invalid b2ContactId+	uint32_t generation;+} b2Contact;++/// The class manages contact between two shapes. A contact exists for each overlapping+/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist+/// that has no contact points.+typedef struct b2ContactSim+{+	int contactId;++	// Cache for contact recycling.+	b2Rot cachedRotationA;+	b2Rot cachedRotationB;+	b2Transform cachedRelativePose;++#if B2_ENABLE_VALIDATION+	int bodyIdA;+	int bodyIdB;+#endif++	int encodedBodySimA;+	int encodedBodySimB;++	// b2ContactFlags+	uint32_t simFlags;++	int shapeIdA;+	int shapeIdB;++	float invMassA;+	float invIA;++	float invMassB;+	float invIB;++	b2Manifold manifold;++	// Mixed friction and restitution+	float friction;+	float restitution;+	float rollingResistance;+	float tangentSpeed;++	b2SimplexCache cache;+} b2ContactSim;++bool b2CanCollide( b2ShapeType typeA, b2ShapeType typeB );++void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB );+void b2DestroyContact( b2World* world, b2Contact* contact );++b2ContactSim* b2GetContactSim( b2World* world, b2Contact* contact );++bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2WorldTransform transformA, b2Vec2 centerOffsetA,+					  b2Shape* shapeB, b2WorldTransform transformB, b2Vec2 centerOffsetB );++b2DeclareArray( b2Contact );+b2DeclareArray( b2ContactSim );++static inline float b2MixFriction( float frictionA, float frictionB )+{+	return sqrtf( frictionA * frictionB );+}++static inline float b2MixRestitution( float restitutionA, float restitutionB )+{+	return b2MaxFloat( restitutionA, restitutionB );+}
+ upstream/src/contact_solver.c view
@@ -0,0 +1,835 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "contact_solver.h"++#include "body.h"+#include "constraint_graph.h"+#include "contact.h"+#include "core.h"+#include "physics_world.h"+#include "platform.h"+#include "solver_set.h"++#include <stddef.h>++// 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)++void b2PrepareContacts_Overflow( b2StepContext* context )+{+	b2TracyCZoneNC( prepare_overflow_contact, "Prepare Overflow Contact", b2_colorYellow, true );++	b2World* world = context->world;+	b2ConstraintGraph* graph = context->graph;+	b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX;+	b2ContactConstraint* constraints = color->overflowConstraints;+	int contactCount = color->contactSims.count;+	b2ContactSim* contacts = color->contactSims.data;+	b2BodyState* states = context->states;++#if B2_ENABLE_VALIDATION+	b2Body* bodies = world->bodies.data;+#endif++	// Stiffer for static contacts to avoid bodies getting pushed through the ground+	b2Softness contactSoftness = context->contactSoftness;+	b2Softness staticSoftness = context->staticSoftness;++	float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f;+	bool anyRestitution = false;++	for ( int i = 0; i < contactCount; ++i )+	{+		b2ContactSim* contactSim = contacts + i;++		b2Manifold* manifold = &contactSim->manifold;+		int pointCount = manifold->pointCount;++		B2_ASSERT( 0 < pointCount && pointCount <= 2 );++		int indexA = b2DecodeAwakeIndex( contactSim->encodedBodySimA );+		int indexB = b2DecodeAwakeIndex( contactSim->encodedBodySimB );++#if B2_ENABLE_VALIDATION+		b2Body* bodyA = bodies + contactSim->bodyIdA;+		B2_ASSERT( contactSim->encodedBodySimA == b2EncodeBodySimIndex( bodyA ) );++		b2Body* bodyB = bodies + contactSim->bodyIdB;+		B2_ASSERT( contactSim->encodedBodySimB == b2EncodeBodySimIndex( bodyB ) );+#endif++		b2ContactConstraint* constraint = constraints + i;++		// 0 is null+		constraint->indexA = indexA + 1;+		constraint->indexB = indexB + 1;+		constraint->normal = manifold->normal;+		constraint->friction = contactSim->friction;+		constraint->restitution = contactSim->restitution;+		constraint->rollingResistance = contactSim->rollingResistance;+		constraint->rollingImpulse = warmStartScale * manifold->rollingImpulse;+		constraint->tangentSpeed = contactSim->tangentSpeed;+		constraint->pointCount = pointCount;++		bool haveRestitution = contactSim->restitution > 0.0f;+		bool hitEvents = ( contactSim->simFlags & b2_simEnableHitEvent ) != 0;+		bool sampleVelocity = haveRestitution || hitEvents;+		anyRestitution = anyRestitution || haveRestitution;++		b2Vec2 vA = b2Vec2_zero;+		float wA = 0.0f;+		b2Vec2 vB = b2Vec2_zero;+		float wB = 0.0f;++		if ( sampleVelocity )+		{+			if ( indexA != B2_NULL_INDEX )+			{+				vA = states[indexA].linearVelocity;+				wA = states[indexA].angularVelocity;+			}++			if ( indexB != B2_NULL_INDEX )+			{+				vB = states[indexB].linearVelocity;+				wB = states[indexB].angularVelocity;+			}+		}++		float mA = contactSim->invMassA;+		float iA = contactSim->invIA;+		float mB = contactSim->invMassB;+		float iB = contactSim->invIB;++		if ( indexA == B2_NULL_INDEX || indexB == B2_NULL_INDEX )+		{+			constraint->softness = staticSoftness;+		}+		else+		{+			constraint->softness = contactSoftness;+		}++		// copy mass into constraint to avoid cache misses during sub-stepping+		constraint->invMassA = mA;+		constraint->invIA = iA;+		constraint->invMassB = mB;+		constraint->invIB = iB;++		{+			float k = iA + iB;+			constraint->rollingMass = k > 0.0f ? 1.0f / k : 0.0f;+		}++		b2Vec2 normal = constraint->normal;+		b2Vec2 tangent = b2RightPerp( constraint->normal );++		for ( int j = 0; j < pointCount; ++j )+		{+			b2ManifoldPoint* mp = manifold->points + j;+			b2ContactConstraintPoint* cp = constraint->points + j;++			cp->normalImpulse = warmStartScale * mp->normalImpulse;+			cp->tangentImpulse = warmStartScale * mp->tangentImpulse;+			cp->totalNormalImpulse = 0.0f;+			cp->restitutionImpulse = 0.0f;++			b2Vec2 rA = mp->anchorA;+			b2Vec2 rB = mp->anchorB;++			cp->anchorA = rA;+			cp->anchorB = rB;+			cp->baseSeparation = mp->separation - b2Dot( b2Sub( rB, rA ), normal );++			float rnA = b2Cross( rA, normal );+			float rnB = b2Cross( rB, normal );+			float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;+			cp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;++			float rtA = b2Cross( rA, tangent );+			float rtB = b2Cross( rB, tangent );+			float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;+			cp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;++			// Only compute the normal velocity terms if needed.+			if ( sampleVelocity )+			{+				b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) );+				b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) );+				float vn = b2Dot( normal, b2Sub( vrB, vrA ) );++				cp->relativeVelocity = vn;+				mp->normalVelocity = vn;+			}+			else+			{+				cp->relativeVelocity = 0.0f;+				mp->normalVelocity = 0.0f;+			}+		}+	}++	if ( anyRestitution )+	{+		// Check if set first? Store per task context and OR?+		b2AtomicStoreInt( &context->anyRestitution, 1 );+	}++	b2TracyCZoneEnd( prepare_overflow_contact );+}++void b2WarmStartContacts_Overflow( b2StepContext* context )+{+	b2TracyCZoneNC( warmstart_overflow_contact, "WarmStart Overflow Contact", b2_colorDarkOrange, true );++	b2ConstraintGraph* graph = context->graph;+	b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX;+	b2ContactConstraint* constraints = color->overflowConstraints;+	int contactCount = color->contactSims.count;+	b2World* world = context->world;+	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	b2BodyState* states = awakeSet->bodyStates.data;++	// This is a dummy state to represent a static body because static bodies don't have a solver body.+	b2BodyState dummyState = b2_identityBodyState;++	for ( int i = 0; i < contactCount; ++i )+	{+		b2ContactConstraint* constraint = constraints + i;++		int indexA = constraint->indexA - 1;+		int indexB = constraint->indexB - 1;++		b2BodyState* stateA = indexA == B2_NULL_INDEX ? &dummyState : states + indexA;+		b2BodyState* stateB = indexB == B2_NULL_INDEX ? &dummyState : states + indexB;++		b2Vec2 vA = stateA->linearVelocity;+		float wA = stateA->angularVelocity;+		b2Vec2 vB = stateB->linearVelocity;+		float wB = stateB->angularVelocity;++		float mA = constraint->invMassA;+		float iA = constraint->invIA;+		float mB = constraint->invMassB;+		float iB = constraint->invIB;++		// Stiffer for static contacts to avoid bodies getting pushed through the ground+		b2Vec2 normal = constraint->normal;+		b2Vec2 tangent = b2RightPerp( constraint->normal );+		int pointCount = constraint->pointCount;++		for ( int j = 0; j < pointCount; ++j )+		{+			b2ContactConstraintPoint* cp = constraint->points + j;++			// fixed anchors+			b2Vec2 rA = cp->anchorA;+			b2Vec2 rB = cp->anchorB;++			b2Vec2 P = b2Add( b2MulSV( cp->normalImpulse, normal ), b2MulSV( cp->tangentImpulse, tangent ) );++			cp->totalNormalImpulse += cp->normalImpulse;++			wA -= iA * b2Cross( rA, P );+			vA = b2MulAdd( vA, -mA, P );+			wB += iB * b2Cross( rB, P );+			vB = b2MulAdd( vB, mB, P );+		}++		wA -= iA * constraint->rollingImpulse;+		wB += iB * constraint->rollingImpulse;++		if ( stateA->flags & b2_dynamicFlag )+		{+			stateA->linearVelocity = vA;+			stateA->angularVelocity = wA;+		}++		if ( stateB->flags & b2_dynamicFlag )+		{+			stateB->linearVelocity = vB;+			stateB->angularVelocity = wB;+		}+	}++	b2TracyCZoneEnd( warmstart_overflow_contact );+}++// Solve the non-penetration constraints with the soft bias. No friction and no restitution.+void b2PushContacts_Overflow( b2StepContext* context )+{+	b2TracyCZoneNC( push_contact, "Push Overflow Contact", b2_colorAliceBlue, true );++	b2ConstraintGraph* graph = context->graph;+	b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX;+	b2ContactConstraint* constraints = color->overflowConstraints;+	int contactCount = color->contactSims.count;+	b2World* world = context->world;+	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	b2BodyState* states = awakeSet->bodyStates.data;++	float inv_h = context->inv_h;+	const float contactSpeed = context->world->contactSpeed;++	// This is a dummy body to represent a static body since static bodies don't have a solver body.+	b2BodyState dummyState = b2_identityBodyState;++	for ( int i = 0; i < contactCount; ++i )+	{+		b2ContactConstraint* constraint = constraints + i;+		float mA = constraint->invMassA;+		float iA = constraint->invIA;+		float mB = constraint->invMassB;+		float iB = constraint->invIB;++		int indexA = constraint->indexA - 1;+		int indexB = constraint->indexB - 1;++		b2BodyState* stateA = indexA == B2_NULL_INDEX ? &dummyState : states + indexA;+		b2Vec2 vA = stateA->linearVelocity;+		float wA = stateA->angularVelocity;+		b2Rot dqA = stateA->deltaRotation;++		b2BodyState* stateB = indexB == B2_NULL_INDEX ? &dummyState : states + indexB;+		b2Vec2 vB = stateB->linearVelocity;+		float wB = stateB->angularVelocity;+		b2Rot dqB = stateB->deltaRotation;++		b2Vec2 dp = b2Sub( stateB->deltaPosition, stateA->deltaPosition );++		b2Vec2 normal = constraint->normal;+		b2Softness softness = constraint->softness;+		int pointCount = constraint->pointCount;++		float vnc = b2Dot( b2Sub( vB, vA ), normal );++		float rnA[2], rnB[2];+		float impulses[2] = { 0.0f, 0.0f };+		float normalImpulse = 0.0f;+		float angularA = 0.0f;+		float angularB = 0.0f;++		for ( int j = 0; j < pointCount; ++j )+		{+			b2ContactConstraintPoint* cp = constraint->points + j;++			// fixed anchor points+			b2Vec2 rA = cp->anchorA;+			b2Vec2 rB = cp->anchorB;++			// compute current separation+			// this is subject to round-off error if the anchor is far from the body center of mass+			b2Vec2 ds = b2Add( dp, b2Sub( b2RotateVector( dqB, rB ), b2RotateVector( dqA, rA ) ) );+			float s = cp->baseSeparation + b2Dot( ds, normal );++			float velocityBias;+			float massScale;+			float impulseScale;+			if ( s > 0.0f )+			{+				// speculative bias is positive+				velocityBias = s * inv_h;+				massScale = 1.0f;+				impulseScale = 0.0f;+			}+			else+			{+				// overlap bias is negative+				velocityBias = b2MaxFloat( softness.massScale * softness.biasRate * s, -contactSpeed );+				massScale = softness.massScale;+				impulseScale = softness.impulseScale;+			}++			rnA[j] = b2Cross( rA, normal );+			rnB[j] = b2Cross( rB, normal );++			// Relative normal velocity at contact.+			float vn = vnc + wB * rnB[j] - wA * rnA[j];+			for ( int k = 0; k < j; ++k )+			{+				// Point 2 only depends on the impulse from point 1.+				vn += ( mA + mB + iA * rnA[k] * rnA[j] + iB * rnB[k] * rnB[j] ) * impulses[k];+			}++			float impulse = b2MaxFloat( -cp->normalMass * ( massScale * vn + velocityBias ) - impulseScale * cp->normalImpulse,+										-cp->normalImpulse );+			cp->normalImpulse += impulse;+			cp->totalNormalImpulse += impulse;++			impulses[j] = impulse;++			// These accumulators are not used until the loop is done.+			normalImpulse += impulse;+			angularA += impulse * rnA[j];+			angularB += impulse * rnB[j];+		}++		b2Vec2 P = b2MulSV( normalImpulse, normal );+		vA = b2MulSub( vA, mA, P );+		wA -= iA * angularA;+		vB = b2MulAdd( vB, mB, P );+		wB += iB * angularB;++		if ( stateA->flags & b2_dynamicFlag )+		{+			stateA->linearVelocity = vA;+			stateA->angularVelocity = wA;+		}++		if ( stateB->flags & b2_dynamicFlag )+		{+			stateB->linearVelocity = vB;+			stateB->angularVelocity = wB;+		}+	}++	b2TracyCZoneEnd( push_contact );+}++// Solve contacts: normal, friction and rolling resistance.+void b2SolveContacts_Overflow( b2StepContext* context )+{+	b2TracyCZoneNC( solve_contact, "Solve Overflow Contact", b2_colorAliceBlue, true );++	b2ConstraintGraph* graph = context->graph;+	b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX;+	b2ContactConstraint* constraints = color->overflowConstraints;+	int contactCount = color->contactSims.count;+	b2World* world = context->world;+	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	b2BodyState* states = awakeSet->bodyStates.data;++	float inv_h = context->inv_h;++	// This is a dummy body to represent a static body since static bodies don't have a solver body.+	b2BodyState dummyState = b2_identityBodyState;++	for ( int i = 0; i < contactCount; ++i )+	{+		b2ContactConstraint* constraint = constraints + i;+		float mA = constraint->invMassA;+		float iA = constraint->invIA;+		float mB = constraint->invMassB;+		float iB = constraint->invIB;++		int indexA = constraint->indexA - 1;+		int indexB = constraint->indexB - 1;++		b2BodyState* stateA = indexA == B2_NULL_INDEX ? &dummyState : states + indexA;+		b2Vec2 vA = stateA->linearVelocity;+		float wA = stateA->angularVelocity;+		b2Rot dqA = stateA->deltaRotation;++		b2BodyState* stateB = indexB == B2_NULL_INDEX ? &dummyState : states + indexB;+		b2Vec2 vB = stateB->linearVelocity;+		float wB = stateB->angularVelocity;+		b2Rot dqB = stateB->deltaRotation;++		b2Vec2 dp = b2Sub( stateB->deltaPosition, stateA->deltaPosition );++		b2Vec2 normal = constraint->normal;+		b2Vec2 tangent = b2RightPerp( normal );+		float friction = constraint->friction;++		int pointCount = constraint->pointCount;+		float totalNormalImpulse = 0.0f;++		b2Vec2 dv = b2Sub( vB, vA );+		float vnc = b2Dot( dv, normal );++		// vt = dot(vrB - sB * tangent - (vrA + sA * tangent), tangent)+		//    = dot(vrB - vrA, tangent) - (sA + sB)+		float vtc = b2Dot( dv, tangent ) - constraint->tangentSpeed;++		float normalImpulses[2] = { 0.0f, 0.0f };+		float tangentImpulses[2] = { 0.0f, 0.0f };++		// Cache cross products+		float rnA[2], rnB[2], rtA[2], rtB[2];+		for ( int j = 0; j < pointCount; ++j )+		{+			b2ContactConstraintPoint* cp = constraint->points + j;+			rnA[j] = b2Cross( cp->anchorA, normal );+			rnB[j] = b2Cross( cp->anchorB, normal );+			rtA[j] = b2Cross( cp->anchorA, tangent );+			rtB[j] = b2Cross( cp->anchorB, tangent );+		}++		for ( int j = 0; j < pointCount; ++j )+		{+			b2ContactConstraintPoint* cp = constraint->points + j;++			// Fixed anchor points.+			b2Vec2 rA = cp->anchorA;+			b2Vec2 rB = cp->anchorB;++			// Compute current separation.+			// This is subject to round-off error if the anchor is far from the body center of mass.+			b2Vec2 ds = b2Add( dp, b2Sub( b2RotateVector( dqB, rB ), b2RotateVector( dqA, rA ) ) );+			float s = cp->baseSeparation + b2Dot( ds, normal );++			// Speculative bias, zero when overlapped.+			float velocityBias = s > 0.0f ? s * inv_h : 0.0f;++			// Relative normal velocity at contact.+			float vn = vnc + wB * rnB[j] - wA * rnA[j];+			for ( int k = 0; k < j; ++k )+			{+				// This allows getting point 2 normal velocity with only one dependency.+				vn += ( mA + mB + iA * rnA[k] * rnA[j] + iB * rnB[k] * rnB[j] ) * normalImpulses[k];+			}++			float impulse = b2MaxFloat( -cp->normalMass * ( vn + velocityBias ), -cp->normalImpulse );+			cp->normalImpulse += impulse;+			cp->totalNormalImpulse += impulse;+			normalImpulses[j] = impulse;++			// This is used for rolling resistance+			totalNormalImpulse += cp->normalImpulse;+		}++		// Rolling resistance+		float rollingImpulse;+		{+			float Cdot = wB - wA;+			for ( int j = 0; j < pointCount; ++j )+			{+				Cdot += ( iA * rnA[j] + iB * rnB[j] ) * normalImpulses[j];+			}++			float lambda = constraint->rollingImpulse;+			float maxLambda = constraint->rollingResistance * totalNormalImpulse;+			constraint->rollingImpulse = b2ClampFloat( lambda - constraint->rollingMass * Cdot, -maxLambda, maxLambda );+			rollingImpulse = constraint->rollingImpulse - lambda;+		}++		// Friction+		for ( int j = 0; j < pointCount; ++j )+		{+			b2ContactConstraintPoint* cp = constraint->points + j;++			// relative tangent velocity at contact+			float vt = vtc + wB * rtB[j] - wA * rtA[j] + ( iA * rtA[j] + iB * rtB[j] ) * rollingImpulse;+			for ( int k = 0; k < pointCount; ++k )+			{+				vt += ( iA * rtA[j] * rnA[k] + iB * rtB[j] * rnB[k] ) * normalImpulses[k];+			}++			for ( int k = 0; k < j; ++k )+			{+				vt += ( mA + mB + iA * rtA[k] * rtA[j] + iB * rtB[k] * rtB[j] ) * tangentImpulses[k];+			}++			// clamp the accumulated force+			float maxFriction = friction * cp->normalImpulse;+			float impulse = b2ClampFloat( -cp->tangentMass * vt, -maxFriction - cp->tangentImpulse, maxFriction - cp->tangentImpulse );+			cp->tangentImpulse += impulse;+			tangentImpulses[j] = impulse;+		}++		float normalImpulse = 0.0f;+		float tangentImpulse = 0.0f;+		float angularA = rollingImpulse;+		float angularB = rollingImpulse;+		for ( int j = 0; j < pointCount; ++j )+		{+			normalImpulse += normalImpulses[j];+			tangentImpulse += tangentImpulses[j];+			angularA += normalImpulses[j] * rnA[j] + tangentImpulses[j] * rtA[j];+			angularB += normalImpulses[j] * rnB[j] + tangentImpulses[j] * rtB[j];+		}++		b2Vec2 P = b2Add( b2MulSV( normalImpulse, normal ), b2MulSV( tangentImpulse, tangent ) );+		vA = b2MulSub( vA, mA, P );+		wA -= iA * angularA;+		vB = b2MulAdd( vB, mB, P );+		wB += iB * angularB;++		if ( stateA->flags & b2_dynamicFlag )+		{+			stateA->linearVelocity = vA;+			stateA->angularVelocity = wA;+		}++		if ( stateB->flags & b2_dynamicFlag )+		{+			stateB->linearVelocity = vB;+			stateB->angularVelocity = wB;+		}+	}++	b2TracyCZoneEnd( solve_contact );+}++void b2ApplyRestitution_Overflow( b2StepContext* context )+{+	b2TracyCZoneNC( restitution_overflow, "Restitution Overflow", b2_colorViolet, true );++	b2ConstraintGraph* graph = context->graph;+	b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX;+	b2ContactConstraint* constraints = color->overflowConstraints;+	int contactCount = color->contactSims.count;+	b2World* world = context->world;+	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	b2BodyState* states = awakeSet->bodyStates.data;++	float threshold = world->restitutionThreshold;+	float inv_h = context->inv_h;+	bool propagate = world->enableRestitutionPropagation;++	b2BodyState dummyState = b2_identityBodyState;++	for ( int i = 0; i < contactCount; ++i )+	{+		b2ContactConstraint* constraint = constraints + i;+		if ( propagate == false && constraint->restitution == 0.0f )+		{+			continue;+		}++		float mA = constraint->invMassA;+		float iA = constraint->invIA;+		float mB = constraint->invMassB;+		float iB = constraint->invIB;++		int indexA = constraint->indexA - 1;+		int indexB = constraint->indexB - 1;++		b2BodyState* stateA = indexA == B2_NULL_INDEX ? &dummyState : states + indexA;+		b2Vec2 vA = stateA->linearVelocity;+		float wA = stateA->angularVelocity;+		b2Rot dqA = stateA->deltaRotation;++		b2BodyState* stateB = indexB == B2_NULL_INDEX ? &dummyState : states + indexB;+		b2Vec2 vB = stateB->linearVelocity;+		float wB = stateB->angularVelocity;+		b2Rot dqB = stateB->deltaRotation;++		b2Vec2 dp = b2Sub( stateB->deltaPosition, stateA->deltaPosition );++		b2Vec2 normal = constraint->normal;+		float restitution = constraint->restitution;+		int pointCount = constraint->pointCount;++		for ( int j = 0; j < pointCount; ++j )+		{+			b2ContactConstraintPoint* cp = constraint->points + j;++			b2Vec2 rA = cp->anchorA;+			b2Vec2 rB = cp->anchorB;++			// The total normal impulse is 0 for speculative points.+			float compressionImpulse = cp->totalNormalImpulse - cp->restitutionImpulse;+			bool armed = restitution > 0.0f && cp->relativeVelocity < -threshold && compressionImpulse > 0.0f;++			float velocityBias;+			if ( armed )+			{+				velocityBias = restitution * cp->relativeVelocity;+			}+			else+			{+				b2Vec2 ds = b2Add( dp, b2Sub( b2RotateVector( dqB, rB ), b2RotateVector( dqA, rA ) ) );+				float s = cp->baseSeparation + b2Dot( ds, normal );++				velocityBias = s > 0.0f ? s * inv_h : 0.0f;+			}++			b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) );+			b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) );+			float vn = b2Dot( b2Sub( vrB, vrA ), normal );++			float impulse = -cp->normalMass * ( vn + velocityBias );++			float newImpulse = b2MaxFloat( cp->normalImpulse + impulse, 0.0f );+			impulse = newImpulse - cp->normalImpulse;++			float approachImpulse = b2MinFloat( b2MaxFloat( -cp->normalMass * vn, 0.0f ), b2MaxFloat( impulse, 0.0f ) );++			if ( armed )+			{+				// Poisson kinetic restitution guarantees no energy gain.+				float allowance = restitution * ( compressionImpulse + approachImpulse ) - cp->restitutionImpulse;+				impulse = b2MinFloat( impulse, approachImpulse + b2MaxFloat( allowance, 0.0f ) );+			}++			cp->normalImpulse += impulse;+			cp->restitutionImpulse += impulse - approachImpulse;+			cp->totalNormalImpulse += impulse;++			b2Vec2 P = b2MulSV( impulse, normal );+			vA = b2MulSub( vA, mA, P );+			wA -= iA * b2Cross( rA, P );++			vB = b2MulAdd( vB, mB, P );+			wB += iB * b2Cross( rB, P );+		}++		if ( stateA->flags & b2_dynamicFlag )+		{+			stateA->linearVelocity = vA;+			stateA->angularVelocity = wA;+		}++		if ( stateB->flags & b2_dynamicFlag )+		{+			stateB->linearVelocity = vB;+			stateB->angularVelocity = wB;+		}+	}++	b2TracyCZoneEnd( restitution_overflow );+}++void b2StoreImpulses_Overflow( b2StepContext* context )+{+	b2TracyCZoneNC( store_impulses, "Store", b2_colorFireBrick, true );++	b2World* world = context->world;+	b2ConstraintGraph* graph = context->graph;+	b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX;+	b2ContactConstraint* constraints = color->overflowConstraints;+	b2ContactSim* contactSims = color->contactSims.data;+	b2TaskContext* taskContext = world->taskContexts.data + 0;+	b2BitSet* hitEventBitSet = &taskContext->hitEventBitSet;+	int contactCount = color->contactSims.count;+	float negHitThreshold = -world->hitEventThreshold;+	bool hasHitEvents = taskContext->hasHitEvents;++	for ( int i = 0; i < contactCount; ++i )+	{+		const b2ContactConstraint* constraint = constraints + i;+		b2ContactSim* contactSim = contactSims + i;+		b2Manifold* manifold = &contactSim->manifold;+		int pointCount = manifold->pointCount;++		for ( int j = 0; j < pointCount; ++j )+		{+			b2ManifoldPoint* mp = manifold->points + j;+			mp->normalImpulse = constraint->points[j].normalImpulse;+			mp->tangentImpulse = constraint->points[j].tangentImpulse;+			mp->totalNormalImpulse = constraint->points[j].totalNormalImpulse;+		}++		if ( ( contactSim->simFlags & b2_simEnableHitEvent ) != 0 )+		{+			for ( int j = 0; j < contactSim->manifold.pointCount; ++j )+			{+				b2ManifoldPoint* mp = manifold->points + j;++				// Need to check total impulse because the point may be speculative and not colliding+				if ( mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f )+				{+					b2SetBit( hitEventBitSet, contactSim->contactId );+					hasHitEvents = true;+					break;+				}+			}+		}++		manifold->rollingImpulse = constraint->rollingImpulse;+	}++	taskContext->hasHitEvents = hasHitEvents;++	b2TracyCZoneEnd( store_impulses );+}++void b2PrepareContacts_Wide( b2SolverBlock block, b2StepContext* context )+{+#if defined( B2_SIMD_HAS_WIDTH_8 )+	if ( context->world->simdWidth == 8 )+	{+		b2PrepareContacts_WideW8( block, context );+		return;+	}+#endif++	b2PrepareContacts_WideW4( block, context );+}++void b2WarmStartContacts_Wide( b2SolverBlock block, b2StepContext* context )+{+#if defined( B2_SIMD_HAS_WIDTH_8 )+	if ( context->world->simdWidth == 8 )+	{+		b2WarmStartContacts_WideW8( block, context );+		return;+	}+#endif++	b2WarmStartContacts_WideW4( block, context );+}++void b2PushContacts_Wide( b2SolverBlock block, b2StepContext* context )+{+#if defined( B2_SIMD_HAS_WIDTH_8 )+	if ( context->world->simdWidth == 8 )+	{+		b2PushContacts_WideW8( block, context );+		return;+	}+#endif++	b2PushContacts_WideW4( block, context );+}++void b2SolveContacts_Wide( b2SolverBlock block, b2StepContext* context )+{+#if defined( B2_SIMD_HAS_WIDTH_8 )+	if ( context->world->simdWidth == 8 )+	{+		b2SolveContacts_WideW8( block, context );+		return;+	}+#endif++	b2SolveContacts_WideW4( block, context );+}++void b2ApplyRestitution_Wide( b2SolverBlock block, b2StepContext* context )+{+#if defined( B2_SIMD_HAS_WIDTH_8 )+	if ( context->world->simdWidth == 8 )+	{+		b2ApplyRestitution_WideW8( block, context );+		return;+	}+#endif++	b2ApplyRestitution_WideW4( block, context );+}++void b2StoreImpulses_Wide( b2SolverBlock block, b2StepContext* context, int workerIndex )+{+#if defined( B2_SIMD_HAS_WIDTH_8 )+	if ( context->world->simdWidth == 8 )+	{+		b2StoreImpulses_WideW8( block, context, workerIndex );+		return;+	}+#endif++	b2StoreImpulses_WideW4( block, context, workerIndex );+}++int b2GetWideContactConstraintByteCount( int simdWidth )+{+#if defined( B2_SIMD_HAS_WIDTH_8 )+	if ( simdWidth == 8 )+	{+		return b2GetWideContactConstraintByteCountW8();+	}+#else+	B2_UNUSED( simdWidth );+#endif++	return b2GetWideContactConstraintByteCountW4();+}
+ upstream/src/contact_solver.h view
@@ -0,0 +1,75 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "solver.h"++typedef struct b2ContactSim b2ContactSim;++typedef struct b2ContactConstraintPoint+{+	b2Vec2 anchorA, anchorB;+	float baseSeparation;+	float relativeVelocity;+	float normalImpulse;+	float tangentImpulse;+	float totalNormalImpulse;+	float restitutionImpulse;+	float normalMass;+	float tangentMass;+} b2ContactConstraintPoint;++typedef struct b2ContactConstraint+{+	// base-1, 0 for null+	int indexA;+	int indexB;+	b2ContactConstraintPoint points[2];+	b2Vec2 normal;+	float invMassA, invMassB;+	float invIA, invIB;+	float friction;+	float restitution;+	float tangentSpeed;+	float rollingResistance;+	float rollingMass;+	float rollingImpulse;+	b2Softness softness;+	int pointCount;+} b2ContactConstraint;++// This function allows hiding SIMD intrinsics in the source file to improve compilation performance.+int b2GetWideContactConstraintByteCount( int simdWidth );++// Overflow contacts don't fit into the constraint graph coloring+void b2PrepareContacts_Overflow( b2StepContext* context );+void b2WarmStartContacts_Overflow( b2StepContext* context );+void b2PushContacts_Overflow( b2StepContext* context );+void b2SolveContacts_Overflow( b2StepContext* context );+void b2ApplyRestitution_Overflow( b2StepContext* context );+void b2StoreImpulses_Overflow( b2StepContext* context );++// Contacts that live within the constraint graph coloring+void b2PrepareContacts_Wide( b2SolverBlock block, b2StepContext* context );+void b2WarmStartContacts_Wide( b2SolverBlock block, b2StepContext* context );+void b2PushContacts_Wide( b2SolverBlock block, b2StepContext* context );+void b2SolveContacts_Wide( b2SolverBlock block, b2StepContext* context );+void b2ApplyRestitution_Wide( b2SolverBlock block, b2StepContext* context );+void b2StoreImpulses_Wide( b2SolverBlock block, b2StepContext* context, int workerIndex );++void b2PrepareContacts_WideW4( b2SolverBlock block, b2StepContext* context );+void b2WarmStartContacts_WideW4( b2SolverBlock block, b2StepContext* context );+void b2PushContacts_WideW4( b2SolverBlock block, b2StepContext* context );+void b2SolveContacts_WideW4( b2SolverBlock block, b2StepContext* context );+void b2ApplyRestitution_WideW4( b2SolverBlock block, b2StepContext* context );+void b2StoreImpulses_WideW4( b2SolverBlock block, b2StepContext* context, int workerIndex );+int b2GetWideContactConstraintByteCountW4( void );++void b2PrepareContacts_WideW8( b2SolverBlock block, b2StepContext* context );+void b2WarmStartContacts_WideW8( b2SolverBlock block, b2StepContext* context );+void b2PushContacts_WideW8( b2SolverBlock block, b2StepContext* context );+void b2SolveContacts_WideW8( b2SolverBlock block, b2StepContext* context );+void b2ApplyRestitution_WideW8( b2SolverBlock block, b2StepContext* context );+void b2StoreImpulses_WideW8( b2SolverBlock block, b2StepContext* context, int workerIndex );+int b2GetWideContactConstraintByteCountW8( void );
+ upstream/src/contact_solver_w4.c view
@@ -0,0 +1,5 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#define B2_SIMD_WIDTH 4+#include "contact_solver_wide.inl"
+ upstream/src/contact_solver_w8.c view
@@ -0,0 +1,22 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#define B2_SIMD_WIDTH 8++#include "core.h"++#if defined( B2_SIMD_AVX2 )++#if defined( _MSC_VER ) && !defined( __clang__ ) && !defined( __AVX2__ )+#error "MSVC must compile this file with /arch:AVX2, or define BOX2D_DISABLE_SIMD"+#endif++#include <immintrin.h>++B2_AVX2_BEGIN++#include "contact_solver_wide.inl"++B2_AVX2_END++#endif
+ upstream/src/contact_solver_wide.inl view
@@ -0,0 +1,2160 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "body.h"+#include "constraint_graph.h"+#include "contact.h"+#include "contact_solver.h"+#include "core.h"+#include "physics_world.h"+#include "platform.h"+#include "solver_set.h"++#include <stddef.h>++#if B2_SIMD_WIDTH == 8+#define B2_WIDE( name ) name##W8+#define B2_SIMD_SHIFT 3+#elif B2_SIMD_WIDTH == 4+#define B2_WIDE( name ) name##W4+#define B2_SIMD_SHIFT 2+#else+#error "B2_SIMD_WIDTH must be 4 or 8"+#endif++#define b2FloatW B2_WIDE( b2Float )+#define b2Vec2W B2_WIDE( b2Vec2 )+#define b2RotW B2_WIDE( b2Rot )+#define b2BodyStateW B2_WIDE( b2BodyState )+#define b2ContactConstraintWide B2_WIDE( b2ContactConstraint )++#if B2_SIMD_WIDTH == 8++#include <immintrin.h>++// wide float holds 8 numbers+typedef __m256 b2FloatW;++#elif defined( B2_SIMD_NEON )++#include <arm_neon.h>++// wide float holds 4 numbers+typedef float32x4_t b2FloatW;++#elif defined( B2_SIMD_SSE2 )++#include <emmintrin.h>++// wide float holds 4 numbers+typedef __m128 b2FloatW;++#else++// scalar math+typedef struct b2FloatW+{+	float x, y, z, w;+} b2FloatW;++#endif++// Wide vec2+typedef struct b2Vec2W+{+	b2FloatW X, Y;+} b2Vec2W;++// Wide rotation+typedef struct b2RotW+{+	b2FloatW C, S;+} b2RotW;++#if B2_SIMD_WIDTH == 8++static inline b2FloatW b2ZeroW( void )+{+	return _mm256_setzero_ps();+}++static inline b2FloatW b2SplatW( float scalar )+{+	return _mm256_set1_ps( scalar );+}++static inline b2FloatW b2SetW( float a, float b, float c, float d, float e, float f, float g, float h )+{+	return _mm256_setr_ps( a, b, c, d, e, f, g, h );+}++static inline b2FloatW b2NegW( b2FloatW a )+{+	// Create a mask with the sign bit set for each element+	__m256 mask = _mm256_set1_ps( -0.0f );++	// XOR the input with the mask to negate each element+	return _mm256_xor_ps( a, mask );+}++static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b )+{+	return _mm256_add_ps( a, b );+}++static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b )+{+	return _mm256_sub_ps( a, b );+}++static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b )+{+	return _mm256_mul_ps( a, b );+}++static inline b2FloatW b2DivW( b2FloatW a, b2FloatW b )+{+	return _mm256_div_ps( a, b );+}++static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c )+{+	// FMA can be emulated: https://github.com/lattera/glibc/blob/master/sysdeps/ieee754/dbl-64/s_fmaf.c#L34+	// return _mm256_fmadd_ps( b, c, a );+	return _mm256_add_ps( _mm256_mul_ps( b, c ), a );+}++static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c )+{+	// return _mm256_fnmadd_ps(b, c, a);+	return _mm256_sub_ps( a, _mm256_mul_ps( b, c ) );+}++static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b )+{+	return _mm256_min_ps( a, b );+}++static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b )+{+	return _mm256_max_ps( a, b );+}++// a = clamp(a, -b, b)+static inline b2FloatW b2SymClampW( b2FloatW a, b2FloatW b )+{+	__m256 mask = _mm256_set1_ps( -0.0f );+	__m256 nb = _mm256_xor_ps( b, mask );+	return _mm256_max_ps( nb, _mm256_min_ps( a, b ) );+}++static inline b2FloatW b2AndW( b2FloatW a, b2FloatW b )+{+	return _mm256_and_ps( a, b );+}++static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b )+{+	return _mm256_cmp_ps( a, b, _CMP_GT_OQ );+}++static inline b2FloatW b2LessThanW( b2FloatW a, b2FloatW b )+{+	return _mm256_cmp_ps( a, b, _CMP_LT_OQ );+}++static inline bool b2AnyTrueW( b2FloatW mask )+{+	return _mm256_movemask_ps( mask ) != 0;+}++static inline bool b2AllZeroW( b2FloatW a )+{+	// Compare each element with zero+	b2FloatW zero = _mm256_setzero_ps();+	b2FloatW cmp = _mm256_cmp_ps( a, zero, _CMP_EQ_OQ );++	// Create a mask from the comparison results+	int mask = _mm256_movemask_ps( cmp );++	// If all elements are zero, the mask will be 0xFF (11111111 in binary)+	return mask == 0xFF;+}++// component-wise returns mask ? b : a+static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask )+{+	return _mm256_blendv_ps( a, b, mask );+}++// Unaligned load.+static inline b2FloatW b2LoadU( const float* data )+{+	return _mm256_loadu_ps( data );+}++// data must be 32-byte aligned+static inline void b2StoreW( float* data, b2FloatW a )+{+	_mm256_store_ps( data, a );+}++static inline b2FloatW b2SoftMaskW( const int* indexA, const int* indexB )+{+	__m256i zero = _mm256_setzero_si256();+	__m256i a = _mm256_cmpeq_epi32( _mm256_load_si256( (const __m256i*)indexA ), zero );+	__m256i b = _mm256_cmpeq_epi32( _mm256_load_si256( (const __m256i*)indexB ), zero );+	return _mm256_castsi256_ps( _mm256_or_si256( a, b ) );+}++#elif defined( B2_SIMD_NEON )++static inline b2FloatW b2ZeroW( void )+{+	return vdupq_n_f32( 0.0f );+}++static inline b2FloatW b2SplatW( float scalar )+{+	return vdupq_n_f32( scalar );+}++static inline b2FloatW b2SetW( float a, float b, float c, float d )+{+	float32_t array[4] = { a, b, c, d };+	return vld1q_f32( array );+}++static inline b2FloatW b2NegW( b2FloatW a )+{+	return vnegq_f32( a );+}++static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b )+{+	return vaddq_f32( a, b );+}++static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b )+{+	return vsubq_f32( a, b );+}++static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b )+{+	return vmulq_f32( a, b );+}++static inline b2FloatW b2DivW( b2FloatW a, b2FloatW b )+{+	return vdivq_f32( a, b );+}++static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c )+{+	return vaddq_f32( a, vmulq_f32( b, c ) );+}++static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c )+{+	return vsubq_f32( a, vmulq_f32( b, c ) );+}++static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b )+{+	return vminq_f32( a, b );+}++static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b )+{+	return vmaxq_f32( a, b );+}++// a = clamp(a, -b, b)+static inline b2FloatW b2SymClampW( b2FloatW a, b2FloatW b )+{+	b2FloatW nb = vnegq_f32( b );+	return vmaxq_f32( nb, vminq_f32( a, b ) );+}++static inline b2FloatW b2AndW( b2FloatW a, b2FloatW b )+{+	return vreinterpretq_f32_u32( vandq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) );+}++static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b )+{+	return vreinterpretq_f32_u32( vcgtq_f32( a, b ) );+}++static inline b2FloatW b2LessThanW( b2FloatW a, b2FloatW b )+{+	return vreinterpretq_f32_u32( vcltq_f32( a, b ) );+}++static inline bool b2AnyTrueW( b2FloatW mask )+{+	return vmaxvq_u32( vreinterpretq_u32_f32( mask ) ) != 0;+}++static inline bool b2AllZeroW( b2FloatW a )+{+	// Create a zero vector for comparison+	b2FloatW 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+	return vminvq_u32( cmp_result ) != 0;+}++// component-wise returns mask ? b : a+static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask )+{+	uint32x4_t mask32 = vreinterpretq_u32_f32( mask );+	return vbslq_f32( mask32, b, a );+}++static inline b2FloatW b2LoadU( const float32_t* data )+{+	return vld1q_f32( data );+}++static inline void b2StoreW( float32_t* data, b2FloatW a )+{+	vst1q_f32( data, a );+}++static inline b2FloatW b2UnpackLoW( b2FloatW a, b2FloatW b )+{+	return vzip1q_f32( a, b );+}++static inline b2FloatW b2UnpackHiW( b2FloatW a, b2FloatW b )+{+	return vzip2q_f32( a, b );+}++static inline b2FloatW b2SoftMaskW( const int* indexA, const int* indexB )+{+	int32x4_t zero = vdupq_n_s32( 0 );+	uint32x4_t a = vceqq_s32( vld1q_s32( indexA ), zero );+	uint32x4_t b = vceqq_s32( vld1q_s32( indexB ), zero );+	return vreinterpretq_f32_u32( vorrq_u32( a, b ) );+}++#elif defined( B2_SIMD_SSE2 )++static inline b2FloatW b2ZeroW( void )+{+	return _mm_setzero_ps();+}++static inline b2FloatW b2SplatW( float scalar )+{+	return _mm_set1_ps( scalar );+}++static inline b2FloatW b2SetW( float a, float b, float c, float d )+{+	return _mm_setr_ps( a, b, c, d );+}++static inline b2FloatW b2NegW( b2FloatW 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 b2FloatW b2AddW( b2FloatW a, b2FloatW b )+{+	return _mm_add_ps( a, b );+}++static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b )+{+	return _mm_sub_ps( a, b );+}++static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b )+{+	return _mm_mul_ps( a, b );+}++static inline b2FloatW b2DivW( b2FloatW a, b2FloatW b )+{+	return _mm_div_ps( a, b );+}++static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c )+{+	return _mm_add_ps( a, _mm_mul_ps( b, c ) );+}++static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c )+{+	return _mm_sub_ps( a, _mm_mul_ps( b, c ) );+}++static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b )+{+	return _mm_min_ps( a, b );+}++static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b )+{+	return _mm_max_ps( a, b );+}++// a = clamp(a, -b, b)+static inline b2FloatW b2SymClampW( b2FloatW a, b2FloatW b )+{+	// 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+	__m128 nb = _mm_xor_ps( b, mask );++	return _mm_max_ps( nb, _mm_min_ps( a, b ) );+}++static inline b2FloatW b2AndW( b2FloatW a, b2FloatW b )+{+	return _mm_and_ps( a, b );+}++static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b )+{+	return _mm_cmpgt_ps( a, b );+}++static inline b2FloatW b2LessThanW( b2FloatW a, b2FloatW b )+{+	return _mm_cmplt_ps( a, b );+}++static inline bool b2AnyTrueW( b2FloatW mask )+{+	return _mm_movemask_ps( mask ) != 0;+}++static inline bool b2AllZeroW( b2FloatW a )+{+	// Compare each element with zero+	b2FloatW zero = _mm_setzero_ps();+	b2FloatW cmp = _mm_cmpeq_ps( a, zero );++	// Create a mask from the comparison results+	int mask = _mm_movemask_ps( cmp );++	// If all elements are zero, the mask will be 0xF (1111 in binary)+	return mask == 0xF;+}++// component-wise returns mask ? b : a+static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask )+{+	return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) );+}++static inline b2FloatW b2LoadU( const float* data )+{+	return _mm_loadu_ps( data );+}++static inline void b2StoreW( float* data, b2FloatW a )+{+	_mm_store_ps( data, a );+}++static inline b2FloatW b2UnpackLoW( b2FloatW a, b2FloatW b )+{+	return _mm_unpacklo_ps( a, b );+}++static inline b2FloatW b2UnpackHiW( b2FloatW a, b2FloatW b )+{+	return _mm_unpackhi_ps( a, b );+}++static inline b2FloatW b2SoftMaskW( const int* indexA, const int* indexB )+{+	__m128i zero = _mm_setzero_si128();+	__m128i a = _mm_cmpeq_epi32( _mm_load_si128( (const __m128i*)indexA ), zero );+	__m128i b = _mm_cmpeq_epi32( _mm_load_si128( (const __m128i*)indexB ), zero );+	return _mm_castsi128_ps( _mm_or_si128( a, b ) );+}++#else++static inline b2FloatW b2ZeroW( void )+{+	return (b2FloatW){ 0.0f, 0.0f, 0.0f, 0.0f };+}++static inline b2FloatW b2SplatW( float scalar )+{+	return (b2FloatW){ scalar, scalar, scalar, scalar };+}++static inline b2FloatW b2SetW( float a, float b, float c, float d )+{+	return (b2FloatW){ a, b, c, d };+}++static inline b2FloatW b2NegW( b2FloatW a )+{+	return (b2FloatW){ -a.x, -a.y, -a.z, -a.w };+}++static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b )+{+	return (b2FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w };+}++static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b )+{+	return (b2FloatW){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w };+}++static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b )+{+	return (b2FloatW){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w };+}++static inline b2FloatW b2DivW( b2FloatW a, b2FloatW b )+{+	return (b2FloatW){ a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w };+}++static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c )+{+	return (b2FloatW){ 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 b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c )+{+	return (b2FloatW){ 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 b2FloatW b2MinW( b2FloatW a, b2FloatW b )+{+	b2FloatW 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 b2FloatW b2MaxW( b2FloatW a, b2FloatW b )+{+	b2FloatW 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;+}++// a = clamp(a, -b, b)+static inline b2FloatW b2SymClampW( b2FloatW a, b2FloatW b )+{+	b2FloatW r;+	r.x = b2ClampFloat( a.x, -b.x, b.x );+	r.y = b2ClampFloat( a.y, -b.y, b.y );+	r.z = b2ClampFloat( a.z, -b.z, b.z );+	r.w = b2ClampFloat( a.w, -b.w, b.w );+	return r;+}++static inline b2FloatW b2AndW( b2FloatW a, b2FloatW b )+{+	b2FloatW 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 b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b )+{+	b2FloatW 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 b2FloatW b2LessThanW( b2FloatW a, b2FloatW b )+{+	b2FloatW 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 b2AnyTrueW( b2FloatW mask )+{+	return mask.x != 0.0f || mask.y != 0.0f || mask.z != 0.0f || mask.w != 0.0f;+}++static inline bool b2AllZeroW( b2FloatW a )+{+	return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f;+}++static inline b2FloatW b2LoadU( const float* data )+{+	return (b2FloatW){ data[0], data[1], data[2], data[3] };+}++static inline void b2StoreW( float* data, b2FloatW a )+{+	data[0] = a.x;+	data[1] = a.y;+	data[2] = a.z;+	data[3] = a.w;+}++// component-wise returns mask ? b : a+static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask )+{+	b2FloatW 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 b2FloatW b2UnpackLoW( b2FloatW a, b2FloatW b )+{+	return (b2FloatW){ a.x, b.x, a.y, b.y };+}++static inline b2FloatW b2UnpackHiW( b2FloatW a, b2FloatW b )+{+	return (b2FloatW){ a.z, b.z, a.w, b.w };+}++static inline b2FloatW b2SoftMaskW( const int* indexA, const int* indexB )+{+	b2FloatW r;+	r.x = indexA[0] == 0 || indexB[0] == 0 ? 1.0f : 0.0f;+	r.y = indexA[1] == 0 || indexB[1] == 0 ? 1.0f : 0.0f;+	r.z = indexA[2] == 0 || indexB[2] == 0 ? 1.0f : 0.0f;+	r.w = indexA[3] == 0 || indexB[3] == 0 ? 1.0f : 0.0f;+	return r;+}+#endif++// a - b+static inline b2Vec2W b2SubVW( b2Vec2W a, b2Vec2W b )+{+	return (b2Vec2W){+		.X = b2SubW( a.X, b.X ),+		.Y = b2SubW( a.Y, b.Y ),+	};+}++static inline b2Vec2W b2RightPerpW( b2Vec2W a )+{+	return (b2Vec2W){ .X = a.Y, .Y = b2NegW( a.X ) };+}++static inline b2FloatW b2DotW( b2Vec2W a, b2Vec2W b )+{+	return b2AddW( b2MulW( a.X, b.X ), b2MulW( a.Y, b.Y ) );+}++static inline b2FloatW b2CrossW( b2Vec2W a, b2Vec2W b )+{+	return b2SubW( b2MulW( a.X, b.Y ), b2MulW( a.Y, b.X ) );+}++static inline b2Vec2W b2RotateVectorW( b2RotW q, b2Vec2W v )+{+	return (b2Vec2W){ b2SubW( b2MulW( q.C, v.X ), b2MulW( q.S, v.Y ) ), b2AddW( b2MulW( q.S, v.X ), b2MulW( q.C, v.Y ) ) };+}++// 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 b2ContactConstraintWide+{+	// Read common+	int indexA[B2_SIMD_WIDTH];+	int indexB[B2_SIMD_WIDTH];++	b2FloatW invMassA, invMassB;+	b2FloatW invIA, invIB;+	b2Vec2W normal;+	b2Vec2W anchorA1, anchorB1;+	b2Vec2W anchorA2, anchorB2;+	b2FloatW normalMass1, normalMass2;+	b2FloatW baseSeparation1;+	b2FloatW baseSeparation2;++	// Write in push and solve+	b2FloatW normalImpulse1, normalImpulse2;+	b2FloatW totalNormalImpulse1, totalNormalImpulse2;++	// Write in solve+	b2FloatW tangentImpulse1, tangentImpulse2;+	b2FloatW rollingImpulse;++	// Read in solve+	b2FloatW friction;+	b2FloatW tangentSpeed;+	b2FloatW rollingResistance;+	b2FloatW tangentMass1, tangentMass2;+	b2FloatW restitution;+	b2FloatW relativeVelocity1, relativeVelocity2;+	b2FloatW restitutionImpulse1, restitutionImpulse2;++} b2ContactConstraintWide;++int B2_WIDE( b2GetWideContactConstraintByteCount )( void )+{+	return sizeof( b2ContactConstraintWide );+}++// Wide version of b2BodyState.+typedef struct b2BodyStateW+{+	b2Vec2W v;+	b2FloatW w;+	b2FloatW flags;+	b2Vec2W dp;+	b2RotW dq;+} b2BodyStateW;++// Custom gather/scatter for each SIMD type.+#if B2_SIMD_WIDTH == 8++// This is a load and 8x8 transpose+B2_FORCE_INLINE b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices )+{+	_Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" );+	B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 );++	// zero means null+	int i1 = indices[0] - 1;+	int i2 = indices[1] - 1;+	int i3 = indices[2] - 1;+	int i4 = indices[3] - 1;+	int i5 = indices[4] - 1;+	int i6 = indices[5] - 1;+	int i7 = indices[6] - 1;+	int i8 = indices[7] - 1;++	// b2BodyState b2_identityBodyState = {{0.0f, 0.0f}, 0.0f, 0, {0.0f, 0.0f}, {1.0f, 0.0f}};+	b2FloatW identity = _mm256_setr_ps( 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, 1.0f, 0.0f );+	b2FloatW b0 = i1 == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + i1 ) );+	b2FloatW b1 = i2 == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + i2 ) );+	b2FloatW b2 = i3 == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + i3 ) );+	b2FloatW b3 = i4 == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + i4 ) );+	b2FloatW b4 = i5 == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + i5 ) );+	b2FloatW b5 = i6 == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + i6 ) );+	b2FloatW b6 = i7 == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + i7 ) );+	b2FloatW b7 = i8 == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + i8 ) );++	b2FloatW t0 = _mm256_unpacklo_ps( b0, b1 );+	b2FloatW t1 = _mm256_unpackhi_ps( b0, b1 );+	b2FloatW t2 = _mm256_unpacklo_ps( b2, b3 );+	b2FloatW t3 = _mm256_unpackhi_ps( b2, b3 );+	b2FloatW t4 = _mm256_unpacklo_ps( b4, b5 );+	b2FloatW t5 = _mm256_unpackhi_ps( b4, b5 );+	b2FloatW t6 = _mm256_unpacklo_ps( b6, b7 );+	b2FloatW t7 = _mm256_unpackhi_ps( b6, b7 );+	b2FloatW tt0 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 1, 0, 1, 0 ) );+	b2FloatW tt1 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 3, 2, 3, 2 ) );+	b2FloatW tt2 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) );+	b2FloatW tt3 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) );+	b2FloatW tt4 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 1, 0, 1, 0 ) );+	b2FloatW tt5 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 3, 2, 3, 2 ) );+	b2FloatW tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) );+	b2FloatW tt7 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) );++	b2BodyStateW simdBody;+	simdBody.v.X = _mm256_permute2f128_ps( tt0, tt4, 0x20 );+	simdBody.v.Y = _mm256_permute2f128_ps( tt1, tt5, 0x20 );+	simdBody.w = _mm256_permute2f128_ps( tt2, tt6, 0x20 );+	simdBody.flags = _mm256_permute2f128_ps( tt3, tt7, 0x20 );+	simdBody.dp.X = _mm256_permute2f128_ps( tt0, tt4, 0x31 );+	simdBody.dp.Y = _mm256_permute2f128_ps( tt1, tt5, 0x31 );+	simdBody.dq.C = _mm256_permute2f128_ps( tt2, tt6, 0x31 );+	simdBody.dq.S = _mm256_permute2f128_ps( tt3, tt7, 0x31 );+	return simdBody;+}++// This writes everything back to the solver bodies but only the velocities change+B2_FORCE_INLINE void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices,+									  const b2BodyStateW* B2_RESTRICT simdBody )+{+	_Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" );+	B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 );+	b2FloatW t0 = _mm256_unpacklo_ps( simdBody->v.X, simdBody->v.Y );+	b2FloatW t1 = _mm256_unpackhi_ps( simdBody->v.X, simdBody->v.Y );+	b2FloatW t2 = _mm256_unpacklo_ps( simdBody->w, simdBody->flags );+	b2FloatW t3 = _mm256_unpackhi_ps( simdBody->w, simdBody->flags );+	b2FloatW t4 = _mm256_unpacklo_ps( simdBody->dp.X, simdBody->dp.Y );+	b2FloatW t5 = _mm256_unpackhi_ps( simdBody->dp.X, simdBody->dp.Y );+	b2FloatW t6 = _mm256_unpacklo_ps( simdBody->dq.C, simdBody->dq.S );+	b2FloatW t7 = _mm256_unpackhi_ps( simdBody->dq.C, simdBody->dq.S );+	b2FloatW tt0 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 1, 0, 1, 0 ) );+	b2FloatW tt1 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 3, 2, 3, 2 ) );+	b2FloatW tt2 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) );+	b2FloatW tt3 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) );+	b2FloatW tt4 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 1, 0, 1, 0 ) );+	b2FloatW tt5 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 3, 2, 3, 2 ) );+	b2FloatW tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) );+	b2FloatW tt7 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) );++	_Static_assert( b2_dynamicFlag == ( 1 << 9 ), "dynamic flag must be bit 9" );+	int dynamicMask =+		_mm256_movemask_ps( _mm256_castsi256_ps( _mm256_slli_epi32( _mm256_castps_si256( simdBody->flags ), 31 - 9 ) ) );++	// I don't use any dummy body in the body array because this will lead to multithreaded sharing and the+	// associated cache flushing.++	// zero means null+	int i1 = indices[0] - 1;+	int i2 = indices[1] - 1;+	int i3 = indices[2] - 1;+	int i4 = indices[3] - 1;+	int i5 = indices[4] - 1;+	int i6 = indices[5] - 1;+	int i7 = indices[6] - 1;+	int i8 = indices[7] - 1;++	if ( dynamicMask & 1 )+	{+		_mm256_store_ps( (float*)( states + i1 ), _mm256_permute2f128_ps( tt0, tt4, 0x20 ) );+	}+	+	if ( dynamicMask & 2 )+	{+		_mm256_store_ps( (float*)( states + i2 ), _mm256_permute2f128_ps( tt1, tt5, 0x20 ) );+	}++	if ( dynamicMask & 4 )+	{+		_mm256_store_ps( (float*)( states + i3 ), _mm256_permute2f128_ps( tt2, tt6, 0x20 ) );+	}++	if ( dynamicMask & 8 )+	{+		_mm256_store_ps( (float*)( states + i4 ), _mm256_permute2f128_ps( tt3, tt7, 0x20 ) );+	}++	if ( dynamicMask & 16 )+	{+		_mm256_store_ps( (float*)( states + i5 ), _mm256_permute2f128_ps( tt0, tt4, 0x31 ) );+	}++	if ( dynamicMask & 32 )+	{+		_mm256_store_ps( (float*)( states + i6 ), _mm256_permute2f128_ps( tt1, tt5, 0x31 ) );+	}++	if ( dynamicMask & 64 )+	{+		_mm256_store_ps( (float*)( states + i7 ), _mm256_permute2f128_ps( tt2, tt6, 0x31 ) );+	}++	if ( dynamicMask & 128 )+	{+		_mm256_store_ps( (float*)( states + i8 ), _mm256_permute2f128_ps( tt3, tt7, 0x31 ) );+	}+}++#elif defined( B2_SIMD_NEON )++// This is a load and transpose+B2_FORCE_INLINE b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices )+{+	_Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" );+	B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 );++	// [vx vy w flags]+	b2FloatW identityA = b2ZeroW();++	// [dpx dpy dqc dqs]++	b2FloatW identityB = b2SetW( 0.0f, 0.0f, 1.0f, 0.0f );++	// zero means null+	int i1 = indices[0] - 1;+	int i2 = indices[1] - 1;+	int i3 = indices[2] - 1;+	int i4 = indices[3] - 1;++	b2FloatW b1a = i1 == B2_NULL_INDEX ? identityA : vld1q_f32( (float*)( states + i1 ) + 0 );+	b2FloatW b1b = i1 == B2_NULL_INDEX ? identityB : vld1q_f32( (float*)( states + i1 ) + 4 );+	b2FloatW b2a = i2 == B2_NULL_INDEX ? identityA : vld1q_f32( (float*)( states + i2 ) + 0 );+	b2FloatW b2b = i2 == B2_NULL_INDEX ? identityB : vld1q_f32( (float*)( states + i2 ) + 4 );+	b2FloatW b3a = i3 == B2_NULL_INDEX ? identityA : vld1q_f32( (float*)( states + i3 ) + 0 );+	b2FloatW b3b = i3 == B2_NULL_INDEX ? identityB : vld1q_f32( (float*)( states + i3 ) + 4 );+	b2FloatW b4a = i4 == B2_NULL_INDEX ? identityA : vld1q_f32( (float*)( states + i4 ) + 0 );+	b2FloatW b4b = i4 == B2_NULL_INDEX ? identityB : vld1q_f32( (float*)( states + i4 ) + 4 );++	// [vx1 vx3 vy1 vy3]+	b2FloatW t1a = b2UnpackLoW( b1a, b3a );++	// [vx2 vx4 vy2 vy4]+	b2FloatW t2a = b2UnpackLoW( b2a, b4a );++	// [w1 w3 f1 f3]+	b2FloatW t3a = b2UnpackHiW( b1a, b3a );++	// [w2 w4 f2 f4]+	b2FloatW t4a = b2UnpackHiW( b2a, b4a );++	b2BodyStateW simdBody;+	simdBody.v.X = b2UnpackLoW( t1a, t2a );+	simdBody.v.Y = b2UnpackHiW( t1a, t2a );+	simdBody.w = b2UnpackLoW( t3a, t4a );+	simdBody.flags = b2UnpackHiW( t3a, t4a );++	b2FloatW t1b = b2UnpackLoW( b1b, b3b );+	b2FloatW t2b = b2UnpackLoW( b2b, b4b );+	b2FloatW t3b = b2UnpackHiW( b1b, b3b );+	b2FloatW t4b = b2UnpackHiW( b2b, b4b );++	simdBody.dp.X = b2UnpackLoW( t1b, t2b );+	simdBody.dp.Y = b2UnpackHiW( t1b, t2b );+	simdBody.dq.C = b2UnpackLoW( t3b, t4b );+	simdBody.dq.S = b2UnpackHiW( t3b, t4b );++	return simdBody;+}++// This writes only the velocities back to the solver bodies+// https://developer.arm.com/documentation/102107a/0100/Floating-point-4x4-matrix-transposition+B2_FORCE_INLINE void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices,+									  const b2BodyStateW* B2_RESTRICT simdBody )+{+	_Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" );+	B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 );++	//	b2FloatW x = b2SetW(0.0f, 1.0f, 2.0f, 3.0f);+	//	b2FloatW y = b2SetW(4.0f, 5.0f, 6.0f, 7.0f);+	//	b2FloatW z = b2SetW(8.0f, 9.0f, 10.0f, 11.0f);+	//	b2FloatW w = b2SetW(12.0f, 13.0f, 14.0f, 15.0f);+	//+	//	float32x4x2_t rr1 = vtrnq_f32( x, y );+	//	float32x4x2_t rr2 = vtrnq_f32( z, w );+	//+	//	float32x4_t b1 = vcombine_f32(vget_low_f32(rr1.val[0]), vget_low_f32(rr2.val[0]));+	//	float32x4_t b2 = vcombine_f32(vget_low_f32(rr1.val[1]), vget_low_f32(rr2.val[1]));+	//	float32x4_t b3 = vcombine_f32(vget_high_f32(rr1.val[0]), vget_high_f32(rr2.val[0]));+	//	float32x4_t b4 = vcombine_f32(vget_high_f32(rr1.val[1]), vget_high_f32(rr2.val[1]));++	// transpose+	float32x4x2_t r1 = vtrnq_f32( simdBody->v.X, simdBody->v.Y );+	float32x4x2_t r2 = vtrnq_f32( simdBody->w, simdBody->flags );++	// zero means null+	int i1 = indices[0] - 1;+	int i2 = indices[1] - 1;+	int i3 = indices[2] - 1;+	int i4 = indices[3] - 1;++	// I don't use any dummy body in the body array because this will lead to multithreaded sharing and the+	// associated cache flushing.+	if ( i1 != B2_NULL_INDEX && ( states[i1].flags & b2_dynamicFlag ) != 0 )+	{+		float32x4_t body1 = vcombine_f32( vget_low_f32( r1.val[0] ), vget_low_f32( r2.val[0] ) );+		b2StoreW( (float*)( states + i1 ), body1 );+	}++	if ( i2 != B2_NULL_INDEX && ( states[i2].flags & b2_dynamicFlag ) != 0 )+	{+		float32x4_t body2 = vcombine_f32( vget_low_f32( r1.val[1] ), vget_low_f32( r2.val[1] ) );+		b2StoreW( (float*)( states + i2 ), body2 );+	}++	if ( i3 != B2_NULL_INDEX && ( states[i3].flags & b2_dynamicFlag ) != 0 )+	{+		float32x4_t body3 = vcombine_f32( vget_high_f32( r1.val[0] ), vget_high_f32( r2.val[0] ) );+		b2StoreW( (float*)( states + i3 ), body3 );+	}++	if ( i4 != B2_NULL_INDEX && ( states[i4].flags & b2_dynamicFlag ) != 0 )+	{+		float32x4_t body4 = vcombine_f32( vget_high_f32( r1.val[1] ), vget_high_f32( r2.val[1] ) );+		b2StoreW( (float*)( states + i4 ), body4 );+	}+}++#elif defined( B2_SIMD_SSE2 )++// This is a load and transpose. Force inline is a big win on MSVC, but Clang already inlined it.+B2_FORCE_INLINE b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices )+{+	_Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" );+	B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 );+	B2_VALIDATE( indices[0] >= 0 && indices[1] >= 0 && indices[2] >= 0 && indices[3] >= 0 );++	// [vx vy w flags]+	b2FloatW identityA = b2ZeroW();++	// [dpx dpy dqc dqs]+	b2FloatW identityB = b2SetW( 0.0f, 0.0f, 1.0f, 0.0f );++	// zero means null+	int i1 = indices[0] - 1;+	int i2 = indices[1] - 1;+	int i3 = indices[2] - 1;+	int i4 = indices[3] - 1;++	b2FloatW b1a = i1 == B2_NULL_INDEX ? identityA : _mm_load_ps( (float*)( states + i1 ) + 0 );+	b2FloatW b1b = i1 == B2_NULL_INDEX ? identityB : _mm_load_ps( (float*)( states + i1 ) + 4 );+	b2FloatW b2a = i2 == B2_NULL_INDEX ? identityA : _mm_load_ps( (float*)( states + i2 ) + 0 );+	b2FloatW b2b = i2 == B2_NULL_INDEX ? identityB : _mm_load_ps( (float*)( states + i2 ) + 4 );+	b2FloatW b3a = i3 == B2_NULL_INDEX ? identityA : _mm_load_ps( (float*)( states + i3 ) + 0 );+	b2FloatW b3b = i3 == B2_NULL_INDEX ? identityB : _mm_load_ps( (float*)( states + i3 ) + 4 );+	b2FloatW b4a = i4 == B2_NULL_INDEX ? identityA : _mm_load_ps( (float*)( states + i4 ) + 0 );+	b2FloatW b4b = i4 == B2_NULL_INDEX ? identityB : _mm_load_ps( (float*)( states + i4 ) + 4 );++	// [vx1 vx3 vy1 vy3]+	b2FloatW t1a = b2UnpackLoW( b1a, b3a );++	// [vx2 vx4 vy2 vy4]+	b2FloatW t2a = b2UnpackLoW( b2a, b4a );++	// [w1 w3 f1 f3]+	b2FloatW t3a = b2UnpackHiW( b1a, b3a );++	// [w2 w4 f2 f4]+	b2FloatW t4a = b2UnpackHiW( b2a, b4a );++	b2BodyStateW simdBody;+	simdBody.v.X = b2UnpackLoW( t1a, t2a );+	simdBody.v.Y = b2UnpackHiW( t1a, t2a );+	simdBody.w = b2UnpackLoW( t3a, t4a );+	simdBody.flags = b2UnpackHiW( t3a, t4a );++	b2FloatW t1b = b2UnpackLoW( b1b, b3b );+	b2FloatW t2b = b2UnpackLoW( b2b, b4b );+	b2FloatW t3b = b2UnpackHiW( b1b, b3b );+	b2FloatW t4b = b2UnpackHiW( b2b, b4b );++	simdBody.dp.X = b2UnpackLoW( t1b, t2b );+	simdBody.dp.Y = b2UnpackHiW( t1b, t2b );+	simdBody.dq.C = b2UnpackLoW( t3b, t4b );+	simdBody.dq.S = b2UnpackHiW( t3b, t4b );++	return simdBody;+}++// This writes only the velocities back to the solver bodies. Force inline is a big win on MSVC, but Clang already inlined it.+B2_FORCE_INLINE void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices,+									  const b2BodyStateW* B2_RESTRICT simdBody )+{+	_Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" );+	B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 );+	B2_VALIDATE( indices[0] >= 0 && indices[1] >= 0 && indices[2] >= 0 && indices[3] >= 0 );++	// [vx1 vy1 vx2 vy2]+	b2FloatW t1 = b2UnpackLoW( simdBody->v.X, simdBody->v.Y );+	// [vx3 vy3 vx4 vy4]+	b2FloatW t2 = b2UnpackHiW( simdBody->v.X, simdBody->v.Y );+	// [w1 f1 w2 f2]+	b2FloatW t3 = b2UnpackLoW( simdBody->w, simdBody->flags );+	// [w3 f3 w4 f4]+	b2FloatW t4 = b2UnpackHiW( simdBody->w, simdBody->flags );++	_Static_assert( b2_dynamicFlag == ( 1 << 9 ), "dynamic flag must be bit 9" );+	int dynamicMask = _mm_movemask_ps( _mm_castsi128_ps( _mm_slli_epi32( _mm_castps_si128( simdBody->flags ), 31 - 9 ) ) );++	// zero means null+	int i1 = indices[0] - 1;+	int i2 = indices[1] - 1;+	int i3 = indices[2] - 1;+	int i4 = indices[3] - 1;++	// I don't use any dummy body in the body array because this will lead to multithreaded cache coherence problems.+	if ( dynamicMask & 1 )+	{+		// [t1.x t1.y t3.x t3.y]+		b2StoreW( (float*)( states + i1 ), _mm_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) ) );+	}++	if ( dynamicMask & 2 )+	{+		// [t1.z t1.w t3.z t3.w]+		b2StoreW( (float*)( states + i2 ), _mm_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) ) );+	}++	if ( dynamicMask & 4 )+	{+		// [t2.x t2.y t4.x t4.y]+		b2StoreW( (float*)( states + i3 ), _mm_shuffle_ps( t2, t4, _MM_SHUFFLE( 1, 0, 1, 0 ) ) );+	}++	if ( dynamicMask & 8 )+	{+		// [t2.z t2.w t4.z t4.w]+		b2StoreW( (float*)( states + i4 ), _mm_shuffle_ps( t2, t4, _MM_SHUFFLE( 3, 2, 3, 2 ) ) );+	}+}++#else++// This is a load and transpose+B2_FORCE_INLINE b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices )+{+	B2_VALIDATE( indices[0] >= 0 && indices[1] >= 0 && indices[2] >= 0 && indices[3] >= 0 );++	b2BodyState identity = b2_identityBodyState;++	// zero means null+	int i1 = indices[0] - 1;+	int i2 = indices[1] - 1;+	int i3 = indices[2] - 1;+	int i4 = indices[3] - 1;++	b2BodyState s1 = i1 == B2_NULL_INDEX ? identity : states[i1];+	b2BodyState s2 = i2 == B2_NULL_INDEX ? identity : states[i2];+	b2BodyState s3 = i3 == B2_NULL_INDEX ? identity : states[i3];+	b2BodyState s4 = i4 == B2_NULL_INDEX ? identity : states[i4];++	b2BodyStateW simdBody;+	simdBody.v.X = (b2FloatW){ s1.linearVelocity.x, s2.linearVelocity.x, s3.linearVelocity.x, s4.linearVelocity.x };+	simdBody.v.Y = (b2FloatW){ s1.linearVelocity.y, s2.linearVelocity.y, s3.linearVelocity.y, s4.linearVelocity.y };+	simdBody.w = (b2FloatW){ s1.angularVelocity, s2.angularVelocity, s3.angularVelocity, s4.angularVelocity };+	simdBody.flags = (b2FloatW){ (float)s1.flags, (float)s2.flags, (float)s3.flags, (float)s4.flags };+	simdBody.dp.X = (b2FloatW){ s1.deltaPosition.x, s2.deltaPosition.x, s3.deltaPosition.x, s4.deltaPosition.x };+	simdBody.dp.Y = (b2FloatW){ s1.deltaPosition.y, s2.deltaPosition.y, s3.deltaPosition.y, s4.deltaPosition.y };+	simdBody.dq.C = (b2FloatW){ s1.deltaRotation.c, s2.deltaRotation.c, s3.deltaRotation.c, s4.deltaRotation.c };+	simdBody.dq.S = (b2FloatW){ s1.deltaRotation.s, s2.deltaRotation.s, s3.deltaRotation.s, s4.deltaRotation.s };++	return simdBody;+}++// This writes only the velocities back to the solver bodies+B2_FORCE_INLINE void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices,+									  const b2BodyStateW* B2_RESTRICT simdBody )+{+	B2_VALIDATE( indices[0] >= 0 && indices[1] >= 0 && indices[2] >= 0 && indices[3] >= 0 );++	// zero means null+	int i1 = indices[0] - 1;+	int i2 = indices[1] - 1;+	int i3 = indices[2] - 1;+	int i4 = indices[3] - 1;++	if ( i1 != B2_NULL_INDEX && ( states[i1].flags & b2_dynamicFlag ) != 0 )+	{+		b2BodyState* state = states + i1;+		state->linearVelocity.x = simdBody->v.X.x;+		state->linearVelocity.y = simdBody->v.Y.x;+		state->angularVelocity = simdBody->w.x;+	}++	if ( i2 != B2_NULL_INDEX && ( states[i2].flags & b2_dynamicFlag ) != 0 )+	{+		b2BodyState* state = states + i2;+		state->linearVelocity.x = simdBody->v.X.y;+		state->linearVelocity.y = simdBody->v.Y.y;+		state->angularVelocity = simdBody->w.y;+	}++	if ( i3 != B2_NULL_INDEX && ( states[i3].flags & b2_dynamicFlag ) != 0 )+	{+		b2BodyState* state = states + i3;+		state->linearVelocity.x = simdBody->v.X.z;+		state->linearVelocity.y = simdBody->v.Y.z;+		state->angularVelocity = simdBody->w.z;+	}++	if ( i4 != B2_NULL_INDEX && ( states[i4].flags & b2_dynamicFlag ) != 0 )+	{+		b2BodyState* state = states + i4;+		state->linearVelocity.x = simdBody->v.X.w;+		state->linearVelocity.y = simdBody->v.Y.w;+		state->angularVelocity = simdBody->w.w;+	}+}++#endif++static b2ContactSim b2_zeroContactSim = { 0 };++// Macro for gathering contact data across lanes.+#if B2_SIMD_WIDTH == 4+#define B2_GATHER_LANES( wide, scalar )                                                                                          \+	wide = b2SetW( contactLanes[0]->scalar, contactLanes[1]->scalar, contactLanes[2]->scalar, contactLanes[3]->scalar )+#elif B2_SIMD_WIDTH == 8+#define B2_GATHER_LANES( wide, scalar )                                                                                          \+	wide = b2SetW( contactLanes[0]->scalar, contactLanes[1]->scalar, contactLanes[2]->scalar, contactLanes[3]->scalar,           \+				   contactLanes[4]->scalar, contactLanes[5]->scalar, contactLanes[6]->scalar, contactLanes[7]->scalar )+#else+#error "Unsupported B2_SIMD_WIDTH"+#endif++// Layout needed for transpose gathers+_Static_assert( offsetof( b2ManifoldPoint, anchorA ) == 0, "manifold point layout" );+_Static_assert( offsetof( b2ManifoldPoint, anchorB ) == 8, "manifold point layout" );+_Static_assert( offsetof( b2ManifoldPoint, separation ) == 16, "manifold point layout" );+_Static_assert( offsetof( b2ManifoldPoint, normalImpulse ) == 20, "manifold point layout" );+_Static_assert( offsetof( b2ManifoldPoint, tangentImpulse ) == 24, "manifold point layout" );+_Static_assert( sizeof( b2ManifoldPoint ) >= 32, "manifold point layout" );++// Prepare wide contact constraints.+// Potential optimization: for SSE2 load 4 manifold points and transpose, like the body gather/scatter.+// I would need to reorder the manifold point so that constraint data comes first.+void B2_WIDE( b2PrepareContacts_Wide )( b2SolverBlock block, b2StepContext* context )+{+	b2TracyCZoneNC( prepare_contact, "Prepare Contact", b2_colorYellow, true );+	b2World* world = context->world;+	b2BodyState* states = context->states;+	b2ContactPrepareSpan* spans = context->contactPrepareSpans;+	b2ContactConstraintWide* wideBase = context->wideContactConstraints;++#if B2_ENABLE_VALIDATION+	b2Body* bodies = world->bodies.data;+#endif++	b2FloatW zeroW = b2ZeroW();+	b2FloatW oneW = b2SplatW( 1.0f );+	b2FloatW warmStartScale = world->enableWarmStarting ? oneW : zeroW;+	bool anyRestitution = false;++	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 = b2MinInt( spans[colorIndex + 1].start, endWideIndex );+		int colorContactCount = spans[colorIndex].count;+		b2ContactSim* contactSims = spans[colorIndex].contacts;++#if B2_ENABLE_VALIDATION+		int expectedWide = colorContactCount > 0 ? ( ( colorContactCount - 1 ) >> B2_SIMD_SHIFT ) + 1 : 0;+		B2_ASSERT( spans[colorIndex + 1].start - spans[colorIndex].start == expectedWide );+#endif++		// Loop over color+		for ( ; wideIndex < colorWideEndIndex; ++wideIndex )+		{+			b2ContactConstraintWide* cw = wideBase + wideIndex;+			int localWideIndex = wideIndex - colorWideStart;++			b2ContactSim* contactLanes[B2_SIMD_WIDTH];+			uint32_t hitEvents = 0;++			for ( int laneIndex = 0; laneIndex < B2_SIMD_WIDTH; ++laneIndex )+			{+				int contactIndex = B2_SIMD_WIDTH * localWideIndex + laneIndex;+				if ( contactIndex < colorContactCount )+				{+					b2ContactSim* c = contactSims + contactIndex;+					contactLanes[laneIndex] = c;++					// index base-1+					cw->indexA[laneIndex] = b2DecodeAwakeIndex( c->encodedBodySimA ) + 1;+					cw->indexB[laneIndex] = b2DecodeAwakeIndex( c->encodedBodySimB ) + 1;+					hitEvents |= ( c->simFlags & b2_simEnableHitEvent );++#if B2_ENABLE_VALIDATION+					b2Body* bodyA = bodies + c->bodyIdA;+					b2Body* bodyB = bodies + c->bodyIdB;+					B2_ASSERT( c->encodedBodySimA == b2EncodeBodySimIndex( bodyA ) );+					B2_ASSERT( c->encodedBodySimB == b2EncodeBodySimIndex( bodyB ) );+#endif+				}+				else+				{+					contactLanes[laneIndex] = &b2_zeroContactSim;+				}+			}++			B2_GATHER_LANES( cw->invMassA, invMassA );+			B2_GATHER_LANES( cw->invMassB, invMassB );+			B2_GATHER_LANES( cw->invIA, invIA );+			B2_GATHER_LANES( cw->invIB, invIB );+			B2_GATHER_LANES( cw->normal.X, manifold.normal.x );+			B2_GATHER_LANES( cw->normal.Y, manifold.normal.y );+			B2_GATHER_LANES( cw->friction, friction );+			B2_GATHER_LANES( cw->tangentSpeed, tangentSpeed );+			B2_GATHER_LANES( cw->rollingResistance, rollingResistance );+			B2_GATHER_LANES( cw->rollingImpulse, manifold.rollingImpulse );+			cw->rollingImpulse = b2MulW( warmStartScale, cw->rollingImpulse );++			B2_GATHER_LANES( cw->restitution, restitution );++			b2Vec2W tangent = b2RightPerpW( cw->normal );++			b2FloatW pointCounts;+#if B2_SIMD_WIDTH == 4+			pointCounts = b2SetW( (float)contactLanes[0]->manifold.pointCount, (float)contactLanes[1]->manifold.pointCount,+								  (float)contactLanes[2]->manifold.pointCount, (float)contactLanes[3]->manifold.pointCount );+#elif B2_SIMD_WIDTH == 8+			pointCounts = b2SetW( (float)contactLanes[0]->manifold.pointCount, (float)contactLanes[1]->manifold.pointCount,+								  (float)contactLanes[2]->manifold.pointCount, (float)contactLanes[3]->manifold.pointCount,+								  (float)contactLanes[4]->manifold.pointCount, (float)contactLanes[5]->manifold.pointCount,+								  (float)contactLanes[6]->manifold.pointCount, (float)contactLanes[7]->manifold.pointCount );+#endif+			b2FloatW twoPointMask = b2GreaterThanW( pointCounts, oneW );++			{+				// Optimized transpose gather. Similar to b2GatherBodies.+#if B2_SIMD_WIDTH == 4+				b2FloatW m1a = b2LoadU( (float*)( contactLanes[0]->manifold.points + 0 ) + 0 );+				b2FloatW m1b = b2LoadU( (float*)( contactLanes[0]->manifold.points + 0 ) + 4 );+				b2FloatW m2a = b2LoadU( (float*)( contactLanes[1]->manifold.points + 0 ) + 0 );+				b2FloatW m2b = b2LoadU( (float*)( contactLanes[1]->manifold.points + 0 ) + 4 );+				b2FloatW m3a = b2LoadU( (float*)( contactLanes[2]->manifold.points + 0 ) + 0 );+				b2FloatW m3b = b2LoadU( (float*)( contactLanes[2]->manifold.points + 0 ) + 4 );+				b2FloatW m4a = b2LoadU( (float*)( contactLanes[3]->manifold.points + 0 ) + 0 );+				b2FloatW m4b = b2LoadU( (float*)( contactLanes[3]->manifold.points + 0 ) + 4 );++				b2FloatW t1a = b2UnpackLoW( m1a, m3a );+				b2FloatW t2a = b2UnpackLoW( m2a, m4a );+				b2FloatW t3a = b2UnpackHiW( m1a, m3a );+				b2FloatW t4a = b2UnpackHiW( m2a, m4a );+				b2FloatW t1b = b2UnpackLoW( m1b, m3b );+				b2FloatW t2b = b2UnpackLoW( m2b, m4b );+				b2FloatW t3b = b2UnpackHiW( m1b, m3b );+				b2FloatW t4b = b2UnpackHiW( m2b, m4b );++				cw->anchorA1.X = b2UnpackLoW( t1a, t2a );+				cw->anchorA1.Y = b2UnpackHiW( t1a, t2a );+				cw->anchorB1.X = b2UnpackLoW( t3a, t4a );+				cw->anchorB1.Y = b2UnpackHiW( t3a, t4a );+				cw->baseSeparation1 = b2UnpackLoW( t1b, t2b );+				cw->normalImpulse1 = b2UnpackHiW( t1b, t2b );+				cw->tangentImpulse1 = b2UnpackLoW( t3b, t4b );+#elif B2_SIMD_WIDTH == 8+				b2FloatW m1 = b2LoadU( (float*)( contactLanes[0]->manifold.points + 0 ) );+				b2FloatW m2 = b2LoadU( (float*)( contactLanes[1]->manifold.points + 0 ) );+				b2FloatW m3 = b2LoadU( (float*)( contactLanes[2]->manifold.points + 0 ) );+				b2FloatW m4 = b2LoadU( (float*)( contactLanes[3]->manifold.points + 0 ) );+				b2FloatW m5 = b2LoadU( (float*)( contactLanes[4]->manifold.points + 0 ) );+				b2FloatW m6 = b2LoadU( (float*)( contactLanes[5]->manifold.points + 0 ) );+				b2FloatW m7 = b2LoadU( (float*)( contactLanes[6]->manifold.points + 0 ) );+				b2FloatW m8 = b2LoadU( (float*)( contactLanes[7]->manifold.points + 0 ) );++				b2FloatW t1 = _mm256_unpacklo_ps( m1, m2 );+				b2FloatW t2 = _mm256_unpackhi_ps( m1, m2 );+				b2FloatW t3 = _mm256_unpacklo_ps( m3, m4 );+				b2FloatW t4 = _mm256_unpackhi_ps( m3, m4 );+				b2FloatW t5 = _mm256_unpacklo_ps( m5, m6 );+				b2FloatW t6 = _mm256_unpackhi_ps( m5, m6 );+				b2FloatW t7 = _mm256_unpacklo_ps( m7, m8 );+				b2FloatW t8 = _mm256_unpackhi_ps( m7, m8 );+				b2FloatW tt1 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) );+				b2FloatW tt2 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) );+				b2FloatW tt3 = _mm256_shuffle_ps( t2, t4, _MM_SHUFFLE( 1, 0, 1, 0 ) );+				b2FloatW tt4 = _mm256_shuffle_ps( t2, t4, _MM_SHUFFLE( 3, 2, 3, 2 ) );+				b2FloatW tt5 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) );+				b2FloatW tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) );+				b2FloatW tt7 = _mm256_shuffle_ps( t6, t8, _MM_SHUFFLE( 1, 0, 1, 0 ) );+				b2FloatW tt8 = _mm256_shuffle_ps( t6, t8, _MM_SHUFFLE( 3, 2, 3, 2 ) );++				cw->anchorA1.X = _mm256_permute2f128_ps( tt1, tt5, 0x20 );+				cw->anchorA1.Y = _mm256_permute2f128_ps( tt2, tt6, 0x20 );+				cw->anchorB1.X = _mm256_permute2f128_ps( tt3, tt7, 0x20 );+				cw->anchorB1.Y = _mm256_permute2f128_ps( tt4, tt8, 0x20 );+				cw->baseSeparation1 = _mm256_permute2f128_ps( tt1, tt5, 0x31 );+				cw->normalImpulse1 = _mm256_permute2f128_ps( tt2, tt6, 0x31 );+				cw->tangentImpulse1 = _mm256_permute2f128_ps( tt3, tt7, 0x31 );+#endif++				// separation1 = cw->baseSeparation1;++				b2FloatW offset = b2DotW( b2SubVW( cw->anchorB1, cw->anchorA1 ), cw->normal );+				cw->baseSeparation1 = b2SubW( cw->baseSeparation1, offset );+				cw->normalImpulse1 = b2MulW( warmStartScale, cw->normalImpulse1 );+				cw->tangentImpulse1 = b2MulW( warmStartScale, cw->tangentImpulse1 );+				cw->totalNormalImpulse1 = zeroW;+				cw->relativeVelocity1 = zeroW;+				cw->restitutionImpulse1 = zeroW;++				// Effective mass along normal.+				{+					b2FloatW rnA = b2CrossW( cw->anchorA1, cw->normal );+					b2FloatW rnB = b2CrossW( cw->anchorB1, cw->normal );+					b2FloatW k = b2AddW( cw->invMassA, cw->invMassB );+					k = b2MulAddW( k, cw->invIA, b2MulW( rnA, rnA ) );+					k = b2MulAddW( k, cw->invIB, b2MulW( rnB, rnB ) );+					b2FloatW test = b2GreaterThanW( k, b2ZeroW() );+					cw->normalMass1 = b2BlendW( b2ZeroW(), b2DivW( oneW, k ), test );+				}++				// Effective mass along tangent.+				{+					b2FloatW rtA = b2CrossW( cw->anchorA1, tangent );+					b2FloatW rtB = b2CrossW( cw->anchorB1, tangent );+					b2FloatW k = b2AddW( cw->invMassA, cw->invMassB );+					k = b2MulAddW( k, cw->invIA, b2MulW( rtA, rtA ) );+					k = b2MulAddW( k, cw->invIB, b2MulW( rtB, rtB ) );+					b2FloatW test = b2GreaterThanW( k, b2ZeroW() );+					cw->tangentMass1 = b2BlendW( b2ZeroW(), b2DivW( oneW, k ), test );+				}+			}++			{+#if B2_SIMD_WIDTH == 4+				b2FloatW m1a = b2LoadU( (float*)( contactLanes[0]->manifold.points + 1 ) + 0 );+				b2FloatW m1b = b2LoadU( (float*)( contactLanes[0]->manifold.points + 1 ) + 4 );+				b2FloatW m2a = b2LoadU( (float*)( contactLanes[1]->manifold.points + 1 ) + 0 );+				b2FloatW m2b = b2LoadU( (float*)( contactLanes[1]->manifold.points + 1 ) + 4 );+				b2FloatW m3a = b2LoadU( (float*)( contactLanes[2]->manifold.points + 1 ) + 0 );+				b2FloatW m3b = b2LoadU( (float*)( contactLanes[2]->manifold.points + 1 ) + 4 );+				b2FloatW m4a = b2LoadU( (float*)( contactLanes[3]->manifold.points + 1 ) + 0 );+				b2FloatW m4b = b2LoadU( (float*)( contactLanes[3]->manifold.points + 1 ) + 4 );++				b2FloatW t1a = b2UnpackLoW( m1a, m3a );+				b2FloatW t2a = b2UnpackLoW( m2a, m4a );+				b2FloatW t3a = b2UnpackHiW( m1a, m3a );+				b2FloatW t4a = b2UnpackHiW( m2a, m4a );+				b2FloatW t1b = b2UnpackLoW( m1b, m3b );+				b2FloatW t2b = b2UnpackLoW( m2b, m4b );+				b2FloatW t3b = b2UnpackHiW( m1b, m3b );+				b2FloatW t4b = b2UnpackHiW( m2b, m4b );++				cw->anchorA2.X = b2UnpackLoW( t1a, t2a );+				cw->anchorA2.Y = b2UnpackHiW( t1a, t2a );+				cw->anchorB2.X = b2UnpackLoW( t3a, t4a );+				cw->anchorB2.Y = b2UnpackHiW( t3a, t4a );+				cw->baseSeparation2 = b2UnpackLoW( t1b, t2b );+				cw->normalImpulse2 = b2UnpackHiW( t1b, t2b );+				cw->tangentImpulse2 = b2UnpackLoW( t3b, t4b );+#elif B2_SIMD_WIDTH == 8+				b2FloatW m1 = b2LoadU( (float*)( contactLanes[0]->manifold.points + 1 ) );+				b2FloatW m2 = b2LoadU( (float*)( contactLanes[1]->manifold.points + 1 ) );+				b2FloatW m3 = b2LoadU( (float*)( contactLanes[2]->manifold.points + 1 ) );+				b2FloatW m4 = b2LoadU( (float*)( contactLanes[3]->manifold.points + 1 ) );+				b2FloatW m5 = b2LoadU( (float*)( contactLanes[4]->manifold.points + 1 ) );+				b2FloatW m6 = b2LoadU( (float*)( contactLanes[5]->manifold.points + 1 ) );+				b2FloatW m7 = b2LoadU( (float*)( contactLanes[6]->manifold.points + 1 ) );+				b2FloatW m8 = b2LoadU( (float*)( contactLanes[7]->manifold.points + 1 ) );++				b2FloatW t1 = _mm256_unpacklo_ps( m1, m2 );+				b2FloatW t2 = _mm256_unpackhi_ps( m1, m2 );+				b2FloatW t3 = _mm256_unpacklo_ps( m3, m4 );+				b2FloatW t4 = _mm256_unpackhi_ps( m3, m4 );+				b2FloatW t5 = _mm256_unpacklo_ps( m5, m6 );+				b2FloatW t6 = _mm256_unpackhi_ps( m5, m6 );+				b2FloatW t7 = _mm256_unpacklo_ps( m7, m8 );+				b2FloatW t8 = _mm256_unpackhi_ps( m7, m8 );+				b2FloatW tt1 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) );+				b2FloatW tt2 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) );+				b2FloatW tt3 = _mm256_shuffle_ps( t2, t4, _MM_SHUFFLE( 1, 0, 1, 0 ) );+				b2FloatW tt4 = _mm256_shuffle_ps( t2, t4, _MM_SHUFFLE( 3, 2, 3, 2 ) );+				b2FloatW tt5 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) );+				b2FloatW tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) );+				b2FloatW tt7 = _mm256_shuffle_ps( t6, t8, _MM_SHUFFLE( 1, 0, 1, 0 ) );+				b2FloatW tt8 = _mm256_shuffle_ps( t6, t8, _MM_SHUFFLE( 3, 2, 3, 2 ) );++				cw->anchorA2.X = _mm256_permute2f128_ps( tt1, tt5, 0x20 );+				cw->anchorA2.Y = _mm256_permute2f128_ps( tt2, tt6, 0x20 );+				cw->anchorB2.X = _mm256_permute2f128_ps( tt3, tt7, 0x20 );+				cw->anchorB2.Y = _mm256_permute2f128_ps( tt4, tt8, 0x20 );+				cw->baseSeparation2 = _mm256_permute2f128_ps( tt1, tt5, 0x31 );+				cw->normalImpulse2 = _mm256_permute2f128_ps( tt2, tt6, 0x31 );+				cw->tangentImpulse2 = _mm256_permute2f128_ps( tt3, tt7, 0x31 );+#endif++				// separation2 = cw->baseSeparation2;++				b2FloatW offset = b2DotW( b2SubVW( cw->anchorB2, cw->anchorA2 ), cw->normal );+				cw->baseSeparation2 = b2SubW( cw->baseSeparation2, offset );+				cw->normalImpulse2 = b2MulW( warmStartScale, cw->normalImpulse2 );+				cw->tangentImpulse2 = b2MulW( warmStartScale, cw->tangentImpulse2 );+				cw->totalNormalImpulse2 = zeroW;+				cw->relativeVelocity2 = zeroW;+				cw->restitutionImpulse2 = zeroW;++				// Effective mass along normal.+				{+					b2FloatW rnA = b2CrossW( cw->anchorA2, cw->normal );+					b2FloatW rnB = b2CrossW( cw->anchorB2, cw->normal );+					b2FloatW k = b2AddW( cw->invMassA, cw->invMassB );+					k = b2MulAddW( k, cw->invIA, b2MulW( rnA, rnA ) );+					k = b2MulAddW( k, cw->invIB, b2MulW( rnB, rnB ) );+					b2FloatW test = b2GreaterThanW( k, b2ZeroW() );+					cw->normalMass2 = b2BlendW( b2ZeroW(), b2DivW( oneW, k ), test );+				}++				// Effective mass along tangent.+				{+					b2FloatW rtA = b2CrossW( cw->anchorA2, tangent );+					b2FloatW rtB = b2CrossW( cw->anchorB2, tangent );+					b2FloatW k = b2AddW( cw->invMassA, cw->invMassB );+					k = b2MulAddW( k, cw->invIA, b2MulW( rtA, rtA ) );+					k = b2MulAddW( k, cw->invIB, b2MulW( rtB, rtB ) );+					b2FloatW test = b2GreaterThanW( k, b2ZeroW() );+					cw->tangentMass2 = b2BlendW( b2ZeroW(), b2DivW( oneW, k ), test );+				}++				// Zero effective mass on lanes that only have one point.+				cw->normalMass2 = b2BlendW( zeroW, cw->normalMass2, twoPointMask );+				cw->tangentMass2 = b2BlendW( zeroW, cw->tangentMass2, twoPointMask );+			}++			// Only sample contact point normal velocity if needed.+			b2FloatW restitutionMask = b2GreaterThanW( cw->restitution, zeroW );+			bool haveRestitution = b2AnyTrueW( restitutionMask );+			anyRestitution = anyRestitution || haveRestitution;++			if ( ( hitEvents & b2_simEnableHitEvent ) || haveRestitution )+			{+				b2BodyStateW bA = b2GatherBodies( states, cw->indexA );+				b2BodyStateW bB = b2GatherBodies( states, cw->indexB );++				{+					b2Vec2W rA = cw->anchorA1;+					b2Vec2W rB = cw->anchorB1;++					// Relative velocity at contact+					b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) );+					b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) );+					b2FloatW vn = b2AddW( b2MulW( dvx, cw->normal.X ), b2MulW( dvy, cw->normal.Y ) );+					cw->relativeVelocity1 = vn;++					_Alignas( 32 ) float normalVelocities[B2_SIMD_WIDTH];+					b2StoreW( normalVelocities, vn );++					for ( int lane = 0; lane < B2_SIMD_WIDTH; ++lane )+					{+						// Check the point count to avoid a race writing to b2_zeroContactSim.+						if ( contactLanes[lane]->manifold.pointCount > 0 )+						{+							contactLanes[lane]->manifold.points[0].normalVelocity = normalVelocities[lane];+						}+					}+				}++				{+					b2Vec2W rA = cw->anchorA2;+					b2Vec2W rB = cw->anchorB2;++					// Relative velocity at contact+					b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) );+					b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) );+					b2FloatW vn = b2AddW( b2MulW( dvx, cw->normal.X ), b2MulW( dvy, cw->normal.Y ) );+					cw->relativeVelocity2 = vn;++					_Alignas( 32 ) float normalVelocities[B2_SIMD_WIDTH];+					b2StoreW( normalVelocities, vn );++					for ( int lane = 0; lane < B2_SIMD_WIDTH; ++lane )+					{+						if ( contactLanes[lane]->manifold.pointCount > 1 )+						{+							contactLanes[lane]->manifold.points[1].normalVelocity = normalVelocities[lane];+						}+					}+				}+			}+		}++		// Advance to next color+		colorIndex += 1;+	}++	if ( anyRestitution )+	{+		b2AtomicStoreInt( &context->anyRestitution, 1 );+	}++	b2TracyCZoneEnd( prepare_contact );+}++void B2_WIDE( b2WarmStartContacts_Wide )( b2SolverBlock block, b2StepContext* context )+{+	b2TracyCZoneNC( warm_start_contact, "Warm Start", b2_colorGreen, true );++	b2BodyState* states = context->states;+	b2ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints;++	for ( int i = block.startIndex; i < block.startIndex + block.count; ++i )+	{+		b2ContactConstraintWide* c = constraints + i;+		b2BodyStateW bA = b2GatherBodies( states, c->indexA );+		b2BodyStateW bB = b2GatherBodies( states, c->indexB );++		b2FloatW tangentX = c->normal.Y;+		b2FloatW tangentY = b2SubW( b2ZeroW(), c->normal.X );++		{+			// fixed anchors+			b2Vec2W rA = c->anchorA1;+			b2Vec2W rB = c->anchorB1;++			b2Vec2W P;+			P.X = b2AddW( b2MulW( c->normalImpulse1, c->normal.X ), b2MulW( c->tangentImpulse1, tangentX ) );+			P.Y = b2AddW( b2MulW( c->normalImpulse1, c->normal.Y ), b2MulW( c->tangentImpulse1, tangentY ) );+			bA.w = b2MulSubW( bA.w, c->invIA, b2CrossW( rA, P ) );+			bA.v.X = b2MulSubW( bA.v.X, c->invMassA, P.X );+			bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, P.Y );+			bB.w = b2MulAddW( bB.w, c->invIB, b2CrossW( rB, P ) );+			bB.v.X = b2MulAddW( bB.v.X, c->invMassB, P.X );+			bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, P.Y );++			c->totalNormalImpulse1 = b2AddW( c->totalNormalImpulse1, c->normalImpulse1 );+		}++		{+			// fixed anchors+			b2Vec2W rA = c->anchorA2;+			b2Vec2W rB = c->anchorB2;++			b2Vec2W P;+			P.X = b2AddW( b2MulW( c->normalImpulse2, c->normal.X ), b2MulW( c->tangentImpulse2, tangentX ) );+			P.Y = b2AddW( b2MulW( c->normalImpulse2, c->normal.Y ), b2MulW( c->tangentImpulse2, tangentY ) );+			bA.w = b2MulSubW( bA.w, c->invIA, b2CrossW( rA, P ) );+			bA.v.X = b2MulSubW( bA.v.X, c->invMassA, P.X );+			bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, P.Y );+			bB.w = b2MulAddW( bB.w, c->invIB, b2CrossW( rB, P ) );+			bB.v.X = b2MulAddW( bB.v.X, c->invMassB, P.X );+			bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, P.Y );++			c->totalNormalImpulse2 = b2AddW( c->totalNormalImpulse2, c->normalImpulse2 );+		}++		bA.w = b2MulSubW( bA.w, c->invIA, c->rollingImpulse );+		bB.w = b2MulAddW( bB.w, c->invIB, c->rollingImpulse );++		b2ScatterBodies( states, c->indexA, &bA );+		b2ScatterBodies( states, c->indexB, &bB );+	}++	b2TracyCZoneEnd( warm_start_contact );+}++void B2_WIDE( b2PushContacts_Wide )( b2SolverBlock block, b2StepContext* context )+{+	b2TracyCZoneNC( solve_contact, "Solve Contact", b2_colorAliceBlue, true );++	b2BodyState* states = context->states;+	b2GraphColor* color = context->graph->colors + block.colorIndex;+	b2ContactConstraintWide* constraints = color->wideConstraints;+	b2FloatW inv_h = b2SplatW( context->inv_h );+	b2FloatW contactSpeed = b2SplatW( -context->world->contactSpeed );+	b2FloatW oneW = b2SplatW( 1.0f );+	b2FloatW zeroW = b2ZeroW();++	b2FloatW dynamicBiasRate = b2SplatW( context->contactSoftness.massScale * context->contactSoftness.biasRate );+	b2FloatW dynamicMassScale = b2SplatW( context->contactSoftness.massScale );+	b2FloatW dynamicImpulseScale = b2SplatW( context->contactSoftness.impulseScale );+	b2FloatW staticBiasRate = b2SplatW( context->staticSoftness.massScale * context->staticSoftness.biasRate );+	b2FloatW staticMassScale = b2SplatW( context->staticSoftness.massScale );+	b2FloatW staticImpulseScale = b2SplatW( context->staticSoftness.impulseScale );++	for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex )+	{+		b2ContactConstraintWide* c = constraints + wideIndex;++		b2BodyStateW bA = b2GatherBodies( states, c->indexA );+		b2BodyStateW bB = b2GatherBodies( states, c->indexB );++		b2FloatW softMask = b2SoftMaskW( c->indexA, c->indexB );+		b2FloatW biasRate = b2BlendW( dynamicBiasRate, staticBiasRate, softMask );+		b2FloatW massScale = b2BlendW( dynamicMassScale, staticMassScale, softMask );+		b2FloatW impulseScale = b2BlendW( dynamicImpulseScale, staticImpulseScale, softMask );++		b2Vec2W normal = c->normal;+		b2Vec2W dp = { b2SubW( bB.dp.X, bA.dp.X ), b2SubW( bB.dp.Y, bA.dp.Y ) };++		b2FloatW velocityBias1, massScale1, impulseScale1;+		{+			// Moving anchors for current separation+			b2Vec2W rsA = b2RotateVectorW( bA.dq, c->anchorA1 );+			b2Vec2W rsB = b2RotateVectorW( bB.dq, c->anchorB1 );++			// compute current separation+			// this is subject to round-off error if the anchor is far from the body center of mass+			b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) };+			b2FloatW s = b2AddW( b2DotW( normal, ds ), c->baseSeparation1 );++			// Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias+			// The contactSpeed is meant to limit stiffness, not increase it.+			b2FloatW separated = b2GreaterThanW( s, zeroW );++			// Speculative bias - positive+			b2FloatW specBias = b2MulW( s, inv_h );++			// Overlap bias - negative when overlapped+			b2FloatW overlapBias = b2MaxW( b2MulW( biasRate, s ), contactSpeed );++			// velocity bias = s > 0 ? spec : overlap+			velocityBias1 = b2BlendW( overlapBias, specBias, separated );++			massScale1 = b2BlendW( massScale, oneW, separated );+			impulseScale1 = b2BlendW( impulseScale, zeroW, separated );+		}++		b2FloatW velocityBias2, massScale2, impulseScale2;+		{+			b2Vec2W rsA = b2RotateVectorW( bA.dq, c->anchorA2 );+			b2Vec2W rsB = b2RotateVectorW( bB.dq, c->anchorB2 );++			b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) };+			b2FloatW s = b2AddW( b2DotW( normal, ds ), c->baseSeparation2 );++			b2FloatW separated = b2GreaterThanW( s, zeroW );+			b2FloatW specBias = b2MulW( s, inv_h );+			b2FloatW overlapBias = b2MaxW( b2MulW( biasRate, s ), contactSpeed );+			velocityBias2 = b2BlendW( overlapBias, specBias, separated );++			massScale2 = b2BlendW( massScale, oneW, separated );+			impulseScale2 = b2BlendW( impulseScale, zeroW, separated );+		}++		b2FloatW rnA1 = b2CrossW( c->anchorA1, normal );+		b2FloatW rnB1 = b2CrossW( c->anchorB1, normal );+		b2FloatW rnA2 = b2CrossW( c->anchorA2, normal );+		b2FloatW rnB2 = b2CrossW( c->anchorB2, normal );++		b2FloatW k21 = b2AddW( b2AddW( c->invMassA, c->invMassB ),+							   b2AddW( b2MulW( b2MulW( c->invIA, rnA1 ), rnA2 ), b2MulW( b2MulW( c->invIB, rnB1 ), rnB2 ) ) );++		b2FloatW dn = b2DotW( b2SubVW( bB.v, bA.v ), normal );+		b2FloatW vn1 = b2AddW( dn, b2SubW( b2MulW( bB.w, rnB1 ), b2MulW( bA.w, rnA1 ) ) );+		b2FloatW vn2 = b2AddW( dn, b2SubW( b2MulW( bB.w, rnB2 ), b2MulW( bA.w, rnA2 ) ) );++		b2FloatW negImpulse1 = b2AddW( b2MulW( c->normalMass1, b2AddW( b2MulW( massScale1, vn1 ), velocityBias1 ) ),+									   b2MulW( impulseScale1, c->normalImpulse1 ) );+		b2FloatW impulse1 = b2MaxW( b2NegW( negImpulse1 ), b2NegW( c->normalImpulse1 ) );+		c->normalImpulse1 = b2AddW( c->normalImpulse1, impulse1 );+		c->totalNormalImpulse1 = b2AddW( c->totalNormalImpulse1, impulse1 );++		vn2 = b2MulAddW( vn2, k21, impulse1 );++		b2FloatW negImpulse2 = b2AddW( b2MulW( c->normalMass2, b2AddW( b2MulW( massScale2, vn2 ), velocityBias2 ) ),+									   b2MulW( impulseScale2, c->normalImpulse2 ) );+		b2FloatW impulse2 = b2MaxW( b2NegW( negImpulse2 ), b2NegW( c->normalImpulse2 ) );+		c->normalImpulse2 = b2AddW( c->normalImpulse2, impulse2 );+		c->totalNormalImpulse2 = b2AddW( c->totalNormalImpulse2, impulse2 );++		b2FloatW impulse = b2AddW( impulse1, impulse2 );+		b2FloatW Px = b2MulW( impulse, normal.X );+		b2FloatW Py = b2MulW( impulse, normal.Y );+		b2FloatW angularA = b2AddW( b2MulW( impulse1, rnA1 ), b2MulW( impulse2, rnA2 ) );+		b2FloatW angularB = b2AddW( b2MulW( impulse1, rnB1 ), b2MulW( impulse2, rnB2 ) );++		bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px );+		bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py );+		bA.w = b2MulSubW( bA.w, c->invIA, angularA );++		bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px );+		bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py );+		bB.w = b2MulAddW( bB.w, c->invIB, angularB );++		b2ScatterBodies( states, c->indexA, &bA );+		b2ScatterBodies( states, c->indexB, &bB );+	}++	b2TracyCZoneEnd( solve_contact );+}++// Solve the normal constraint, friction, and rolling resistance.+void B2_WIDE( b2SolveContacts_Wide )( b2SolverBlock block, b2StepContext* context )+{+	b2TracyCZoneNC( solve_contact, "Solve Contact", b2_colorAliceBlue, true );++	b2BodyState* states = context->states;+	b2GraphColor* color = context->graph->colors + block.colorIndex;+	b2ContactConstraintWide* constraints = color->wideConstraints;+	b2FloatW inv_h = b2SplatW( context->inv_h );+	b2FloatW zeroW = b2ZeroW();++	for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex )+	{+		b2ContactConstraintWide* c = constraints + wideIndex;++		b2BodyStateW bA = b2GatherBodies( states, c->indexA );+		b2BodyStateW bB = b2GatherBodies( states, c->indexB );++		b2Vec2W normal = c->normal;+		b2Vec2W tangent = b2RightPerpW( normal );+		b2Vec2W dp = { b2SubW( bB.dp.X, bA.dp.X ), b2SubW( bB.dp.Y, bA.dp.Y ) };++		b2FloatW velocityBias1, velocityBias2;+		{+			// Moving anchors for current separation+			b2Vec2W rsA = b2RotateVectorW( bA.dq, c->anchorA1 );+			b2Vec2W rsB = b2RotateVectorW( bB.dq, c->anchorB1 );++			// compute current separation+			// this is subject to round-off error if the anchor is far from the body center of mass+			b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) };+			b2FloatW s = b2AddW( b2DotW( normal, ds ), c->baseSeparation1 );++			// Speculative bias, positive if separated and zero if overlapped+			velocityBias1 = b2MaxW( zeroW, b2MulW( s, inv_h ) );+		}++		{+			b2Vec2W rsA = b2RotateVectorW( bA.dq, c->anchorA2 );+			b2Vec2W rsB = b2RotateVectorW( bB.dq, c->anchorB2 );++			b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) };+			b2FloatW s = b2AddW( b2DotW( normal, ds ), c->baseSeparation2 );++			velocityBias2 = b2MaxW( zeroW, b2MulW( s, inv_h ) );+		}++		b2FloatW rnA1 = b2CrossW( c->anchorA1, normal );+		b2FloatW rnB1 = b2CrossW( c->anchorB1, normal );+		b2FloatW rnA2 = b2CrossW( c->anchorA2, normal );+		b2FloatW rnB2 = b2CrossW( c->anchorB2, normal );+		b2FloatW rtA1 = b2CrossW( c->anchorA1, tangent );+		b2FloatW rtB1 = b2CrossW( c->anchorB1, tangent );+		b2FloatW rtA2 = b2CrossW( c->anchorA2, tangent );+		b2FloatW rtB2 = b2CrossW( c->anchorB2, tangent );++		b2FloatW wnA1 = b2MulW( c->invIA, rnA1 );+		b2FloatW wnB1 = b2MulW( c->invIB, rnB1 );+		b2FloatW wnA2 = b2MulW( c->invIA, rnA2 );+		b2FloatW wnB2 = b2MulW( c->invIB, rnB2 );+		b2FloatW wtA1 = b2MulW( c->invIA, rtA1 );+		b2FloatW wtB1 = b2MulW( c->invIB, rtB1 );++		b2FloatW invMass = b2AddW( c->invMassA, c->invMassB );+		b2FloatW k21 = b2AddW( invMass, b2AddW( b2MulW( wnA1, rnA2 ), b2MulW( wnB1, rnB2 ) ) );+		b2FloatW k31 = b2AddW( b2MulW( wnA1, rtA1 ), b2MulW( wnB1, rtB1 ) );+		b2FloatW k32 = b2AddW( b2MulW( wnA2, rtA1 ), b2MulW( wnB2, rtB1 ) );+		b2FloatW k41 = b2AddW( b2MulW( wnA1, rtA2 ), b2MulW( wnB1, rtB2 ) );+		b2FloatW k42 = b2AddW( b2MulW( wnA2, rtA2 ), b2MulW( wnB2, rtB2 ) );+		b2FloatW k43 = b2AddW( invMass, b2AddW( b2MulW( wtA1, rtA2 ), b2MulW( wtB1, rtB2 ) ) );++		b2Vec2W dv = b2SubVW( bB.v, bA.v );+		b2FloatW dn = b2DotW( dv, normal );+		b2FloatW dt = b2SubW( b2DotW( dv, tangent ), c->tangentSpeed );++		b2FloatW vn1 = b2AddW( dn, b2SubW( b2MulW( bB.w, rnB1 ), b2MulW( bA.w, rnA1 ) ) );+		b2FloatW vn2 = b2AddW( dn, b2SubW( b2MulW( bB.w, rnB2 ), b2MulW( bA.w, rnA2 ) ) );+		b2FloatW vt1 = b2AddW( dt, b2SubW( b2MulW( bB.w, rtB1 ), b2MulW( bA.w, rtA1 ) ) );+		b2FloatW vt2 = b2AddW( dt, b2SubW( b2MulW( bB.w, rtB2 ), b2MulW( bA.w, rtA2 ) ) );++		b2FloatW normalImpulse1 = b2MaxW( b2NegW( b2MulW( c->normalMass1, b2AddW( vn1, velocityBias1 ) ) ),+										  b2NegW( c->normalImpulse1 ) );+		c->normalImpulse1 = b2AddW( c->normalImpulse1, normalImpulse1 );+		c->totalNormalImpulse1 = b2AddW( c->totalNormalImpulse1, normalImpulse1 );++		vn2 = b2MulAddW( vn2, k21, normalImpulse1 );++		b2FloatW normalImpulse2 = b2MaxW( b2NegW( b2MulW( c->normalMass2, b2AddW( vn2, velocityBias2 ) ) ),+										  b2NegW( c->normalImpulse2 ) );+		c->normalImpulse2 = b2AddW( c->normalImpulse2, normalImpulse2 );+		c->totalNormalImpulse2 = b2AddW( c->totalNormalImpulse2, normalImpulse2 );++		vt1 = b2MulAddW( b2MulAddW( vt1, k31, normalImpulse1 ), k32, normalImpulse2 );+		vt2 = b2MulAddW( b2MulAddW( vt2, k41, normalImpulse1 ), k42, normalImpulse2 );++		b2FloatW angularA = b2AddW( b2MulW( normalImpulse1, rnA1 ), b2MulW( normalImpulse2, rnA2 ) );+		b2FloatW angularB = b2AddW( b2MulW( normalImpulse1, rnB1 ), b2MulW( normalImpulse2, rnB2 ) );++		// Rolling resistance+		if ( b2AllZeroW( c->rollingResistance ) == false )+		{+			b2FloatW relativeW = b2SubW( b2SubW( bA.w, bB.w ), b2AddW( b2MulW( b2AddW( wnA1, wnB1 ), normalImpulse1 ),+																	  b2MulW( b2AddW( wnA2, wnB2 ), normalImpulse2 ) ) );+			b2FloatW k = b2AddW( c->invIA, c->invIB );+			b2FloatW mask = b2GreaterThanW( k, zeroW );+			b2FloatW deltaLambda = b2BlendW( zeroW, b2DivW( relativeW, k ), mask );+			b2FloatW lambda = c->rollingImpulse;+			b2FloatW maxLambda = b2MulW( c->rollingResistance, b2AddW( c->normalImpulse1, c->normalImpulse2 ) );+			c->rollingImpulse = b2SymClampW( b2AddW( lambda, deltaLambda ), maxLambda );+			deltaLambda = b2SubW( c->rollingImpulse, lambda );++			b2FloatW wtA2 = b2MulW( c->invIA, rtA2 );+			b2FloatW wtB2 = b2MulW( c->invIB, rtB2 );+			vt1 = b2MulAddW( vt1, b2AddW( wtA1, wtB1 ), deltaLambda );+			vt2 = b2MulAddW( vt2, b2AddW( wtA2, wtB2 ), deltaLambda );++			angularA = b2AddW( angularA, deltaLambda );+			angularB = b2AddW( angularB, deltaLambda );+		}++		b2FloatW maxFriction1 = b2MulW( c->friction, c->normalImpulse1 );+		b2FloatW tangentImpulse1 = b2NegW( b2MulW( c->tangentMass1, vt1 ) );+		tangentImpulse1 = b2MaxW( b2SubW( b2NegW( maxFriction1 ), c->tangentImpulse1 ),+								  b2MinW( tangentImpulse1, b2SubW( maxFriction1, c->tangentImpulse1 ) ) );+		c->tangentImpulse1 = b2AddW( c->tangentImpulse1, tangentImpulse1 );++		vt2 = b2MulAddW( vt2, k43, tangentImpulse1 );++		b2FloatW maxFriction2 = b2MulW( c->friction, c->normalImpulse2 );+		b2FloatW tangentImpulse2 = b2NegW( b2MulW( c->tangentMass2, vt2 ) );+		tangentImpulse2 = b2MaxW( b2SubW( b2NegW( maxFriction2 ), c->tangentImpulse2 ),+								  b2MinW( tangentImpulse2, b2SubW( maxFriction2, c->tangentImpulse2 ) ) );+		c->tangentImpulse2 = b2AddW( c->tangentImpulse2, tangentImpulse2 );++		angularA = b2AddW( angularA, b2AddW( b2MulW( tangentImpulse1, rtA1 ), b2MulW( tangentImpulse2, rtA2 ) ) );+		angularB = b2AddW( angularB, b2AddW( b2MulW( tangentImpulse1, rtB1 ), b2MulW( tangentImpulse2, rtB2 ) ) );++		b2FloatW normalImpulse = b2AddW( normalImpulse1, normalImpulse2 );+		b2FloatW tangentImpulse = b2AddW( tangentImpulse1, tangentImpulse2 );+		b2FloatW Px = b2AddW( b2MulW( normalImpulse, normal.X ), b2MulW( tangentImpulse, tangent.X ) );+		b2FloatW Py = b2AddW( b2MulW( normalImpulse, normal.Y ), b2MulW( tangentImpulse, tangent.Y ) );++		bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px );+		bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py );+		bA.w = b2MulSubW( bA.w, c->invIA, angularA );++		bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px );+		bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py );+		bB.w = b2MulAddW( bB.w, c->invIB, angularB );++		b2ScatterBodies( states, c->indexA, &bA );+		b2ScatterBodies( states, c->indexB, &bB );+	}++	b2TracyCZoneEnd( solve_contact );+}++void B2_WIDE( b2ApplyRestitution_Wide )( b2SolverBlock block, b2StepContext* context )+{+	b2TracyCZoneNC( restitution, "Restitution", b2_colorDodgerBlue, true );++	b2BodyState* states = context->states;+	b2GraphColor* color = context->graph->colors + block.colorIndex;+	b2ContactConstraintWide* constraints = color->wideConstraints;+	b2FloatW inv_h = b2SplatW( context->inv_h );+	b2FloatW negRestitutionThreshold = b2SplatW( -context->world->restitutionThreshold );+	b2FloatW zeroW = b2ZeroW();+	bool propagate = context->world->enableRestitutionPropagation;++	for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex )+	{+		b2ContactConstraintWide* c = constraints + wideIndex;+		if ( propagate == false && b2AllZeroW( c->restitution ) )+		{+			continue;+		}++		b2BodyStateW bA = b2GatherBodies( states, c->indexA );+		b2BodyStateW bB = b2GatherBodies( states, c->indexB );++		b2FloatW restitutionMask = b2GreaterThanW( c->restitution, zeroW );+		b2FloatW normalMass1 = propagate ? c->normalMass1 : b2BlendW( zeroW, c->normalMass1, restitutionMask );+		b2FloatW normalMass2 = propagate ? c->normalMass2 : b2BlendW( zeroW, c->normalMass2, restitutionMask );+		b2Vec2W dp = { b2SubW( bB.dp.X, bA.dp.X ), b2SubW( bB.dp.Y, bA.dp.Y ) };++		{+			b2Vec2W rA = c->anchorA1;+			b2Vec2W rB = c->anchorB1;++			b2FloatW compressionImpulse = b2SubW( c->totalNormalImpulse1, c->restitutionImpulse1 );+			b2FloatW armed = b2AndW( b2AndW( restitutionMask, b2LessThanW( c->relativeVelocity1, negRestitutionThreshold ) ),+									 b2GreaterThanW( compressionImpulse, zeroW ) );++			b2Vec2W rsA = b2RotateVectorW( bA.dq, rA );+			b2Vec2W rsB = b2RotateVectorW( bB.dq, rB );+			b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) };+			b2FloatW s = b2AddW( b2DotW( c->normal, ds ), c->baseSeparation1 );++			b2FloatW specBias = b2MaxW( zeroW, b2MulW( s, inv_h ) );+			b2FloatW velocityBias = b2BlendW( specBias, b2MulW( c->restitution, c->relativeVelocity1 ), armed );++			b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) );+			b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) );+			b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) );++			b2FloatW negImpulse = b2MulW( normalMass1, b2AddW( vn, velocityBias ) );++			b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse1, negImpulse ), zeroW );+			b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse1 );++			b2FloatW approachImpulse = b2MinW( b2MaxW( b2NegW( b2MulW( normalMass1, vn ) ), zeroW ), b2MaxW( impulse, zeroW ) );+			b2FloatW allowance =+				b2SubW( b2MulW( c->restitution, b2AddW( compressionImpulse, approachImpulse ) ), c->restitutionImpulse1 );+			b2FloatW maxImpulse = b2AddW( approachImpulse, b2MaxW( allowance, zeroW ) );+			impulse = b2BlendW( impulse, b2MinW( impulse, maxImpulse ), armed );++			c->normalImpulse1 = b2AddW( c->normalImpulse1, impulse );+			c->restitutionImpulse1 = b2AddW( c->restitutionImpulse1, b2SubW( impulse, approachImpulse ) );+			c->totalNormalImpulse1 = b2AddW( c->totalNormalImpulse1, impulse );++			b2FloatW Px = b2MulW( impulse, c->normal.X );+			b2FloatW Py = b2MulW( impulse, c->normal.Y );++			bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px );+			bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py );+			bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) );++			bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px );+			bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py );+			bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) );+		}++		{+			b2Vec2W rA = c->anchorA2;+			b2Vec2W rB = c->anchorB2;++			b2FloatW compressionImpulse = b2SubW( c->totalNormalImpulse2, c->restitutionImpulse2 );+			b2FloatW armed = b2AndW( b2AndW( restitutionMask, b2LessThanW( c->relativeVelocity2, negRestitutionThreshold ) ),+									 b2GreaterThanW( compressionImpulse, zeroW ) );++			b2Vec2W rsA = b2RotateVectorW( bA.dq, rA );+			b2Vec2W rsB = b2RotateVectorW( bB.dq, rB );+			b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) };+			b2FloatW s = b2AddW( b2DotW( c->normal, ds ), c->baseSeparation2 );++			b2FloatW specBias = b2MaxW( zeroW, b2MulW( s, inv_h ) );+			b2FloatW velocityBias = b2BlendW( specBias, b2MulW( c->restitution, c->relativeVelocity2 ), armed );++			b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) );+			b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) );+			b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) );++			b2FloatW negImpulse = b2MulW( normalMass2, b2AddW( vn, velocityBias ) );++			b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse2, negImpulse ), zeroW );+			b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse2 );++			b2FloatW approachImpulse = b2MinW( b2MaxW( b2NegW( b2MulW( normalMass2, vn ) ), zeroW ), b2MaxW( impulse, zeroW ) );+			b2FloatW allowance =+				b2SubW( b2MulW( c->restitution, b2AddW( compressionImpulse, approachImpulse ) ), c->restitutionImpulse2 );+			b2FloatW maxImpulse = b2AddW( approachImpulse, b2MaxW( allowance, zeroW ) );+			impulse = b2BlendW( impulse, b2MinW( impulse, maxImpulse ), armed );++			c->normalImpulse2 = b2AddW( c->normalImpulse2, impulse );+			c->restitutionImpulse2 = b2AddW( c->restitutionImpulse2, b2SubW( impulse, approachImpulse ) );+			c->totalNormalImpulse2 = b2AddW( c->totalNormalImpulse2, impulse );++			b2FloatW Px = b2MulW( impulse, c->normal.X );+			b2FloatW Py = b2MulW( impulse, c->normal.Y );++			bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px );+			bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py );+			bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) );++			bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px );+			bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py );+			bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) );+		}++		b2ScatterBodies( states, c->indexA, &bA );+		b2ScatterBodies( states, c->indexB, &bB );+	}++	b2TracyCZoneEnd( restitution );+}++// I tried adding this to the last relax iterations but it was slower.+//+// Runs as a flat parallel-for over the whole wide constraint range. Per-color+// contact sims are looked up through the prepareSpans cursor rather than the+// block's colorIndex, matching the layout of b2PrepareContacts_Wide.+//+// I could store the manifold pointer in the b2ContactConstraintWide to simplify+// this. But it is important to keep the constraint small.+void B2_WIDE( b2StoreImpulses_Wide )( b2SolverBlock block, b2StepContext* context, int workerIndex )+{+	b2TracyCZoneNC( store_impulses, "Store", b2_colorFireBrick, true );++	b2World* world = context->world;+	const b2ContactPrepareSpan* spans = context->contactPrepareSpans;+	const b2ContactConstraintWide* wideBase = context->wideContactConstraints;+	b2TaskContext* taskContext = world->taskContexts.data + workerIndex;+	b2BitSet* 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 colorWideEndIndex = b2MinInt( spans[colorIndex + 1].start, endWideIndex );+		int colorWideStart = spans[colorIndex].start;+		int colorContactCount = spans[colorIndex].count;+		b2ContactSim* contactSims = spans[colorIndex].contacts;++		for ( ; wideIndex < colorWideEndIndex; ++wideIndex )+		{+			const b2ContactConstraintWide* c = wideBase + wideIndex;+			const float* rollingImpulse = (float*)&c->rollingImpulse;+			const float* normalImpulse1 = (float*)&c->normalImpulse1;+			const float* normalImpulse2 = (float*)&c->normalImpulse2;+			const float* tangentImpulse1 = (float*)&c->tangentImpulse1;+			const float* tangentImpulse2 = (float*)&c->tangentImpulse2;+			const float* totalNormalImpulse1 = (float*)&c->totalNormalImpulse1;+			const float* totalNormalImpulse2 = (float*)&c->totalNormalImpulse2;++			int localWideIndex = wideIndex - colorWideStart;+			int baseIndex = B2_SIMD_WIDTH * localWideIndex;++			for ( int laneIndex = 0; laneIndex < B2_SIMD_WIDTH; ++laneIndex )+			{+				int contactIndex = baseIndex + laneIndex;+				if ( contactIndex >= colorContactCount )+				{+					break;+				}++				b2ContactSim* contactSim = contactSims + contactIndex;+				b2Manifold* m = &contactSim->manifold;+				m->rollingImpulse = rollingImpulse[laneIndex];++				m->points[0].normalImpulse = normalImpulse1[laneIndex];+				m->points[0].tangentImpulse = tangentImpulse1[laneIndex];+				m->points[0].totalNormalImpulse = totalNormalImpulse1[laneIndex];++				m->points[1].normalImpulse = normalImpulse2[laneIndex];+				m->points[1].tangentImpulse = tangentImpulse2[laneIndex];+				m->points[1].totalNormalImpulse = totalNormalImpulse2[laneIndex];++				// Check for hit events to speed up serial processing later in the step+				if ( ( contactSim->simFlags & b2_simEnableHitEvent ) != 0 )+				{+					for ( int k = 0; k < contactSim->manifold.pointCount; ++k )+					{+						b2ManifoldPoint* 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 )+						{+							b2SetBit( hitEventBitSet, contactSim->contactId );+							hasHitEvents = true;+							break;+						}+					}+				}+			}+		}++		// Advance to next color+		colorIndex += 1;+	}++	taskContext->hasHitEvents = hasHitEvents;++	b2TracyCZoneEnd( store_impulses );+}
+ upstream/src/container.h view
@@ -0,0 +1,179 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "core.h"++#include <stddef.h>+#if defined( _MSC_VER )+#include <vcruntime_string.h>+#else+#include <string.h>+#endif++#define b2DeclareArray( T )                                                                                                      \+	typedef struct b2DynamicArray_##T                                                                                            \+	{                                                                                                                            \+		struct T* data;                                                                                                          \+		int count;                                                                                                               \+		int capacity;                                                                                                            \+	} b2DynamicArray_##T++#define b2DeclareArrayNative( T )                                                                                                \+	typedef struct b2DynamicArray_##T                                                                                            \+	{                                                                                                                            \+		T* data;                                                                                                                 \+		int count;                                                                                                               \+		int capacity;                                                                                                            \+	} b2DynamicArray_##T++// Define an array.+// It may be zero initialized:+// b2Array(int) myArray = { 0 };+#define b2Array( T ) b2DynamicArray_##T++// Alternative to zero initialization+#define b2Array_Create( a )                                                                                                      \+	do                                                                                                                           \+	{                                                                                                                            \+		( a ).data = NULL;                                                                                                       \+		( a ).count = 0;                                                                                                         \+		( a ).capacity = 0;                                                                                                      \+	}                                                                                                                            \+	while ( 0 )++#define b2Array_CreateN( a, n )                                                                                                  \+	do                                                                                                                           \+	{                                                                                                                            \+		( a ).data = ( n ) > 0 ? b2GrowAlloc( NULL, 0, ( n ) * sizeof( *( a ).data ) ) : NULL;                                   \+		( a ).count = 0;                                                                                                         \+		( a ).capacity = ( n );                                                                                                  \+	}                                                                                                                            \+	while ( 0 )++#define b2Array_Destroy( a )                                                                                                     \+	do                                                                                                                           \+	{                                                                                                                            \+		b2Free( ( a ).data, ( a ).capacity * sizeof( *( a ).data ) );                                                            \+		( a ).data = NULL;                                                                                                       \+		( a ).count = 0;                                                                                                         \+		( a ).capacity = 0;                                                                                                      \+	}                                                                                                                            \+	while ( 0 )++#define b2Array_Reserve( a, n )                                                                                                  \+	do                                                                                                                           \+	{                                                                                                                            \+		if ( ( a ).capacity < n )                                                                                                \+		{                                                                                                                        \+			int oldSize = ( a ).capacity * sizeof( *( a ).data );                                                                \+			int newSize = ( n ) * sizeof( *( a ).data );                                                                         \+			( a ).data = b2GrowAlloc( ( a ).data, oldSize, newSize );                                                            \+			( a ).capacity = ( n );                                                                                              \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++#define b2Array_Grow( a, n )                                                                                                     \+	do                                                                                                                           \+	{                                                                                                                            \+		if ( ( a ).capacity < ( n ) )                                                                                            \+		{                                                                                                                        \+			int newCapacity = 2 * ( a ).capacity > ( n ) ? 2 * ( a ).capacity : ( n );                                           \+			b2Array_Reserve( a, newCapacity );                                                                                   \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++#define b2Array_Resize( a, n )                                                                                                   \+	do                                                                                                                           \+	{                                                                                                                            \+		b2Array_Reserve( a, n );                                                                                                 \+		( a ).count = ( n );                                                                                                     \+	}                                                                                                                            \+	while ( 0 )++#define b2Array_ResizeAndSetZero( a, n )                                                                                         \+	do                                                                                                                           \+	{                                                                                                                            \+		b2Array_Reserve( a, n );                                                                                                 \+		memset( ( a ).data, 0, ( n ) * sizeof( *( a ).data ) );                                                                  \+		( a ).count = ( n );                                                                                                     \+	}                                                                                                                            \+	while ( 0 )++// Push a new element by value+#define b2Array_Push( a, value )                                                                                                 \+	do                                                                                                                           \+	{                                                                                                                            \+		int elementSize = sizeof( *( a ).data );                                                                                 \+		if ( ( a ).count >= ( a ).capacity )                                                                                     \+		{                                                                                                                        \+			int oldSize = ( a ).capacity * elementSize;                                                                          \+			int newCapacity = ( a ).capacity == 0 ? 8 : 2 * ( a ).capacity;                                                      \+			int newSize = newCapacity * elementSize;                                                                             \+			( a ).data = b2GrowAlloc( ( a ).data, oldSize, newSize );                                                            \+			( a ).capacity = newCapacity;                                                                                        \+		}                                                                                                                        \+		( a ).data[( a ).count] = ( value );                                                                                     \+		( a ).count += 1;                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++// Get a pointer to an element+#define b2Array_Get( a, index ) ( B2_ASSERT( 0 <= ( index ) && ( index ) < ( a ).count ), ( a ).data + ( index ) )++// Create a new uninitialized element and return a pointer to it+#define b2Array_Emplace( a ) ( b2EmplaceHelper( (void**)&( a ).data, &( a ).count, &( a ).capacity, sizeof( *( a ).data ) ) )++// Remove the last element and return it by value.+#define b2Array_Pop( a ) ( B2_ASSERT( 0 < ( a ).count ), ( a ).data[-1 + ( a ).count--] )++// Remove an element by swapping with the last element. If the index is the last element it returns+// B2_NULL_INDEX, otherwise it returns the index of the last element (which is now out of bounds).+#define b2Array_RemoveSwap( a, index ) b2RemoveHelper( ( a ).data, &( a ).count, ( index ), sizeof( *( a ).data ) )++B2_INLINE void* b2EmplaceHelper( void** data, int* count, int* capacity, int elementSize )+{+	if ( *count >= *capacity )+	{+		int oldCapacity = *capacity;+		int oldSize = oldCapacity * elementSize;+		int newCapacity = ( oldCapacity == 0 ? 8 : 2 * oldCapacity );+		int newSize = newCapacity * elementSize;+		*data = b2GrowAlloc( *data, oldSize, newSize );+		*capacity = newCapacity;+	}+	return (char*)*data + ( *count )++ * elementSize;+}++B2_INLINE int b2RemoveHelper( void* data, int* count, int index, int elementSize )+{+	B2_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 B2_NULL_INDEX;+}++#define b2Array_Clear( a )                                                                                                       \+	do                                                                                                                           \+	{                                                                                                                            \+		( a ).count = 0;                                                                                                         \+	}                                                                                                                            \+	while ( 0 )++#define b2Array_ByteCount( a ) ( ( a ).capacity * (int)sizeof( *( a ).data ) )++b2DeclareArrayNative( int );+b2DeclareArrayNative( uint64_t );+b2DeclareArray( b2AABB );+b2DeclareArray( b2Body );+b2DeclareArray( b2BodySim );+b2DeclareArray( b2BodyState );
+ upstream/src/core.c view
@@ -0,0 +1,237 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "core.h"++#include "box2d/math_functions.h"++#if defined( B2_COMPILER_MSVC )+#define _CRTDBG_MAP_ALLOC+#include <crtdbg.h>+#include <stdlib.h>+#else+#include <stdlib.h>+#endif++#include <stdarg.h>+#include <stdio.h>+#include <string.h>++#ifdef BOX2D_PROFILE++#include <tracy/TracyC.h>+#define b2TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size )+#define b2TracyCFree( ptr ) TracyCFree( ptr )++#else++#define b2TracyCAlloc( ptr, size )+#define b2TracyCFree( ptr )++#endif++#include "platform.h"++// This allows the user to change the length units at runtime+float b2_lengthUnitsPerMeter = 1.0f;++void b2SetLengthUnitsPerMeter( float lengthUnits )+{+	B2_CHECK_INPUT( b2IsValidFloat( lengthUnits ) && lengthUnits > 0.0f );+	b2_lengthUnitsPerMeter = lengthUnits;+}++float b2GetLengthUnitsPerMeter( void )+{+	return b2_lengthUnitsPerMeter;+}++static int b2DefaultAssertFcn( const char* condition, const char* fileName, int lineNumber )+{+	fprintf( stderr, "BOX2D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber );+	fflush( stderr );++	// return non-zero to break to debugger+	return 1;+}++static b2AssertCallbackFcn* b2AssertHandler = b2DefaultAssertFcn;++void b2SetAssertCallback( b2AssertCallbackFcn* assertFcn )+{+	B2_ASSERT( assertFcn != NULL );+	b2AssertHandler = assertFcn;+}++#if !defined( NDEBUG ) || defined( B2_ENABLE_ASSERT )+int b2InternalAssert( const char* condition, const char* fileName, int lineNumber )+{+	int result = b2AssertHandler( condition, fileName, lineNumber );+	if ( result )+	{+		B2_BREAKPOINT;+	}+	return result;+}+#endif++static void b2DefaultLogFcn( const char* message )+{+	printf( "Box2D: %s\n", message );+}++static b2LogCallbackFcn* b2LogHandler = b2DefaultLogFcn;++void b2SetLogCallback( b2LogCallbackFcn* logFcn )+{+	B2_ASSERT( logFcn != NULL );+	b2LogHandler = logFcn;+}++void b2Log( const char* format, ... )+{+	va_list args;+	va_start( args, format );+	char buffer[512];+	vsnprintf( buffer, sizeof( buffer ), format, args );+	b2LogHandler( buffer );+	va_end( args );+}++b2Version b2GetVersion( void )+{+	return (b2Version){+		.major = B2_VERSION_MAJOR,+		.minor = B2_VERSION_MINOR,+		.revision = B2_VERSION_REVISION,+	};+}++bool b2IsDoublePrecision( void )+{+#if defined( BOX2D_DOUBLE_PRECISION )+	return true;+#else+	return false;+#endif+}++static b2AllocCallbackFcn* b2_allocFcn = NULL;+static b2FreeCallbackFcn* b2_freeFcn = NULL;++static b2AtomicI64 b2_byteCount;++void b2SetAllocator( b2AllocCallbackFcn* allocFcn, b2FreeCallbackFcn* freeFcn )+{+	b2_allocFcn = allocFcn;+	b2_freeFcn = freeFcn;+}++void* b2Alloc( size_t size )+{+	if ( size == 0 )+	{+		return NULL;+	}++	b2AtomicFetchAddI64( &b2_byteCount, size );++	// Allocation must be a multiple of alignment or risk a seg fault+	// https://en.cppreference.com/w/c/memory/aligned_alloc+	size_t alignedSize = ( ( size - 1 ) | ( B2_ALIGNMENT - 1 ) ) + 1;++	if ( b2_allocFcn != NULL )+	{+		void* ptr = b2_allocFcn( alignedSize, B2_ALIGNMENT );+		b2TracyCAlloc( ptr, size );++		B2_ASSERT( ptr != NULL );+		B2_ASSERT( ( (uintptr_t)ptr & ( B2_ALIGNMENT - 1 ) ) == 0 );++		return ptr;+	}++#ifdef B2_PLATFORM_WINDOWS+	void* ptr = _aligned_malloc( alignedSize, B2_ALIGNMENT );+#elif defined( B2_PLATFORM_ANDROID )+	void* ptr = NULL;+	if ( posix_memalign( &ptr, B2_ALIGNMENT, alignedSize ) != 0 )+	{+		// allocation failed, exit the application+		exit( EXIT_FAILURE );+	}+#else+	void* ptr = aligned_alloc( B2_ALIGNMENT, alignedSize );+#endif++	b2TracyCAlloc( ptr, size );++	B2_ASSERT( ptr != NULL );+	B2_ASSERT( ( (uintptr_t)ptr & 0x1F ) == 0 );++	return ptr;+}++void* b2AllocZero( size_t size )+{+	void* memory = b2Alloc( size );+	memset( memory, 0, size );+	return memory;+}++void b2Free( void* mem, size_t size )+{+	if ( mem == NULL )+	{+		return;+	}++	b2TracyCFree( mem );++	if ( b2_freeFcn != NULL )+	{+		size_t alignedSize = size > 0 ? ( ( size - 1 ) | ( B2_ALIGNMENT - 1 ) ) + 1 : 0;+		b2_freeFcn( mem, alignedSize );+	}+	else+	{+#ifdef B2_PLATFORM_WINDOWS+		_aligned_free( mem );+#else+		free( mem );+#endif+	}++	b2AtomicFetchAddI64( &b2_byteCount, -(int64_t)size );+}++void* b2GrowAlloc( void* oldMem, size_t oldSize, size_t newSize )+{+	B2_ASSERT( newSize > oldSize );+	void* newMem = b2Alloc( newSize );+	if ( oldSize > 0 )+	{+		memcpy( newMem, oldMem, oldSize );+		b2Free( oldMem, oldSize );+	}+	return newMem;+}++void* b2GrowAllocZero( void* oldMem, size_t oldSize, size_t newSize )+{+	B2_ASSERT( newSize > oldSize );+	void* newMem = b2Alloc( newSize );+	if ( oldSize > 0 )+	{+		memcpy( newMem, oldMem, oldSize );+		b2Free( oldMem, oldSize );+	}++	memset( (char*)newMem + oldSize, 0, newSize - oldSize );+	return newMem;+}++size_t b2GetByteCount( void )+{+	return (size_t)b2AtomicLoadI64( &b2_byteCount );+}
+ upstream/src/core.h view
@@ -0,0 +1,229 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box2d/base.h"++// clang-format off++// for performance comparisons+#define B2_RESTRICT restrict++#ifdef NDEBUG+	#define B2_DEBUG 0+#else+	#define B2_DEBUG 1+#endif++// Define platform+#if defined(_WIN32) || defined(_WIN64)+	#define B2_PLATFORM_WINDOWS+#elif defined( __ANDROID__ )+	#define B2_PLATFORM_ANDROID+#elif defined( __linux__ )+	#define B2_PLATFORM_LINUX+#elif defined( __APPLE__ )+	#include <TargetConditionals.h>+	#if defined( TARGET_OS_IPHONE ) && !TARGET_OS_IPHONE+		#define B2_PLATFORM_MACOS+	#else+		#define B2_PLATFORM_IOS+	#endif+#elif defined( __EMSCRIPTEN__ )+	#define B2_PLATFORM_WASM+#else+	#define B2_PLATFORM_UNKNOWN+#endif++// Define CPU+#if defined( __x86_64__ ) || defined( _M_X64 ) || defined( __i386__ ) || defined( _M_IX86 )+	#define B2_CPU_X86_X64+#elif defined( __aarch64__ ) || defined( _M_ARM64 ) || defined( __arm__ ) || defined( _M_ARM )+	#define B2_CPU_ARM+#elif defined( __EMSCRIPTEN__ )+	#define B2_CPU_WASM+#else+	#define B2_CPU_UNKNOWN+#endif++// Define SIMD+#if defined( BOX2D_DISABLE_SIMD )+	#define B2_SIMD_NONE+#else+	#if defined( B2_CPU_X86_X64 )+		#define B2_SIMD_SSE2+		#define B2_SIMD_HAS_WIDTH_8+		#if defined( __AVX2__ ) || ( defined( B2_SIMD_WIDTH ) && B2_SIMD_WIDTH == 8 )+			#define B2_SIMD_AVX2+		#endif+	#elif defined( B2_CPU_ARM )+		#if defined( __aarch64__ ) || defined( _M_ARM64 )+			#define B2_SIMD_NEON+		#else+			// arm32 gets scalar math because 32-bit Neon has determinism problems+			// and unverifiable performance.+			#define B2_SIMD_NONE+		#endif+	#elif defined( B2_CPU_WASM )+		#define B2_CPU_WASM+		#define B2_SIMD_SSE2+	#else+		#define B2_SIMD_NONE+	#endif+#endif++#if defined( B2_SIMD_AVX2 ) && defined( __clang__ )+	#define B2_AVX2_BEGIN _Pragma( "clang attribute push( __attribute__( ( target( \"avx2\" ) ) ), apply_to = function )" )+	#define B2_AVX2_END _Pragma( "clang attribute pop" )+#elif defined( B2_SIMD_AVX2 ) && defined( __GNUC__ )+	#define B2_AVX2_BEGIN _Pragma( "GCC push_options" ) _Pragma( "GCC target( \"avx2\" )" )+	#define B2_AVX2_END _Pragma( "GCC pop_options" )+#else+	#define B2_AVX2_BEGIN+	#define B2_AVX2_END+#endif++// Define compiler+#if defined( __clang__ )+	#define B2_COMPILER_CLANG+#elif defined( __GNUC__ )+	#define B2_COMPILER_GCC+#elif defined( _MSC_VER )+	#define B2_COMPILER_MSVC+#endif++/// Tracy profiler instrumentation+/// https://github.com/wolfpld/tracy+#ifdef BOX2D_PROFILE+	#include <tracy/TracyC.h>+	#define b2TracyCZoneC( ctx, color, active ) TracyCZoneC( ctx, color, active )+	#define b2TracyCZoneNC( ctx, name, color, active ) TracyCZoneNC( ctx, name, color, active )+	#define b2TracyCZoneEnd( ctx ) TracyCZoneEnd( ctx )+	#define b2TracyCFrame TracyCFrameMark+	#define b2TracyCSetThreadName( name ) TracyCSetThreadName( name )+#else+	#define b2TracyCZoneC( ctx, color, active )+	#define b2TracyCZoneNC( ctx, name, color, active )+	#define b2TracyCZoneEnd( ctx )+	#define b2TracyCFrame+	#define b2TracyCSetThreadName( name )+#endif++// clang-format on++// Returns the number of elements of an array+#define B2_ARRAY_COUNT( A ) ( (int)( sizeof( A ) / sizeof( *A ) ) )++// Used to prevent the compiler from warning about unused variables+#define B2_UNUSED( ... ) (void)sizeof(( __VA_ARGS__, 0 ))++// Use to validate definitions. Do not take my cookie.+#define B2_SECRET_COOKIE 1152023++// Snoop counters. These should be disabled in optimized builds because they are expensive.+#if defined( box2d_EXPORTS )+#define B2_SNOOP_TABLE_COUNTERS B2_DEBUG+#define B2_SNOOP_PAIR_COUNTERS B2_DEBUG+#define B2_SNOOP_TOI_COUNTERS B2_DEBUG+#else+#define B2_SNOOP_TABLE_COUNTERS 0+#define B2_SNOOP_PAIR_COUNTERS 0+#define B2_SNOOP_TOI_COUNTERS 0+#endif++#ifdef __cplusplus+#define B2_TYPE_OF( A ) decltype( A )+#else+#define B2_TYPE_OF( A ) __typeof__( A )+#endif++#define B2_SWAP( x, y )                                                                                                          \+	do                                                                                                                           \+	{                                                                                                                            \+		B2_TYPE_OF( x ) B2_SWAP_TEMP = x;                                                                                        \+		x = y;                                                                                                                   \+		y = B2_SWAP_TEMP;                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++#define B2_CHECK_DEF( DEF ) B2_ASSERT( DEF->internalValue == B2_SECRET_COOKIE )++// Used to validate API inputs.+#define B2_CHECK_INPUT( CONDITION )                                                                                              \+	do                                                                                                                           \+	{                                                                                                                            \+		if ( ( CONDITION ) == false )                                                                                            \+		{                                                                                                                        \+			b2Log( "invalid input: %s in %s\n", #CONDITION, __func__ );                                                          \+			B2_ASSERT( CONDITION );                                                                                              \+			return;                                                                                                              \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++#define B2_CHECK_INPUT_RETURN( CONDITION, RESULT )                                                                               \+	do                                                                                                                           \+	{                                                                                                                            \+		if ( ( CONDITION ) == false )                                                                                            \+		{                                                                                                                        \+			b2Log( "invalid input: %s in %s\n", #CONDITION, __func__ );                                                          \+			B2_ASSERT( CONDITION );                                                                                              \+			return RESULT;                                                                                                       \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++typedef struct b2AtomicInt+{+	int value;+} b2AtomicInt;++typedef struct b2AtomicU32+{+	uint32_t value;+} b2AtomicU32;++typedef struct b2AtomicI64+{+	// 64-bit atomic wants 8-byte alignment+	_Alignas( 8 ) int64_t value;+} b2AtomicI64;++// Use 64 byte alignment for everything. Needed for tree nodes.+#define B2_ALIGNMENT 64++void* b2Alloc( size_t size );+void* b2AllocZero( size_t size );+#define B2_ALLOC_STRUCT( type ) b2Alloc( sizeof( type ) )+#define B2_ALLOC_ARRAY( count, type ) b2Alloc( count * sizeof( type ) )++void b2Free( void* mem, size_t size );+#define B2_FREE_STRUCT( mem, type ) b2Free( mem, sizeof( type ) );+#define B2_FREE_ARRAY( mem, count, type ) b2Free( mem, count * sizeof( type ) )++void* b2GrowAlloc( void* oldMem, size_t oldSize, size_t newSize );+void* b2GrowAllocZero( void* oldMem, size_t oldSize, size_t newSize );++#define B2_GROW( A, N, M ) b2GrowAlloc( ( A ), ( N ) * sizeof( *( A ) ), ( M ) * sizeof( *( A ) ) )+#define B2_GROW_ZERO( A, N, M ) b2GrowAllocZero( ( A ), ( N ) * sizeof( *( A ) ), ( M ) * sizeof( *( A ) ) )++void b2Log( const char* format, ... );++typedef struct b2Mutex b2Mutex;+b2Mutex* b2CreateMutex( void );+void b2DestroyMutex( b2Mutex* m );+void b2LockMutex( b2Mutex* m );+void b2UnlockMutex( b2Mutex* m );++typedef struct b2Semaphore b2Semaphore;+b2Semaphore* b2CreateSemaphore( int initCount );+void b2DestroySemaphore( b2Semaphore* s );+void b2WaitSemaphore( b2Semaphore* s );+void b2SignalSemaphore( b2Semaphore* s );++typedef void b2ThreadFunction( void* context );+typedef struct b2Thread b2Thread;+// Name may be NULL, otherwise it is copied.+b2Thread* b2CreateThread( b2ThreadFunction* function, void* context, const char* name );+void b2JoinThread( b2Thread* t );
+ upstream/src/distance.c view
@@ -0,0 +1,1441 @@++// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "core.h"++#include "box2d/collision.h"+#include "box2d/constants.h"+#include "box2d/math_functions.h"++#include <float.h>+#include <stddef.h>++b2Transform b2GetSweepTransform( const b2Sweep* sweep, float time )+{+	// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/+	b2Transform xf;+	xf.p = b2Add( b2MulSV( 1.0f - time, sweep->c1 ), b2MulSV( time, sweep->c2 ) );++	b2Rot q = {+		( 1.0f - time ) * sweep->q1.c + time * sweep->q2.c,+		( 1.0f - time ) * sweep->q1.s + time * sweep->q2.s,+	};++	xf.q = b2NormalizeRot( q );++	// Shift to origin+	xf.p = b2Sub( xf.p, b2RotateVector( xf.q, sweep->localCenter ) );+	return xf;+}++/// Follows Ericson 5.1.9 Closest Points of Two Line Segments+b2SegmentDistanceResult b2ComputeSegmentDistance( b2Vec2 p1, b2Vec2 q1, b2Vec2 p2, b2Vec2 q2 )+{+	b2SegmentDistanceResult result = { 0 };++	b2Vec2 d1 = b2Sub( q1, p1 );+	b2Vec2 d2 = b2Sub( q2, p2 );+	b2Vec2 r = b2Sub( p1, p2 );+	float dd1 = b2Dot( d1, d1 );+	float dd2 = b2Dot( d2, d2 );+	float rd1 = b2Dot( r, d1 );+	float rd2 = b2Dot( r, d2 );++	const float epsSqr = FLT_EPSILON * FLT_EPSILON;++	if ( dd1 < epsSqr || dd2 < epsSqr )+	{+		// Handle all degeneracies+		if ( dd1 >= epsSqr )+		{+			// Segment 2 is degenerate+			result.fraction1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f );+			result.fraction2 = 0.0f;+		}+		else if ( dd2 >= epsSqr )+		{+			// Segment 1 is degenerate+			result.fraction1 = 0.0f;+			result.fraction2 = b2ClampFloat( rd2 / dd2, 0.0f, 1.0f );+		}+		else+		{+			result.fraction1 = 0.0f;+			result.fraction2 = 0.0f;+		}+	}+	else+	{+		// Non-degenerate segments+		float d12 = b2Dot( d1, d2 );++		float denominator = dd1 * dd2 - d12 * d12;++		// Fraction on segment 1+		float f1 = 0.0f;+		if ( denominator != 0.0f )+		{+			// not parallel+			f1 = b2ClampFloat( ( d12 * rd2 - rd1 * dd2 ) / denominator, 0.0f, 1.0f );+		}++		// Compute point on segment 2 closest to p1 + f1 * d1+		float f2 = ( d12 * f1 + rd2 ) / dd2;++		// Clamping of segment 2 requires a do over on segment 1+		if ( f2 < 0.0f )+		{+			f2 = 0.0f;+			f1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f );+		}+		else if ( f2 > 1.0f )+		{+			f2 = 1.0f;+			f1 = b2ClampFloat( ( d12 - rd1 ) / dd1, 0.0f, 1.0f );+		}++		result.fraction1 = f1;+		result.fraction2 = f2;+	}++	result.closest1 = b2MulAdd( p1, result.fraction1, d1 );+	result.closest2 = b2MulAdd( p2, result.fraction2, d2 );+	result.distanceSquared = b2DistanceSquared( result.closest1, result.closest2 );+	return result;+}++b2ShapeProxy b2MakeProxy( const b2Vec2* points, int count, float radius )+{+	count = b2MinInt( count, B2_MAX_POLYGON_VERTICES );+	b2ShapeProxy proxy;+	for ( int i = 0; i < count; ++i )+	{+		proxy.points[i] = points[i];+	}+	proxy.count = count;+	proxy.radius = radius;+	return proxy;+}++b2ShapeProxy b2MakeOffsetProxy( const b2Vec2* points, int count, float radius, b2Vec2 position, b2Rot rotation )+{+	count = b2MinInt( count, B2_MAX_POLYGON_VERTICES );+	b2Transform transform = {+		.p = position,+		.q = rotation,+	};+	b2ShapeProxy proxy;+	for ( int i = 0; i < count; ++i )+	{+		proxy.points[i] = b2TransformPoint( transform, points[i] );+	}+	proxy.count = count;+	proxy.radius = radius;+	return proxy;+}++static inline b2Vec2 b2Weight2( float a1, b2Vec2 w1, float a2, b2Vec2 w2 )+{+	return (b2Vec2){ a1 * w1.x + a2 * w2.x, a1 * w1.y + a2 * w2.y };+}++static inline b2Vec2 b2Weight3( float a1, b2Vec2 w1, float a2, b2Vec2 w2, float a3, b2Vec2 w3 )+{+	return (b2Vec2){ a1 * w1.x + a2 * w2.x + a3 * w3.x, a1 * w1.y + a2 * w2.y + a3 * w3.y };+}++static inline int b2FindSupport( const b2ShapeProxy* proxy, b2Vec2 direction )+{+	const b2Vec2* points = proxy->points;+	int count = proxy->count;++	int bestIndex = 0;+	float bestValue = b2Dot( points[0], direction );+	for ( int i = 1; i < count; ++i )+	{+		float value = b2Dot( points[i], direction );+		if ( value > bestValue )+		{+			bestIndex = i;+			bestValue = value;+		}+	}++	return bestIndex;+}++static b2Simplex b2MakeSimplexFromCache( b2SimplexCache cache, const b2ShapeProxy* proxyA, const b2ShapeProxy* proxyB )+{+	B2_ASSERT( cache.count <= 3 );+	b2Simplex s;++	// Copy data from cache.+	s.count = cache.count;++	b2SimplexVertex* vertices[] = { &s.v1, &s.v2, &s.v3 };+	for ( int i = 0; i < s.count; ++i )+	{+		b2SimplexVertex* v = vertices[i];+		v->indexA = cache.indexA[i];+		v->indexB = cache.indexB[i];+		v->wA = proxyA->points[v->indexA];+		v->wB = proxyB->points[v->indexB];+		v->w = b2Sub( v->wA, v->wB );++		// invalid+		v->a = -1.0f;+	}++	// If the cache is empty or invalid ...+	if ( s.count == 0 )+	{+		b2SimplexVertex* v = vertices[0];+		v->indexA = 0;+		v->indexB = 0;+		v->wA = proxyA->points[0];+		v->wB = proxyB->points[0];+		v->w = b2Sub( v->wA, v->wB );+		v->a = 1.0f;+		s.count = 1;+	}++	return s;+}++static b2SimplexCache b2MakeSimplexCache( const b2Simplex* simplex )+{+	b2SimplexCache cache = { 0 };+	cache.count = (uint16_t)simplex->count;+	const b2SimplexVertex* vertices[] = { &simplex->v1, &simplex->v2, &simplex->v3 };+	for ( int i = 0; i < simplex->count; ++i )+	{+		cache.indexA[i] = (uint8_t)vertices[i]->indexA;+		cache.indexB[i] = (uint8_t)vertices[i]->indexB;+	}++	return cache;+}++static void b2ComputeWitnessPoints( const b2Simplex* s, b2Vec2* a, b2Vec2* b )+{+	switch ( s->count )+	{+		case 1:+			*a = s->v1.wA;+			*b = s->v1.wB;+			break;++		case 2:+			*a = b2Weight2( s->v1.a, s->v1.wA, s->v2.a, s->v2.wA );+			*b = b2Weight2( s->v1.a, s->v1.wB, s->v2.a, s->v2.wB );+			break;++		case 3:+			*a = b2Weight3( s->v1.a, s->v1.wA, s->v2.a, s->v2.wA, s->v3.a, s->v3.wA );+			// todo why are these not equal?+			//*b = b2Weight3(s->v1.a, s->v1.wB, s->v2.a, s->v2.wB, s->v3.a, s->v3.wB);+			*b = *a;+			break;++		default:+			*a = b2Vec2_zero;+			*b = b2Vec2_zero;+			B2_ASSERT( false );+			break;+	}+}++// Solve a line segment using barycentric coordinates.+//+// p = a1 * w1 + a2 * w2+// a1 + a2 = 1+//+// The vector from the origin to the closest point on the line is+// perpendicular to the line.+// e12 = w2 - w1+// dot(p, e) = 0+// a1 * dot(w1, e) + a2 * dot(w2, e) = 0+//+// 2-by-2 linear system+// [1      1     ][a1] = [1]+// [w1.e12 w2.e12][a2] = [0]+//+// Define+// d12_1 =  dot(w2, e12)+// d12_2 = -dot(w1, e12)+// d12 = d12_1 + d12_2+//+// Solution+// a1 = d12_1 / d12+// a2 = d12_2 / d12+//+// returns a vector that points towards the origin+static b2Vec2 b2SolveSimplex2( b2Simplex* s )+{+	b2Vec2 w1 = s->v1.w;+	b2Vec2 w2 = s->v2.w;+	b2Vec2 e12 = b2Sub( w2, w1 );++	// w1 region+	float d12_2 = -b2Dot( w1, e12 );+	if ( d12_2 <= 0.0f )+	{+		// a2 <= 0, so we clamp it to 0+		s->v1.a = 1.0f;+		s->count = 1;+		return b2Neg( w1 );+	}++	// w2 region+	float d12_1 = b2Dot( w2, e12 );+	if ( d12_1 <= 0.0f )+	{+		// a1 <= 0, so we clamp it to 0+		s->v2.a = 1.0f;+		s->count = 1;+		s->v1 = s->v2;+		return b2Neg( w2 );+	}++	// Must be in e12 region.+	float inv_d12 = 1.0f / ( d12_1 + d12_2 );+	s->v1.a = d12_1 * inv_d12;+	s->v2.a = d12_2 * inv_d12;+	s->count = 2;+	return b2CrossSV( b2Cross( b2Add( w1, w2 ), e12 ), e12 );+}++static b2Vec2 b2SolveSimplex3( b2Simplex* s )+{+	b2Vec2 w1 = s->v1.w;+	b2Vec2 w2 = s->v2.w;+	b2Vec2 w3 = s->v3.w;++	// Edge12+	// [1      1     ][a1] = [1]+	// [w1.e12 w2.e12][a2] = [0]+	// a3 = 0+	b2Vec2 e12 = b2Sub( w2, w1 );+	float w1e12 = b2Dot( w1, e12 );+	float w2e12 = b2Dot( w2, e12 );+	float d12_1 = w2e12;+	float d12_2 = -w1e12;++	// Edge13+	// [1      1     ][a1] = [1]+	// [w1.e13 w3.e13][a3] = [0]+	// a2 = 0+	b2Vec2 e13 = b2Sub( w3, w1 );+	float w1e13 = b2Dot( w1, e13 );+	float w3e13 = b2Dot( w3, e13 );+	float d13_1 = w3e13;+	float d13_2 = -w1e13;++	// Edge23+	// [1      1     ][a2] = [1]+	// [w2.e23 w3.e23][a3] = [0]+	// a1 = 0+	b2Vec2 e23 = b2Sub( w3, w2 );+	float w2e23 = b2Dot( w2, e23 );+	float w3e23 = b2Dot( w3, e23 );+	float d23_1 = w3e23;+	float d23_2 = -w2e23;++	// Triangle123+	float n123 = b2Cross( e12, e13 );++	float d123_1 = n123 * b2Cross( w2, w3 );+	float d123_2 = n123 * b2Cross( w3, w1 );+	float d123_3 = n123 * b2Cross( w1, w2 );++	// w1 region+	if ( d12_2 <= 0.0f && d13_2 <= 0.0f )+	{+		s->v1.a = 1.0f;+		s->count = 1;+		return b2Neg( w1 );+	}++	// e12+	if ( d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f )+	{+		float inv_d12 = 1.0f / ( d12_1 + d12_2 );+		s->v1.a = d12_1 * inv_d12;+		s->v2.a = d12_2 * inv_d12;+		s->count = 2;+		return b2CrossSV( b2Cross( b2Add( w1, w2 ), e12 ), e12 );+	}++	// e13+	if ( d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f )+	{+		float inv_d13 = 1.0f / ( d13_1 + d13_2 );+		s->v1.a = d13_1 * inv_d13;+		s->v3.a = d13_2 * inv_d13;+		s->count = 2;+		s->v2 = s->v3;+		return b2CrossSV( b2Cross( b2Add( w1, w3 ), e13 ), e13 );+	}++	// w2 region+	if ( d12_1 <= 0.0f && d23_2 <= 0.0f )+	{+		s->v2.a = 1.0f;+		s->count = 1;+		s->v1 = s->v2;+		return b2Neg( w2 );+	}++	// w3 region+	if ( d13_1 <= 0.0f && d23_1 <= 0.0f )+	{+		s->v3.a = 1.0f;+		s->count = 1;+		s->v1 = s->v3;+		return b2Neg( w3 );+	}++	// e23+	if ( d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f )+	{+		float inv_d23 = 1.0f / ( d23_1 + d23_2 );+		s->v2.a = d23_1 * inv_d23;+		s->v3.a = d23_2 * inv_d23;+		s->count = 2;+		s->v1 = s->v3;+		return b2CrossSV( b2Cross( b2Add( w2, w3 ), e23 ), e23 );+	}++	// Must be in triangle123+	float inv_d123 = 1.0f / ( d123_1 + d123_2 + d123_3 );+	s->v1.a = d123_1 * inv_d123;+	s->v2.a = d123_2 * inv_d123;+	s->v3.a = d123_3 * inv_d123;+	s->count = 3;++	// No search direction+	return b2Vec2_zero;+}++// Uses GJK for computing the distance between convex shapes.+// https://box2d.org/files/ErinCatto_GJK_GDC2010.pdf+// I spent time optimizing this and could find no further significant gains 3/30/2025+b2DistanceOutput b2ComputeShapeDistance( const b2DistanceInput* input, b2SimplexCache* cache, b2Simplex* simplexes, int simplexCapacity )+{+	B2_UNUSED( simplexes, simplexCapacity );+	B2_ASSERT( input->proxyA.count > 0 && input->proxyB.count > 0 );+	B2_ASSERT( input->proxyA.radius >= 0.0f );+	B2_ASSERT( input->proxyB.radius >= 0.0f );++	b2DistanceOutput output = { 0 };++	const b2ShapeProxy* proxyA = &input->proxyA;++	// Get proxyB in frame A to avoid further transforms in the main loop.+	// This is still a performance gain at 8 points.+	b2ShapeProxy localProxyB;+	{+		localProxyB.count = input->proxyB.count;+		localProxyB.radius = input->proxyB.radius;+		for ( int i = 0; i < localProxyB.count; ++i )+		{+			localProxyB.points[i] = b2TransformPoint( input->transform, input->proxyB.points[i] );+		}+	}++	// Initialize the simplex.+	b2Simplex simplex = b2MakeSimplexFromCache( *cache, proxyA, &localProxyB );++	int simplexIndex = 0;+	if ( simplexes != NULL && simplexIndex < simplexCapacity )+	{+		simplexes[simplexIndex] = simplex;+		simplexIndex += 1;+	}++	// Get simplex vertices as an array.+	b2SimplexVertex* vertices[] = { &simplex.v1, &simplex.v2, &simplex.v3 };++	b2Vec2 nonUnitNormal = b2Vec2_zero;++	// These store the vertices of the last simplex so that we can check for duplicates and prevent cycling.+	int saveA[3], saveB[3];++	// Main iteration loop. All computations are done in frame A.+	const int maxIterations = 20;+	int iteration = 0;+	while ( iteration < maxIterations )+	{+		// Copy simplex so we can identify duplicates.+		int saveCount = simplex.count;+		for ( int i = 0; i < saveCount; ++i )+		{+			saveA[i] = vertices[i]->indexA;+			saveB[i] = vertices[i]->indexB;+		}++		b2Vec2 d = { 0 };+		switch ( simplex.count )+		{+			case 1:+				d = b2Neg( simplex.v1.w );+				break;++			case 2:+				d = b2SolveSimplex2( &simplex );+				break;++			case 3:+				d = b2SolveSimplex3( &simplex );+				break;++			default:+				B2_ASSERT( false );+		}++		// If we have 3 points, then the origin is in the corresponding triangle.+		if ( simplex.count == 3 )+		{+			// Overlap+			*cache = b2MakeSimplexCache( &simplex );+			b2Vec2 localPointA, localPointB;+			b2ComputeWitnessPoints( &simplex, &localPointA, &localPointB );+			output.distance = 0.0f;+			output.pointA = localPointA;+			output.pointB = localPointB;+			return output;+		}++#ifndef NDEBUG+		if ( simplexes != NULL && simplexIndex < simplexCapacity )+		{+			simplexes[simplexIndex] = simplex;+			simplexIndex += 1;+		}+#endif++		// Ensure the search direction is numerically fit.+		if ( b2Dot( d, d ) < 1000.0f * FLT_MIN )+		{+			// This is unlikely but could lead to bad cycling.+			// The branch predictor seems to make this check have low cost.++			// The origin is probably contained by a line segment+			// or triangle. Thus the shapes are overlapped.++			*cache = b2MakeSimplexCache( &simplex );++			// Must return overlap due to invalid normal.+			b2Vec2 localPointA, localPointB;+			b2ComputeWitnessPoints( &simplex, &localPointA, &localPointB );+			output.distance = 0.0f;+			output.pointA = localPointA;+			output.pointB = localPointB;+			return output;+		}++		// Save the normal+		nonUnitNormal = d;++		// Compute a tentative new simplex vertex using support points.+		// support = support(a, d) - support(b, -d)+		b2SimplexVertex* vertex = vertices[simplex.count];+		vertex->indexA = b2FindSupport( proxyA, d );+		vertex->wA = proxyA->points[vertex->indexA];+		vertex->indexB = b2FindSupport( &localProxyB, b2Neg( d ) );+		vertex->wB = localProxyB.points[vertex->indexB];+		vertex->w = b2Sub( vertex->wA, vertex->wB );++		// Iteration count is equated to the number of support point calls.+		++iteration;++		// Check for duplicate support points. This is the main termination criteria.+		bool duplicate = false;+		for ( int i = 0; i < saveCount; ++i )+		{+			if ( vertex->indexA == saveA[i] && vertex->indexB == saveB[i] )+			{+				duplicate = true;+				break;+			}+		}++		// If we found a duplicate support point we must exit to avoid cycling.+		if ( duplicate )+		{+			break;+		}++		// New vertex is valid and needed.+		simplex.count += 1;+	}++#ifndef NDEBUG+	if ( simplexes != NULL && simplexIndex < simplexCapacity )+	{+		simplexes[simplexIndex] = simplex;+		simplexIndex += 1;+	}+#endif++	// Prepare output in frame A+	b2Vec2 normal = b2Normalize( nonUnitNormal );+	B2_ASSERT( b2IsNormalized( normal ) );++	b2Vec2 localPointA, localPointB;+	b2ComputeWitnessPoints( &simplex, &localPointA, &localPointB );+	output.normal = normal;+	output.distance = b2Distance( localPointA, localPointB );+	output.pointA = localPointA;+	output.pointB = localPointB;+	output.iterations = iteration;+	output.simplexCount = simplexIndex;++	// Cache the simplex+	*cache = b2MakeSimplexCache( &simplex );++	// Apply radii if requested+	if ( input->useRadii )+	{+		float radiusA = input->proxyA.radius;+		float radiusB = input->proxyB.radius;+		output.distance = b2MaxFloat( 0.0f, output.distance - radiusA - radiusB );++		// Keep closest points on perimeter even if overlapped, this way the points move smoothly.+		output.pointA = b2MulAdd( output.pointA, radiusA, normal );+		output.pointB = b2MulSub( output.pointB, radiusB, normal );+	}++	return output;+}++// Shape cast using conservative advancement+b2CastOutput b2CastShape( const b2ShapeCastPairInput* input )+{+	// Compute tolerance+	float linearSlop = B2_LINEAR_SLOP;+	float totalRadius = input->proxyA.radius + input->proxyB.radius;+	float target = b2MaxFloat( linearSlop, totalRadius - linearSlop );+	float tolerance = 0.25f * linearSlop;++	B2_ASSERT( target > tolerance );++	// Prepare input for distance query+	b2SimplexCache cache = { 0 };++	float fraction = 0.0f;++	b2DistanceInput 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;++	b2Vec2 delta2 = input->translationB;+	b2CastOutput output = { 0 };++	int iteration = 0;+	const int maxIterations = 20;++	for ( ; iteration < maxIterations; ++iteration )+	{+		output.iterations += 1;++		b2DistanceOutput distanceOutput = b2ComputeShapeDistance( &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+					b2Vec2 c1 = b2MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal );+					b2Vec2 c2 = b2MulAdd( distanceOutput.pointB, -input->proxyB.radius, distanceOutput.normal );+					output.point = b2Lerp( c1, c2, 0.5f );+					return output;+				}+			}+			else+			{+				// Regular hit+				B2_ASSERT( distanceOutput.distance > 0.0f && b2IsNormalized( distanceOutput.normal ) );+				output.fraction = fraction;+				output.point = b2MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal );+				output.normal = distanceOutput.normal;+				output.hit = true;+				return output;+			}+		}++		B2_ASSERT( distanceOutput.distance > 0.0f );+		B2_ASSERT( b2IsNormalized( distanceOutput.normal ) );++		// Check if shapes are approaching each other+		float denominator = b2Dot( delta2, distanceOutput.normal );+		if ( denominator >= 0.0f )+		{+			// Miss+			return output;+		}++		// Advance sweep+		fraction += ( target - distanceOutput.distance ) / denominator;+		if ( fraction >= input->maxFraction )+		{+			// Miss+			return output;+		}++		distanceInput.transform.p = b2MulAdd( input->transform.p, fraction, delta2 );+	}++	// Failure!+	return output;+}++// Note: the code below is experimental and probably broken+#if 0++static inline b2Vec2 b2ComputeSimplexClosestPoint( const b2Simplex* s )+{+	if ( s->count == 1 )+	{+		return s->v1.w;+	}++	if ( s->count == 2 )+	{+		return b2Weight2( s->v1.a, s->v1.w, s->v2.a, s->v2.w );+	}++	return b2Vec2_zero;+}++typedef struct b2ShapeCastData+{+	b2Simplex simplex;+	b2Vec2 closestA, closestB;+	b2Vec2 normal;+	b2Vec2 p0;+	float fraction;+} b2ShapeCastData;++// GJK-raycast+// Algorithm by Gino van den Bergen.+// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010+// This needs the simplex of A - B because the translation is for B and this+// is how the relative motion works out when both shapes are translating.+// This is similar to ray vs polygon and involves plane clipping. See b2CastRayPolygon.+// In this case the polygon is just points and there are no planes. This uses a modified+// version of GJK to generate planes for clipping.+// The algorithm works by incrementally building clipping planes using GJK. Once a valid+// clip plane is found the simplex origin is moved to the current fraction on the ray.+// This resets the simplex after every clip. Later I should compare performance.+// However, adapting this to work with encroachment is tricky and confusing because encroachment+// needs distance.+// Note: this algorithm is difficult to debug and not worth the effort in my opinion 4/1/2025+b2CastOutput b2ShapeCastMerged( const b2ShapeCastPairInput* input, b2ShapeCastData* debugData, int debugCapacity )+{+	B2_UNUSED( debugData, debugCapacity );++	b2CastOutput output = { 0 };+	output.fraction = input->maxFraction;++	b2ShapeProxy proxyA = input->proxyA;++	b2Transform xf = input->transform;++	// Put proxyB in proxyA's frame to reduce round-off error+	b2ShapeProxy proxyB;+	proxyB.count = input->proxyB.count;+	proxyB.radius = input->proxyB.radius;+	B2_ASSERT( proxyB.count <= B2_MAX_POLYGON_VERTICES );++	for ( int i = 0; i < proxyB.count; ++i )+	{+		proxyB.points[i] = b2TransformPoint( xf, input->proxyB.points[i] );+	}++	float radius = proxyA.radius + proxyB.radius;++	b2Vec2 r = input->translationB;+	float lambda = 0.0f;+	float maxFraction = input->maxFraction;++	// Initial simplex+	b2Simplex simplex;+	simplex.count = 0;++	// Get simplex vertices as an array.+	b2SimplexVertex* vertices[] = { &simplex.v1, &simplex.v2, &simplex.v3 };++	// Get an initial point in A - B+	b2Vec2 wA = proxyA.points[0];+	b2Vec2 wB = proxyB.points[0];+	b2Vec2 v = b2Sub( wA, wB );+	b2Vec2 d = b2Neg( v );++	// Sigma is the target distance between proxies+	const float linearSlop = B2_LINEAR_SLOP;+	const float sigma = b2MaxFloat( linearSlop, radius - linearSlop );+	float tolerance = 0.5f * linearSlop;+	float stolSquared = ( sigma + tolerance ) * ( sigma + tolerance );++	// Main iteration loop.+	const int maxIterations = 20;+	int iteration = 0;+	while ( iteration < maxIterations && b2LengthSquared( v ) > stolSquared )+	{+		B2_ASSERT( simplex.count < 3 );++		// Support in direction d (A - B)+		int indexA = b2FindSupport( &proxyA, d );+		wA = proxyA.points[indexA];+		int indexB = b2FindSupport( &proxyB, b2Neg( d ) );+		wB = proxyB.points[indexB];+		b2Vec2 p0 = b2Sub( wA, wB );++		// d is a normal at p, normalize to work with sigma+		b2Vec2 normal = b2Normalize( d );++		// Intersect ray with plane+		// p = origin + t * r+		// dot(n, p - p0) = sigma+		// dot(n, origin - p0) + t * dot(n, r) = sigma+		// t = ( dot(n, p0) + sigma) / dot(n, r)+		// if t < (dot(n, p0) + sigma) / dot(n, r) then t can be increased+		// or (flipping sign because dot(n,r) < 0)+		// dot(n, p0) + sigma < t * dot(n, r) && dot(n, r) < 0+		float np0 = b2Dot( normal, p0 );+		float nr = b2Dot( normal, r );+		if ( np0 + sigma < lambda * nr )+		{+			if ( nr >= 0.0f )+			{+				// miss+				return output;+			}++			lambda = ( np0 + sigma ) / nr;+			if ( lambda > maxFraction )+			{+				// too far+				return output;+			}++			// reset the simplex+			simplex.count = 0;+		}++		// Shift by lambda * r because we want the closest point to the current clip point.+		// Note that the support point p is not shifted because we want the plane equation+		// to be formed in un-shifted space.+		b2SimplexVertex* vertex = vertices[simplex.count];+		vertex->indexA = indexB;+		vertex->wA = wA;+		vertex->indexB = indexA;+		vertex->wB = (b2Vec2){ wB.x + lambda * r.x, wB.y + lambda * r.y };+		vertex->w = b2Sub( vertex->wA, vertex->wB );+		vertex->a = 1.0f;+		simplex.count += 1;++		switch ( simplex.count )+		{+			case 1:+				d = b2Neg( simplex.v1.w );+				break;++			case 2:+				d = b2SolveSimplex2( &simplex );+				break;++			case 3:+				d = b2SolveSimplex3( &simplex );+				break;++			default:+				B2_ASSERT( false );+		}++#ifndef NDEBUG+		if ( debugData != NULL && output.iterations < debugCapacity )+		{+			debugData[output.iterations].simplex = simplex;+			debugData[output.iterations].normal = normal;+			debugData[output.iterations].p0 = p0;+			b2Vec2 cA, cB;+			b2ComputeSimplexWitnessPoints( &cA, &cB, &simplex );+			debugData[output.iterations].closestA = cA;+			debugData[output.iterations].closestB = cB;+			debugData[output.iterations].fraction = lambda;+		}+#endif++		output.iterations += 1;++		// If we have 3 points, then the origin is in the corresponding triangle.+		if ( simplex.count == 3 )+		{+			// Overlap+			return output;+		}++		// Get distance vector+		v = b2ComputeSimplexClosestPoint( &simplex );++		// Iteration count is equated to the number of support point calls.+		++iteration;+	}++	if ( iteration == 0 || lambda == 0.0f )+	{+		// Initial overlap+		return output;+	}++	// Prepare output.+	b2Vec2 pointA, pointB;+	b2ComputeSimplexWitnessPoints( &pointB, &pointA, &simplex );++	b2Vec2 n = b2Normalize( b2Neg( v ) );+	b2Vec2 point = { pointA.x + proxyA.radius * n.x, pointA.y + proxyA.radius * n.y };++	// Results stay in frame A, matching b2CastShape+	output.point = point;+	output.normal = n;+	output.fraction = lambda;+	output.iterations = iteration;+	output.hit = true;+	return output;+}+#endif++// Warning: writing to these globals significantly slows multithreading performance+#if B2_SNOOP_TOI_COUNTERS+float b2_toiTime, b2_toiMaxTime;+int b2_toiCalls, b2_toiDistanceIterations, b2_toiMaxDistanceIterations;+int b2_toiRootIterations, b2_toiMaxRootIterations;+int b2_toiFailedCount;+int b2_toiOverlappedCount;+int b2_toiHitCount;+int b2_toiSeparatedCount;+#endif++typedef enum b2SeparationType+{+	b2_pointsType,+	b2_faceAType,+	b2_faceBType+} b2SeparationType;++typedef struct b2SeparationFunction+{+	const b2ShapeProxy* proxyA;+	const b2ShapeProxy* proxyB;+	b2Sweep sweepA, sweepB;+	b2Vec2 localPoint;+	b2Vec2 axis;+	b2SeparationType type;+} b2SeparationFunction;++static b2SeparationFunction b2MakeSeparationFunction( b2SimplexCache cache, const b2ShapeProxy* proxyA, const b2Sweep* sweepA,+													  const b2ShapeProxy* proxyB, const b2Sweep* sweepB, float t1 )+{+	b2SeparationFunction f;++	f.proxyA = proxyA;+	f.proxyB = proxyB;+	int count = cache.count;+	B2_ASSERT( 0 < count && count < 3 );++	f.sweepA = *sweepA;+	f.sweepB = *sweepB;++	b2Transform xfA = b2GetSweepTransform( sweepA, t1 );+	b2Transform xfB = b2GetSweepTransform( sweepB, t1 );++	if ( count == 1 )+	{+		f.type = b2_pointsType;+		b2Vec2 localPointA = proxyA->points[cache.indexA[0]];+		b2Vec2 localPointB = proxyB->points[cache.indexB[0]];+		b2Vec2 pointA = b2TransformPoint( xfA, localPointA );+		b2Vec2 pointB = b2TransformPoint( xfB, localPointB );+		f.axis = b2Normalize( b2Sub( pointB, pointA ) );+		f.localPoint = b2Vec2_zero;+		return f;+	}++	if ( cache.indexA[0] == cache.indexA[1] )+	{+		// Two points on B and one on A.+		f.type = b2_faceBType;+		b2Vec2 localPointB1 = proxyB->points[cache.indexB[0]];+		b2Vec2 localPointB2 = proxyB->points[cache.indexB[1]];++		f.axis = b2CrossVS( b2Sub( localPointB2, localPointB1 ), 1.0f );+		f.axis = b2Normalize( f.axis );+		b2Vec2 normal = b2RotateVector( xfB.q, f.axis );++		f.localPoint = (b2Vec2){ 0.5f * ( localPointB1.x + localPointB2.x ), 0.5f * ( localPointB1.y + localPointB2.y ) };+		b2Vec2 pointB = b2TransformPoint( xfB, f.localPoint );++		b2Vec2 localPointA = proxyA->points[cache.indexA[0]];+		b2Vec2 pointA = b2TransformPoint( xfA, localPointA );++		float s = b2Dot( b2Sub( pointA, pointB ), normal );+		if ( s < 0.0f )+		{+			f.axis = b2Neg( f.axis );+		}+		return f;+	}++	// Two points on A and one or two points on B.+	f.type = b2_faceAType;+	b2Vec2 localPointA1 = proxyA->points[cache.indexA[0]];+	b2Vec2 localPointA2 = proxyA->points[cache.indexA[1]];++	f.axis = b2CrossVS( b2Sub( localPointA2, localPointA1 ), 1.0f );+	f.axis = b2Normalize( f.axis );+	b2Vec2 normal = b2RotateVector( xfA.q, f.axis );++	f.localPoint = (b2Vec2){ 0.5f * ( localPointA1.x + localPointA2.x ), 0.5f * ( localPointA1.y + localPointA2.y ) };+	b2Vec2 pointA = b2TransformPoint( xfA, f.localPoint );++	b2Vec2 localPointB = proxyB->points[cache.indexB[0]];+	b2Vec2 pointB = b2TransformPoint( xfB, localPointB );++	float s = b2Dot( b2Sub( pointB, pointA ), normal );+	if ( s < 0.0f )+	{+		f.axis = b2Neg( f.axis );+	}+	return f;+}++static float b2FindMinSeparation( const b2SeparationFunction* f, int* indexA, int* indexB, float t )+{+	b2Transform xfA = b2GetSweepTransform( &f->sweepA, t );+	b2Transform xfB = b2GetSweepTransform( &f->sweepB, t );++	switch ( f->type )+	{+		case b2_pointsType:+		{+			b2Vec2 axisA = b2InvRotateVector( xfA.q, f->axis );+			b2Vec2 axisB = b2InvRotateVector( xfB.q, b2Neg( f->axis ) );++			*indexA = b2FindSupport( f->proxyA, axisA );+			*indexB = b2FindSupport( f->proxyB, axisB );++			b2Vec2 localPointA = f->proxyA->points[*indexA];+			b2Vec2 localPointB = f->proxyB->points[*indexB];++			b2Vec2 pointA = b2TransformPoint( xfA, localPointA );+			b2Vec2 pointB = b2TransformPoint( xfB, localPointB );++			float separation = b2Dot( b2Sub( pointB, pointA ), f->axis );+			return separation;+		}++		case b2_faceAType:+		{+			b2Vec2 normal = b2RotateVector( xfA.q, f->axis );+			b2Vec2 pointA = b2TransformPoint( xfA, f->localPoint );++			b2Vec2 axisB = b2InvRotateVector( xfB.q, b2Neg( normal ) );++			*indexA = -1;+			*indexB = b2FindSupport( f->proxyB, axisB );++			b2Vec2 localPointB = f->proxyB->points[*indexB];+			b2Vec2 pointB = b2TransformPoint( xfB, localPointB );++			float separation = b2Dot( b2Sub( pointB, pointA ), normal );+			return separation;+		}++		case b2_faceBType:+		{+			b2Vec2 normal = b2RotateVector( xfB.q, f->axis );+			b2Vec2 pointB = b2TransformPoint( xfB, f->localPoint );++			b2Vec2 axisA = b2InvRotateVector( xfA.q, b2Neg( normal ) );++			*indexB = -1;+			*indexA = b2FindSupport( f->proxyA, axisA );++			b2Vec2 localPointA = f->proxyA->points[*indexA];+			b2Vec2 pointA = b2TransformPoint( xfA, localPointA );++			float separation = b2Dot( b2Sub( pointA, pointB ), normal );+			return separation;+		}++		default:+			B2_ASSERT( false );+			*indexA = -1;+			*indexB = -1;+			return 0.0f;+	}+}++//+static float b2EvaluateSeparation( const b2SeparationFunction* f, int indexA, int indexB, float t )+{+	b2Transform xfA = b2GetSweepTransform( &f->sweepA, t );+	b2Transform xfB = b2GetSweepTransform( &f->sweepB, t );++	switch ( f->type )+	{+		case b2_pointsType:+		{+			b2Vec2 localPointA = f->proxyA->points[indexA];+			b2Vec2 localPointB = f->proxyB->points[indexB];++			b2Vec2 pointA = b2TransformPoint( xfA, localPointA );+			b2Vec2 pointB = b2TransformPoint( xfB, localPointB );++			float separation = b2Dot( b2Sub( pointB, pointA ), f->axis );+			return separation;+		}++		case b2_faceAType:+		{+			b2Vec2 normal = b2RotateVector( xfA.q, f->axis );+			b2Vec2 pointA = b2TransformPoint( xfA, f->localPoint );++			b2Vec2 localPointB = f->proxyB->points[indexB];+			b2Vec2 pointB = b2TransformPoint( xfB, localPointB );++			float separation = b2Dot( b2Sub( pointB, pointA ), normal );+			return separation;+		}++		case b2_faceBType:+		{+			b2Vec2 normal = b2RotateVector( xfB.q, f->axis );+			b2Vec2 pointB = b2TransformPoint( xfB, f->localPoint );++			b2Vec2 localPointA = f->proxyA->points[indexA];+			b2Vec2 pointA = b2TransformPoint( xfA, localPointA );++			float separation = b2Dot( b2Sub( pointA, pointB ), normal );+			return separation;+		}++		default:+			B2_ASSERT( false );+			return 0.0f;+	}+}++// CCD via the local separating axis method. This seeks progression+// by computing the largest time at which separation is maintained.+b2TOIOutput b2ComputeTimeOfImpact( const b2TOIInput* input )+{+#if B2_SNOOP_TOI_COUNTERS+	uint64_t ticks = b2GetTicks();+	++b2_toiCalls;+#endif++	b2TOIOutput output = { 0 };+	output.state = b2_toiStateUnknown;+	output.fraction = input->maxFraction;++	b2Sweep sweepA = input->sweepA;+	b2Sweep sweepB = input->sweepB;+	B2_ASSERT( b2IsNormalizedRot( sweepA.q1 ) && b2IsNormalizedRot( sweepA.q2 ) );+	B2_ASSERT( b2IsNormalizedRot( sweepB.q1 ) && b2IsNormalizedRot( sweepB.q2 ) );++	// todo_erin+	// c1 can be at the origin yet the points are far away+	// b2Vec2 origin = b2Add(sweepA.c1, input->proxyA.points[0]);++	const b2ShapeProxy* proxyA = &input->proxyA;+	const b2ShapeProxy* proxyB = &input->proxyB;++	float tMax = input->maxFraction;++	// Setup target distance and tolerance+	float totalRadius = proxyA->radius + proxyB->radius;+	float target = b2MaxFloat( B2_LINEAR_SLOP, totalRadius - B2_LINEAR_SLOP );+	float tolerance = 0.25f * B2_LINEAR_SLOP;+	B2_ASSERT( target > tolerance );++	float t1 = 0.0f;+	const int k_maxIterations = 20;+	int distanceIterations = 0;++	// Prepare input for distance query.+	b2SimplexCache cache = { 0 };+	b2DistanceInput distanceInput = { 0 };+	distanceInput.proxyA = input->proxyA;+	distanceInput.proxyB = input->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.+		b2Transform xfA = b2GetSweepTransform( &sweepA, t1 );+		b2Transform xfB = b2GetSweepTransform( &sweepB, t1 );+		distanceInput.transform = b2InvMulTransforms( xfA, xfB );+		b2DistanceOutput distanceOutput = b2ComputeShapeDistance( &distanceInput, &cache, NULL, 0 );++		// The distance query runs in frame A, project the witness data back to world+		b2Vec2 worldNormal = b2RotateVector( xfA.q, distanceOutput.normal );+		b2Vec2 worldPointA = b2TransformPoint( xfA, distanceOutput.pointA );+		b2Vec2 worldPointB = b2TransformPoint( xfA, distanceOutput.pointB );++		// Progressive time of impact. This handles slender geometry well but introduces+		// significant time loss.+		// if (distanceIterations == 0)+		//{+		//	if ( distanceOutput.distance > totalRadius + B2_SPECULATIVE_DISTANCE )+		//	{+		//		target = totalRadius + B2_SPECULATIVE_DISTANCE - tolerance;+		//	}+		//	else+		//	{+		//		target = distanceOutput.distance - 1.5f * tolerance;+		//		target = b2MaxFloat( target, 2.0f * tolerance );+		//	}+		//}++		distanceIterations += 1;+#if B2_SNOOP_TOI_COUNTERS+		b2_toiDistanceIterations += 1;+#endif++		// If the shapes are overlapped, we give up on continuous collision.+		if ( distanceOutput.distance <= 0.0f )+		{+			// Failure!+			output.state = b2_toiStateOverlapped;+#if B2_SNOOP_TOI_COUNTERS+			b2_toiOverlappedCount += 1;+#endif+			output.fraction = 0.0f;+			break;+		}++		if ( distanceOutput.distance <= target + tolerance )+		{+			// Success!+			output.state = b2_toiStateHit;+#if B2_SNOOP_TOI_COUNTERS+			b2_toiHitCount += 1;+#endif+			// Averaged hit point+			b2Vec2 pA = b2MulAdd( worldPointA, proxyA->radius, worldNormal );+			b2Vec2 pB = b2MulAdd( worldPointB, -proxyB->radius, worldNormal );+			output.point = b2Lerp( pA, pB, 0.5f );+			output.normal = worldNormal;+			output.fraction = t1;+			break;+		}++		// Initialize the separating axis.+		b2SeparationFunction fcn = b2MakeSeparationFunction( cache, proxyA, &sweepA, proxyB, &sweepB, t1 );+#if 0+		// Dump the curve seen by the root finder+		{+			const int N = 100;+			float dx = 1.0f / N;+			float xs[N + 1];+			float fs[N + 1];++			float x = 0.0f;++			for (int i = 0; i <= N; ++i)+			{+				sweepA.GetTransform(&xfA, x);+				sweepB.GetTransform(&xfB, x);+				float f = fcn.Evaluate(xfA, xfB) - target;++				printf("%g %g\n", x, f);++				xs[i] = x;+				fs[i] = f;++				x += dx;+			}+		}+#endif++		// Compute the TOI on the separating axis. We do this by successively+		// resolving the deepest point. This loop is bounded by the number of vertices.+		bool done = false;+		float t2 = tMax;+		int pushBackIterations = 0;+		for ( ;; )+		{+			// Find the deepest point at t2. Store the witness point indices.+			int indexA, indexB;+			float s2 = b2FindMinSeparation( &fcn, &indexA, &indexB, t2 );++			// Is the final configuration separated?+			if ( s2 > target + tolerance )+			{+				// Victory!+				output.state = b2_toiStateSeparated;+#if B2_SNOOP_TOI_COUNTERS+				b2_toiSeparatedCount += 1;+#endif+				output.fraction = tMax;+				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 = b2EvaluateSeparation( &fcn, indexA, indexB, t1 );++			// Check for initial overlap. This might happen if the root finder runs out of iterations.+			if ( s1 < target - tolerance )+			{+				output.state = b2_toiStateFailed;+#if B2_SNOOP_TOI_COUNTERS+				b2_toiFailedCount += 1;+#endif+				output.fraction = t1;+				done = true;+				break;+			}++			// Check for touching+			if ( s1 <= target + tolerance )+			{+				// Success! t1 should hold the TOI (could be 0.0).+				output.state = b2_toiStateHit;+#if B2_SNOOP_TOI_COUNTERS+				b2_toiHitCount += 1;+#endif+				// Averaged hit point+				b2Vec2 pA = b2MulAdd( worldPointA, proxyA->radius, worldNormal );+				b2Vec2 pB = b2MulAdd( worldPointB, -proxyB->radius, worldNormal );+				output.point = b2Lerp( pA, pB, 0.5f );+				output.normal = worldNormal;+				output.fraction = t1;+				done = true;+				break;+			}++			// Compute 1D root of: f(x) - target = 0+			int rootIterationCount = 0;+			float a1 = t1, 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 );+				}++				rootIterationCount += 1;++#if B2_SNOOP_TOI_COUNTERS+				++b2_toiRootIterations;+#endif++				float s = b2EvaluateSeparation( &fcn, indexA, indexB, t );++				// Has the separation reached tolerance?+				if ( b2AbsFloat( 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 == 50 )+				{+					break;+				}+			}++#if B2_SNOOP_TOI_COUNTERS+			b2_toiMaxRootIterations = b2MaxInt( b2_toiMaxRootIterations, rootIterationCount );+#endif++			pushBackIterations += 1;++			if ( pushBackIterations == B2_MAX_POLYGON_VERTICES )+			{+				break;+			}+		}++		if ( done )+		{+			break;+		}++		if ( distanceIterations == k_maxIterations )+		{+			// Root finder got stuck. Semi-victory.+			output.state = b2_toiStateFailed;+#if B2_SNOOP_TOI_COUNTERS+			b2_toiFailedCount += 1;+#endif+			// Averaged hit point+			b2Vec2 pA = b2MulAdd( worldPointA, proxyA->radius, worldNormal );+			b2Vec2 pB = b2MulAdd( worldPointB, -proxyB->radius, worldNormal );+			output.point = b2Lerp( pA, pB, 0.5f );+			output.normal = worldNormal;+			output.fraction = t1;+			break;+		}+	}++#if B2_SNOOP_TOI_COUNTERS+	b2_toiMaxDistanceIterations = b2MaxInt( b2_toiMaxDistanceIterations, distanceIterations );++	float time = b2GetMilliseconds( ticks );+	b2_toiMaxTime = b2MaxFloat( b2_toiMaxTime, time );+	b2_toiTime += time;+#endif++	return output;+}
+ upstream/src/distance_joint.c view
@@ -0,0 +1,616 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS )+#define _CRT_SECURE_NO_WARNINGS+#endif++#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 "box2d/box2d.h"++void b2DistanceJoint_SetRestLength( b2JointId jointId, float length )+{+	B2_CHECK_INPUT( b2IsValidFloat( length ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointSetRestLength, jointId, length );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	b2DistanceJoint* joint = &base->distanceJoint;++	joint->length = b2ClampFloat( length, B2_LINEAR_SLOP, B2_HUGE );+	joint->impulse = 0.0f;+	joint->lowerImpulse = 0.0f;+	joint->upperImpulse = 0.0f;+}++float b2DistanceJoint_GetRestLength( b2JointId jointId )+{+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	b2DistanceJoint* joint = &base->distanceJoint;+	return joint->length;+}++void b2DistanceJoint_EnableLimit( b2JointId jointId, bool enableLimit )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointEnableLimit, jointId, enableLimit );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	b2DistanceJoint* joint = &base->distanceJoint;+	joint->enableLimit = enableLimit;+}++bool b2DistanceJoint_IsLimitEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	return joint->distanceJoint.enableLimit;+}++void b2DistanceJoint_SetLimits( b2JointId jointId, float minLength, float maxLength )+{+	B2_CHECK_INPUT( b2IsValidFloat( minLength ) );+	B2_CHECK_INPUT( b2IsValidFloat( maxLength ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointSetLimits, jointId, minLength, maxLength );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	b2DistanceJoint* joint = &base->distanceJoint;++	minLength = b2ClampFloat( minLength, B2_LINEAR_SLOP, B2_HUGE );+	maxLength = b2ClampFloat( maxLength, B2_LINEAR_SLOP, B2_HUGE );+	joint->minLength = b2MinFloat( minLength, maxLength );+	joint->maxLength = b2MaxFloat( minLength, maxLength );+}++float b2DistanceJoint_GetLowerLimit( b2JointId jointId )+{+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	b2DistanceJoint* joint = &base->distanceJoint;+	return joint->minLength;+}++float b2DistanceJoint_GetUpperLimit( b2JointId jointId )+{+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	b2DistanceJoint* joint = &base->distanceJoint;+	return joint->maxLength;+}++float b2DistanceJoint_GetCurrentLength( b2JointId jointId )+{+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return 0.0f;+	}++	// Relative to body A so the difference stays in float precision far from the origin+	b2WorldTransform wxfA = b2GetBodyTransform( world, base->bodyIdA );+	b2Transform transformA = b2ToRelativeTransform( wxfA, wxfA.p );+	b2Transform transformB = b2ToRelativeTransform( b2GetBodyTransform( world, base->bodyIdB ), wxfA.p );++	b2Vec2 pA = b2TransformPoint( transformA, base->localFrameA.p );+	b2Vec2 pB = b2TransformPoint( transformB, base->localFrameB.p );+	b2Vec2 d = b2Sub( pB, pA );+	float length = b2Length( d );+	return length;+}++void b2DistanceJoint_EnableSpring( b2JointId jointId, bool enableSpring )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointEnableSpring, jointId, enableSpring );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	base->distanceJoint.enableSpring = enableSpring;+}++bool b2DistanceJoint_IsSpringEnabled( b2JointId jointId )+{+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	return base->distanceJoint.enableSpring;+}++void b2DistanceJoint_SetSpringForceRange( b2JointId jointId, float lowerForce, float upperForce )+{+	B2_CHECK_INPUT( b2IsValidFloat( lowerForce ) );+	B2_CHECK_INPUT( b2IsValidFloat( upperForce ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointSetSpringForceRange, jointId, lowerForce, upperForce );+	B2_ASSERT( lowerForce <= upperForce );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	base->distanceJoint.lowerSpringForce = b2MinFloat( lowerForce, upperForce );+	base->distanceJoint.upperSpringForce = b2MaxFloat( lowerForce, upperForce );+}++void b2DistanceJoint_GetSpringForceRange( b2JointId jointId, float* lowerForce, float* upperForce )+{+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	*lowerForce = base->distanceJoint.lowerSpringForce;+	*upperForce = base->distanceJoint.upperSpringForce;+}++void b2DistanceJoint_SetSpringHertz( b2JointId jointId, float hertz )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointSetSpringHertz, jointId, hertz );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	base->distanceJoint.hertz = hertz;+}++void b2DistanceJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )+{+	B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointSetSpringDampingRatio, jointId, dampingRatio );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	base->distanceJoint.dampingRatio = dampingRatio;+}++float b2DistanceJoint_GetSpringHertz( b2JointId jointId )+{+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	b2DistanceJoint* joint = &base->distanceJoint;+	return joint->hertz;+}++float b2DistanceJoint_GetSpringDampingRatio( b2JointId jointId )+{+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	b2DistanceJoint* joint = &base->distanceJoint;+	return joint->dampingRatio;+}++float b2DistanceJoint_GetSpringForce( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	b2DistanceJoint* joint = &base->distanceJoint;+	if ( joint->enableSpring )+	{+		return joint->impulse * world->inv_h;+	}++	return 0.0f;+}++void b2DistanceJoint_EnableMotor( b2JointId jointId, bool enableMotor )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointEnableMotor, jointId, enableMotor );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	if ( enableMotor != joint->distanceJoint.enableMotor )+	{+		joint->distanceJoint.enableMotor = enableMotor;+		joint->distanceJoint.motorImpulse = 0.0f;+	}+}++bool b2DistanceJoint_IsMotorEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	return joint->distanceJoint.enableMotor;+}++void b2DistanceJoint_SetTargetMotorSpeed( b2JointId jointId, float motorSpeed )+{+	B2_CHECK_INPUT( b2IsValidFloat( motorSpeed ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointSetTargetMotorSpeed, jointId, motorSpeed );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	joint->distanceJoint.motorSpeed = motorSpeed;+}++float b2DistanceJoint_GetTargetMotorSpeed( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	return joint->distanceJoint.motorSpeed;+}++float b2DistanceJoint_GetCurrentMotorForce( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	return world->inv_h * base->distanceJoint.motorImpulse;+}++void b2DistanceJoint_SetMaxMotorForce( b2JointId jointId, float force )+{+	B2_CHECK_INPUT( b2IsValidFloat( force ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, DistanceJointSetMaxMotorForce, jointId, force );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	joint->distanceJoint.maxMotorForce = force;+}++float b2DistanceJoint_GetMaxMotorForce( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );+	return joint->distanceJoint.maxMotorForce;+}++b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base )+{+	b2DistanceJoint* joint = &base->distanceJoint;++	b2WorldTransform worldTransformA = b2GetBodyTransform( world, base->bodyIdA );+	b2Transform transformA = b2ToRelativeTransform( worldTransformA, worldTransformA.p );+	b2WorldTransform worldTransformB = b2GetBodyTransform( world, base->bodyIdB );+	b2Transform transformB = b2ToRelativeTransform( worldTransformB, worldTransformA.p );++	b2Vec2 pA = b2TransformPoint( transformA, base->localFrameA.p );+	b2Vec2 pB = b2TransformPoint( transformB, base->localFrameB.p );+	b2Vec2 d = b2Sub( pB, pA );+	b2Vec2 axis = b2Normalize( d );+	float force = ( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ) * world->inv_h;+	return b2MulSV( 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 b2PrepareDistanceJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_distanceJoint );++	// chase body id to the solver set where the body lives+	int idA = base->bodyIdA;+	int idB = base->bodyIdB;++	b2World* world = context->world;+	b2Body* bodyA = b2Array_Get( world->bodies, idA );+	b2Body* bodyB = b2Array_Get( world->bodies, idB );++	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );++	b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex );+	b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex );++	int localIndexA = bodyA->localIndex;+	int localIndexB = bodyB->localIndex;++	b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA );+	b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB );++	float mA = bodySimA->invMass;+	float iA = bodySimA->invInertia;+	float mB = bodySimB->invMass;+	float iB = bodySimB->invInertia;++	base->invMassA = mA;+	base->invMassB = mB;+	base->invIA = iA;+	base->invIB = iB;++	b2DistanceJoint* joint = &base->distanceJoint;++	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;++	// initial anchors in world space+	joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );+	joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );+	joint->deltaCenter = b2SubPos( bodySimB->center, bodySimA->center );++	b2Vec2 rA = joint->anchorA;+	b2Vec2 rB = joint->anchorB;+	b2Vec2 separation = b2Add( b2Sub( rB, rA ), joint->deltaCenter );+	b2Vec2 axis = b2Normalize( separation );++	// compute effective mass+	float crA = b2Cross( rA, axis );+	float crB = b2Cross( rB, axis );+	float k = mA + mB + iA * crA * crA + iB * crB * crB;+	joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f;++	joint->distanceSoftness = b2MakeSoft( 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 b2WarmStartDistanceJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_distanceJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2DistanceJoint* joint = &base->distanceJoint;+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );++	b2Vec2 ds = b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), b2Sub( rB, rA ) );+	b2Vec2 separation = b2Add( joint->deltaCenter, ds );+	b2Vec2 axis = b2Normalize( separation );++	float axialImpulse = joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse;+	b2Vec2 P = b2MulSV( axialImpulse, axis );++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P );+		stateA->angularVelocity -= iA * b2Cross( rA, P );+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P );+		stateB->angularVelocity += iB * b2Cross( rB, P );+	}+}++void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBias )+{+	B2_ASSERT( base->type == b2_distanceJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2DistanceJoint* joint = &base->distanceJoint;+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 vA = stateA->linearVelocity;+	float wA = stateA->angularVelocity;+	b2Vec2 vB = stateB->linearVelocity;+	float wB = stateB->angularVelocity;++	// current anchors+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );++	// current separation+	b2Vec2 ds = b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), b2Sub( rB, rA ) );+	b2Vec2 separation = b2Add( joint->deltaCenter, ds );++	float length = b2Length( separation );+	b2Vec2 axis = b2Normalize( 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))+			b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) );+			float cdot = b2Dot( axis, vr );+			float c = length - joint->length;+			float bias = joint->distanceSoftness.biasRate * c;++			float 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 = b2ClampFloat( joint->impulse + impulse, joint->lowerSpringForce * h, joint->upperSpringForce * h );+			impulse = joint->impulse - oldImpulse;++			b2Vec2 P = b2MulSV( impulse, axis );+			vA = b2MulSub( vA, mA, P );+			wA -= iA * b2Cross( rA, P );+			vB = b2MulAdd( vB, mB, P );+			wB += iB * b2Cross( rB, P );+		}++		if ( joint->enableMotor )+		{+			b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) );+			float Cdot = b2Dot( axis, vr );+			float impulse = joint->axialMass * ( joint->motorSpeed - Cdot );+			float oldImpulse = joint->motorImpulse;+			float maxImpulse = context->h * joint->maxMotorForce;+			joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse );+			impulse = joint->motorImpulse - oldImpulse;++			b2Vec2 P = b2MulSV( impulse, axis );+			vA = b2MulSub( vA, mA, P );+			wA -= iA * b2Cross( rA, P );+			vB = b2MulAdd( vB, mB, P );+			wB += iB * b2Cross( rB, P );+		}++		if ( joint->enableLimit )+		{+			// lower limit+			{+				b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) );+				float cdot = b2Dot( axis, vr );++				float c = length - joint->minLength;++				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->lowerImpulse;+				float newImpulse = b2MaxFloat( 0.0f, joint->lowerImpulse + impulse );+				impulse = newImpulse - joint->lowerImpulse;+				joint->lowerImpulse = newImpulse;++				b2Vec2 P = b2MulSV( impulse, axis );+				vA = b2MulSub( vA, mA, P );+				wA -= iA * b2Cross( rA, P );+				vB = b2MulAdd( vB, mB, P );+				wB += iB * b2Cross( rB, P );+			}++			// upper+			{+				b2Vec2 vr = b2Add( b2Sub( vA, vB ), b2Sub( b2CrossSV( wA, rA ), b2CrossSV( wB, rB ) ) );+				float Cdot = b2Dot( 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 = b2MaxFloat( 0.0f, joint->upperImpulse + impulse );+				impulse = newImpulse - joint->upperImpulse;+				joint->upperImpulse = newImpulse;++				b2Vec2 P = b2MulSV( -impulse, axis );+				vA = b2MulSub( vA, mA, P );+				wA -= iA * b2Cross( rA, P );+				vB = b2MulAdd( vB, mB, P );+				wB += iB * b2Cross( rB, P );+			}+		}+	}+	else+	{+		// rigid constraint+		b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) );+		float Cdot = b2Dot( axis, vr );++		float C = length - joint->length;++		float bias = 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;++		b2Vec2 P = b2MulSV( impulse, axis );+		vA = b2MulSub( vA, mA, P );+		wA -= iA * b2Cross( rA, P );+		vB = b2MulAdd( vB, mB, P );+		wB += iB * b2Cross( rB, P );+	}++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = vA;+		stateA->angularVelocity = wA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = vB;+		stateB->angularVelocity = wB;+	}+}++void b2DrawDistanceJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB )+{+	B2_ASSERT( base->type == b2_distanceJoint );++	b2DistanceJoint* joint = &base->distanceJoint;++	b2Pos pA = b2TransformWorldPoint( transformA, base->localFrameA.p );+	b2Pos pB = b2TransformWorldPoint( transformB, base->localFrameB.p );++	b2Vec2 axis = b2Normalize( b2SubPos( pB, pA ) );++	if ( joint->minLength < joint->maxLength && joint->enableLimit )+	{+		b2Pos pMin = b2OffsetPos( pA, b2MulSV( joint->minLength, axis ) );+		b2Pos pMax = b2OffsetPos( pA, b2MulSV( joint->maxLength, axis ) );+		b2Vec2 offset = b2MulSV( 0.05f * b2GetLengthUnitsPerMeter(), b2RightPerp( axis ) );++		if ( joint->minLength > B2_LINEAR_SLOP )+		{+			draw->DrawLineFcn( b2OffsetPos( pMin, b2Neg( offset ) ), b2OffsetPos( pMin, offset ), b2_colorLightGreen,+							   draw->context );+		}++		if ( joint->maxLength < B2_HUGE )+		{+			draw->DrawLineFcn( b2OffsetPos( pMax, b2Neg( offset ) ), b2OffsetPos( pMax, offset ), b2_colorRed, draw->context );+		}++		if ( joint->minLength > B2_LINEAR_SLOP && joint->maxLength < B2_HUGE )+		{+			draw->DrawLineFcn( pMin, pMax, b2_colorGray, draw->context );+		}+	}++	draw->DrawLineFcn( pA, pB, b2_colorWhite, draw->context );+	draw->DrawPointFcn( pA, 4.0f, b2_colorWhite, draw->context );+	draw->DrawPointFcn( pB, 4.0f, b2_colorWhite, draw->context );++	if ( joint->hertz > 0.0f && joint->enableSpring )+	{+		b2Pos pRest = b2OffsetPos( pA, b2MulSV( joint->length, axis ) );+		draw->DrawPointFcn( pRest, 4.0f, b2_colorBlue, draw->context );+	}+}
+ upstream/src/dynamic_tree.c view
@@ -0,0 +1,2155 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "dynamic_tree.h"++#include "aabb.h"+#include "core.h"+#include "platform.h"+#include "simd.h"++#include "box2d/collision.h"+#include "box2d/constants.h"+#include "box2d/math_functions.h"++#include <float.h>+#include <string.h>++_Static_assert( B2_ROOT_NODE == 0, "bad root" );+_Static_assert( sizeof( b2TreeNode ) == 32, "expected size" );+_Static_assert( sizeof( b2TreeProxy ) == 24, "expected size" );++// The free sibling pair list is chained through the parent index of their first node. A pair comes from the+// free list when there is one and from the bump pointer otherwise. A rebuild empties the free list, so+// allocations can just bump.+static int b2AllocateSiblingPair( b2DynamicTree* tree )+{+	// Allocate from free list.+	if ( tree->pairFreeList != B2_NULL_INDEX )+	{+		int pair = tree->pairFreeList;+		tree->pairFreeList = tree->parents[pair];+		return pair;+	}++	// Extend the bump allocator if needed.+	if ( tree->nodeEnd + 2 > tree->nodeCapacity )+	{+		int oldCapacity = tree->nodeCapacity;+		int newCapacity = oldCapacity + ( oldCapacity >> 1 );+		newCapacity += newCapacity & 1;+		tree->nodes = B2_GROW_ZERO( tree->nodes, oldCapacity, newCapacity );+		tree->parents = B2_GROW_ZERO( tree->parents, oldCapacity, newCapacity );+		tree->nodeCapacity = newCapacity;++		// The spare has to match, the rebuild allocates it again.+		b2Free( tree->swapNodes, oldCapacity * sizeof( b2TreeNode ) );+		tree->swapNodes = NULL;+	}++	int pair = tree->nodeEnd;+	tree->nodeEnd += 2;+	return pair;+}++static void b2FreePair( b2DynamicTree* tree, int pair )+{+	B2_ASSERT( ( pair & 1 ) == 0 && 2 <= pair && pair < tree->nodeEnd );+	tree->nodes[pair] = b2MakeEmptyNode();+	tree->nodes[pair + 1] = b2MakeEmptyNode();+	tree->parents[pair] = tree->pairFreeList;+	tree->parents[pair + 1] = B2_NULL_INDEX;+	tree->pairFreeList = pair;+}++void b2CreateDynamicTreeInternal( b2DynamicTree* tree, int proxyCapacity )+{+	int capacity = b2MaxInt( proxyCapacity, 16 );++	// memset needed for deterministic serialization.+	memset( tree, 0, sizeof( b2DynamicTree ) );++	// A tree of n proxies has 2n - 1 nodes plus the empty node beside the root+	tree->nodeCapacity = 2 * capacity;+	tree->nodes = (b2TreeNode*)b2AllocZero( tree->nodeCapacity * sizeof( b2TreeNode ) );+	tree->parents = (int32_t*)b2AllocZero( tree->nodeCapacity * sizeof( int32_t ) );+	tree->pairFreeList = B2_NULL_INDEX;++	// The root and the empty node always exist+	tree->nodes[B2_ROOT_NODE] = b2MakeEmptyNode();+	tree->nodes[B2_ROOT_NODE + 1] = b2MakeEmptyNode();+	tree->parents[B2_ROOT_NODE] = B2_NULL_INDEX;+	tree->parents[B2_ROOT_NODE + 1] = B2_NULL_INDEX;++	// This is the bump index.+	tree->nodeEnd = 2;+	tree->dfsOrdered = true;++	tree->proxyCapacity = capacity;+	tree->proxyCount = 0;+	tree->proxies = (b2TreeProxy*)b2AllocZero( tree->proxyCapacity * sizeof( b2TreeProxy ) );++	// Build a linked list for the free list.+	for ( int i = 0; i < tree->proxyCapacity - 1; ++i )+	{+		tree->proxies[i].node = B2_NULL_INDEX;+		tree->proxies[i].next = i + 1;+	}++	tree->proxies[tree->proxyCapacity - 1].node = B2_NULL_INDEX;+	tree->proxies[tree->proxyCapacity - 1].next = B2_NULL_INDEX;+	tree->proxyFreeList = 0;++	tree->leafIndices = NULL;+	tree->leafNodes = NULL;+	tree->leafBoxes = NULL;+	tree->leafCenters = NULL;+	tree->binIndices = NULL;+	tree->rebuildCapacity = 0;+}++void b2DestroyDynamicTreeInternal( b2DynamicTree* tree )+{+	b2Free( tree->nodes, tree->nodeCapacity * sizeof( b2TreeNode ) );+	b2Free( tree->parents, tree->nodeCapacity * sizeof( int32_t ) );+	b2Free( tree->proxies, tree->proxyCapacity * sizeof( b2TreeProxy ) );+	b2Free( tree->swapNodes, tree->nodeCapacity * sizeof( b2TreeNode ) );+	b2Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int32_t ) );+	b2Free( tree->leafNodes, tree->rebuildCapacity * sizeof( b2TreeNode ) );+	b2Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b2AABB ) );+	b2Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b2Vec2 ) );+	b2Free( tree->binIndices, tree->rebuildCapacity * sizeof( int32_t ) );++	memset( tree, 0, sizeof( b2DynamicTree ) );+}++b2DynamicTree* b2CreateDynamicTree( int proxyCapacity )+{+	b2DynamicTree* tree = (b2DynamicTree*)b2Alloc( sizeof( b2DynamicTree ) );+	b2CreateDynamicTreeInternal( tree, proxyCapacity );+	return tree;+}++void b2DestroyDynamicTree( b2DynamicTree* tree )+{+	if ( tree == NULL )+	{+		return;+	}++	b2DestroyDynamicTreeInternal( tree );+	b2Free( tree, sizeof( b2DynamicTree ) );+}++// Allocate a proxy from the pool. Grow the pool if necessary.+static int b2AllocateProxy( b2DynamicTree* tree )+{+	// Expand the pool as needed.+	if ( tree->proxyFreeList == B2_NULL_INDEX )+	{+		B2_ASSERT( tree->proxyCount == tree->proxyCapacity );++		// The free list is empty. Rebuild a bigger pool.+		// A restored pool can have capacity 1, so guarantee strict growth.+		int oldCapacity = tree->proxyCapacity;+		tree->proxyCapacity += b2MaxInt( oldCapacity >> 1, 1 );+		tree->proxies = B2_GROW_ZERO( tree->proxies, oldCapacity, tree->proxyCapacity );++		// Build a linked list for the free list.+		for ( int i = oldCapacity; i < tree->proxyCapacity - 1; ++i )+		{+			tree->proxies[i].node = B2_NULL_INDEX;+			tree->proxies[i].next = i + 1;+		}++		tree->proxies[tree->proxyCapacity - 1].node = B2_NULL_INDEX;+		tree->proxies[tree->proxyCapacity - 1].next = B2_NULL_INDEX;+		tree->proxyFreeList = oldCapacity;+	}++	// Peel a proxy off the free list.+	int proxyIndex = tree->proxyFreeList;+	tree->proxyFreeList = tree->proxies[proxyIndex].next;+	memset( tree->proxies + proxyIndex, 0, sizeof( b2TreeProxy ) );+	tree->proxies[proxyIndex].node = B2_NULL_INDEX;+	tree->proxies[proxyIndex].next = B2_NULL_INDEX;+	++tree->proxyCount;+	return proxyIndex;+}++// Return a proxy to the pool.+static void b2FreeProxy( b2DynamicTree* tree, int proxyId )+{+	B2_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );+	B2_ASSERT( 0 < tree->proxyCount );+	tree->proxies[proxyId].node = B2_NULL_INDEX;+	tree->proxies[proxyId].next = tree->proxyFreeList;+	tree->proxyFreeList = proxyId;+	--tree->proxyCount;+}++static inline int b2GetNodeHeight( const b2TreeNode* node )+{+	return b2IsLeaf( node ) ? 0 : node->height;+}++// The internal node above a children pair+static inline b2TreeNode b2MakeInternalNode( const b2TreeNode* nodes, int pair )+{+	const b2TreeNode* c1 = nodes + pair;+	const b2TreeNode* c2 = nodes + pair + 1;++	b2TreeNode node = { 0 };+	node.aabb = b2AABB_Union( c1->aabb, c2->aabb );+	node.flagIndex = (uint32_t)pair | ( ( c1->flagIndex | c2->flagIndex ) & B2_MOVED_NODE );+	node.height = 1 + b2MaxInt( b2GetNodeHeight( c1 ), b2GetNodeHeight( c2 ) );+	return node;+}++static inline b2TreeNode b2MakeLeafNode( b2AABB aabb, int proxyId, uint64_t userData, bool moved )+{+	b2TreeNode node = { 0 };+	node.aabb = aabb;+	node.flagIndex = (uint32_t)proxyId | B2_LEAF_NODE | ( moved ? B2_MOVED_NODE : 0 );+	node.shapeIndex = (uint32_t)userData;+	return node;+}++// A node landed at a new index, so tell what hangs below it+static inline void b2LinkChildren( b2DynamicTree* tree, int nodeIndex )+{+	const b2TreeNode* node = tree->nodes + nodeIndex;+	if ( b2IsLeaf( node ) )+	{+		tree->proxies[b2GetProxyId( node )].node = nodeIndex;+	}+	else+	{+		int pair = b2GetLeftChild( node );+		tree->parents[pair] = nodeIndex;+		tree->parents[pair + 1] = nodeIndex;+	}+}++// The sweep refit visits indices from high to low, so it needs every child above its parent+static inline bool b2IsNodeOrdered( const b2TreeNode* nodes, int nodeIndex )+{+	const b2TreeNode* node = nodes + nodeIndex;+	return b2IsLeaf( node ) || nodeIndex < b2GetLeftChild( node );+}++// 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 b2FindBestSibling( const b2DynamicTree* tree, b2AABB boxD )+{+	b2Vec2 centerD = b2AABB_Center( boxD );+	float areaD = b2Perimeter( boxD );++	const b2TreeNode* nodes = tree->nodes;+	int nodeIndex = B2_ROOT_NODE;++	if ( b2IsLeaf( nodes + nodeIndex ) )+	{+		return nodeIndex;+	}++	b2AABB rootBox = nodes[nodeIndex].aabb;++	// Area of current node+	float areaBase = b2Perimeter( rootBox );++	// Area of inflated node+	float directCost = b2Perimeter( b2AABB_Union( rootBox, boxD ) );+	float inheritedCost = 0.0f;++	int bestSibling = nodeIndex;+	float bestCost = directCost;++	// Descend the tree, following a single greedy path.+	for ( ;; )+	{+		int child1 = b2GetLeftChild( nodes + nodeIndex );+		int child2 = child1 + 1;++		// 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 = nodeIndex;+			bestCost = cost;+		}++		// Inheritance cost seen by children+		inheritedCost += directCost - areaBase;++		bool leaf1 = b2IsLeaf( nodes + child1 );+		bool leaf2 = b2IsLeaf( nodes + child2 );++		// Cost of descending into child 1+		float lowerCost1 = FLT_MAX;+		b2AABB box1 = nodes[child1].aabb;+		float directCost1 = b2Perimeter( b2AABB_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;+			if ( cost1 < bestCost )+			{+				bestSibling = child1;+				bestCost = cost1;+			}+		}+		else+		{+			// Child 1 is an internal node+			area1 = b2Perimeter( box1 );++			// Lower bound cost of inserting under child 1. The minimum accounts for two possibilities:+			// 1. Child1 could be the sibling with cost1 = inheritedCost + directCost1+			// 2. A descendant of child1 could be the sibling with the lower bound cost of+			//       cost1 = inheritedCost + (directCost1 - area1) + areaD+			// This minimum here leads to the minimum of these two costs.+			lowerCost1 = inheritedCost + directCost1 + b2MinFloat( areaD - area1, 0.0f );+		}++		// Cost of descending into child 2+		float lowerCost2 = FLT_MAX;+		b2AABB box2 = nodes[child2].aabb;+		float directCost2 = b2Perimeter( b2AABB_Union( box2, boxD ) );+		float area2 = 0.0f;+		if ( leaf2 )+		{+			float cost2 = directCost2 + inheritedCost;+			if ( cost2 < bestCost )+			{+				bestSibling = child2;+				bestCost = cost2;+			}+		}+		else+		{+			area2 = b2Perimeter( box2 );+			lowerCost2 = inheritedCost + directCost2 + b2MinFloat( 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 )+		{+			B2_ASSERT( lowerCost1 < FLT_MAX );+			B2_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.+			b2Vec2 d1 = b2Sub( b2AABB_Center( box1 ), centerD );+			b2Vec2 d2 = b2Sub( b2AABB_Center( box2 ), centerD );+			lowerCost1 = b2LengthSquared( d1 );+			lowerCost2 = b2LengthSquared( d2 );+		}++		// Descend+		if ( lowerCost1 < lowerCost2 && leaf1 == false )+		{+			nodeIndex = child1;+			areaBase = area1;+			directCost = directCost1;+		}+		else+		{+			nodeIndex = child2;+			areaBase = area2;+			directCost = directCost2;+		}+	}++	return bestSibling;+}++// Swap a child of A with a grandchild under the other child.+// Example swapping B with G:+// (A (B (D E) C (F G)) -> (A (G) C (F B (D E))+static void b2SwapNodes( b2DynamicTree* tree, int iDown, int iUp )+{+	b2TreeNode* nodes = tree->nodes;+	B2_SWAP( nodes[iDown], nodes[iUp] );+	b2LinkChildren( tree, iDown );+	b2LinkChildren( tree, iUp );++	if ( b2IsNodeOrdered( nodes, iDown ) == false || b2IsNodeOrdered( nodes, iUp ) == false )+	{+		tree->dfsOrdered = false;+	}++	// The sibling of the node that went down holds a different subtree now+	int iC = iDown ^ 1;+	nodes[iC] = b2MakeInternalNode( nodes, b2GetLeftChild( nodes + iC ) );+}++// Perform a left or right rotation if node A is imbalanced.+// Tree: (A (B (D E) C (F G))+static void b2RotateNodes( b2DynamicTree* tree, int iA )+{+	b2TreeNode* nodes = tree->nodes;+	const b2TreeNode* A = nodes + iA;+	B2_ASSERT( b2IsLeaf( A ) == false );++	int iB = b2GetLeftChild( A );+	int iC = iB + 1;+	const b2TreeNode* B = nodes + iB;+	const b2TreeNode* C = nodes + iC;++	bool leafB = b2IsLeaf( B );+	bool leafC = b2IsLeaf( C );+	if ( leafB && leafC )+	{+		return;+	}++	int bestDown = B2_NULL_INDEX;+	int bestUp = B2_NULL_INDEX;+	float bestDelta = 0.0f;++	if ( leafC == false )+	{+		// Swap B with F or G+		int iF = b2GetLeftChild( C );+		int iG = iF + 1;+		float areaC = b2Perimeter( C->aabb );++		// B <-> F then C (B G)+		float deltaBF = b2Perimeter( b2UnionV( B->aabb, nodes[iG].aabb ) ) - areaC;+		if ( deltaBF < bestDelta )+		{+			bestDown = iB;+			bestUp = iF;+			bestDelta = deltaBF;+		}++		// B <-> G then C (F B)+		float deltaBG = b2Perimeter( b2UnionV( B->aabb, nodes[iF].aabb ) ) - areaC;+		if ( deltaBG < bestDelta )+		{+			bestDown = iB;+			bestUp = iG;+			bestDelta = deltaBG;+		}+	}++	if ( leafB == false )+	{+		// Swap C with D or E+		int iD = b2GetLeftChild( B );+		int iE = iD + 1;+		float areaB = b2Perimeter( B->aabb );++		// C <-> D then B (C E)+		float deltaCD = b2Perimeter( b2UnionV( C->aabb, nodes[iE].aabb ) ) - areaB;+		if ( deltaCD < bestDelta )+		{+			bestDown = iC;+			bestUp = iD;+			bestDelta = deltaCD;+		}++		// C <-> E then B (D C)+		float deltaCE = b2Perimeter( b2UnionV( C->aabb, nodes[iD].aabb ) ) - areaB;+		if ( deltaCE < bestDelta )+		{+			bestDown = iC;+			bestUp = iE;+			bestDelta = deltaCE;+		}+	}++	if ( bestDown != B2_NULL_INDEX )+	{+		b2SwapNodes( tree, bestDown, bestUp );+	}+}++static void b2InsertLeaf( b2DynamicTree* tree, b2AABB aabb, int proxyId, bool moved, bool shouldRotate )+{+	b2TreeProxy* proxy = tree->proxies + proxyId;+	b2TreeNode leaf = b2MakeLeafNode( aabb, proxyId, proxy->userData, moved );++	if ( b2IsEmptyNode( tree->nodes + B2_ROOT_NODE ) )+	{+		tree->nodes[B2_ROOT_NODE] = leaf;+		proxy->node = B2_ROOT_NODE;+		return;+	}++	int sibling = b2FindBestSibling( tree, aabb );++	// The sibling's position becomes the new parent. The sibling moves down into a new pair+	// beside the leaf, which puts it below its own children when it is internal.+	int pair = b2AllocateSiblingPair( tree );+	b2TreeNode* nodes = tree->nodes;+	int32_t* parents = tree->parents;++	nodes[pair] = nodes[sibling];+	nodes[pair + 1] = leaf;+	parents[pair] = sibling;+	parents[pair + 1] = sibling;+	b2LinkChildren( tree, pair );+	proxy->node = pair + 1;++	nodes[sibling] = b2MakeInternalNode( nodes, pair );++	if ( b2IsNodeOrdered( nodes, sibling ) == false || b2IsNodeOrdered( nodes, pair ) == false )+	{+		tree->dfsOrdered = false;+	}++	// Walk back up the tree refitting ancestors, the root included.+	int index = sibling;+	while ( index != B2_NULL_INDEX )+	{+		if ( shouldRotate )+		{+			b2RotateNodes( tree, index );+		}++		nodes[index] = b2MakeInternalNode( nodes, b2GetLeftChild( nodes + index ) );+		index = parents[index];+	}+}++static void b2RemoveLeaf( b2DynamicTree* tree, int proxyId )+{+	b2TreeNode* nodes = tree->nodes;+	int32_t* parents = tree->parents;++	int leaf = tree->proxies[proxyId].node;+	B2_ASSERT( 0 <= leaf && leaf < tree->nodeEnd );+	B2_ASSERT( b2IsLeaf( nodes + leaf ) && b2GetProxyId( nodes + leaf ) == proxyId );++	if ( leaf == B2_ROOT_NODE )+	{+		nodes[B2_ROOT_NODE] = b2MakeEmptyNode();+		return;+	}++	// The sibling takes the parent's position and the pair is freed. This keeps the order,+	// the sibling only moves down.+	int parent = parents[leaf];+	nodes[parent] = nodes[leaf ^ 1];+	b2LinkChildren( tree, parent );+	b2FreePair( tree, leaf & ~1 );++	// Update ancestors.+	int index = parents[parent];+	while ( index != B2_NULL_INDEX )+	{+		nodes[index] = b2MakeInternalNode( nodes, b2GetLeftChild( nodes + index ) );+		index = parents[index];+	}+}++// 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 b2CreateTreeProxyInternal( b2DynamicTree* tree, b2AABB aabb, uint64_t categoryBits, uint64_t userData,+									   bool markMoved )+{+	B2_VALIDATE( b2IsValidAABB( aabb ) );++	int proxyId = b2AllocateProxy( tree );++	b2TreeProxy* proxy = tree->proxies + proxyId;+	proxy->categoryBits = categoryBits;+	proxy->userData = userData;++	bool shouldRotate = true;+	b2InsertLeaf( tree, aabb, proxyId, markMoved, shouldRotate );++	return proxyId;+}++int b2CreateTreeProxy( b2DynamicTree* tree, b2AABB aabb, uint64_t categoryBits, uint64_t userData )+{+	return b2CreateTreeProxyInternal( tree, aabb, categoryBits, userData, false );+}++void b2DestroyTreeProxy( b2DynamicTree* tree, int proxyId )+{+	B2_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );++	b2RemoveLeaf( tree, proxyId );+	b2FreeProxy( tree, proxyId );+}++int b2DynamicTree_GetProxyCount( const b2DynamicTree* tree )+{+	return tree->proxyCount;+}++void b2DynamicTree_MoveProxyInternal( b2DynamicTree* tree, int proxyId, b2AABB aabb, bool markMoved )+{+	B2_VALIDATE( b2IsValidAABB( aabb ) );+	B2_VALIDATE( aabb.upperBound.x - aabb.lowerBound.x < B2_HUGE );+	B2_VALIDATE( aabb.upperBound.y - aabb.lowerBound.y < B2_HUGE );+	B2_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );++	b2RemoveLeaf( tree, proxyId );++	bool shouldRotate = false;+	b2InsertLeaf( tree, aabb, proxyId, markMoved, shouldRotate );+}++void b2DynamicTree_MoveProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb )+{+	b2DynamicTree_MoveProxyInternal( tree, proxyId, aabb, false );+}++void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb )+{+	B2_VALIDATE( b2IsValidAABB( aabb ) );+	B2_VALIDATE( aabb.upperBound.x - aabb.lowerBound.x < B2_HUGE );+	B2_VALIDATE( aabb.upperBound.y - aabb.lowerBound.y < B2_HUGE );+	B2_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );++	b2TreeNode* nodes = tree->nodes;+	const int32_t* parents = tree->parents;++	int index = tree->proxies[proxyId].node;+	b2TreeNode* node = nodes + index;+	B2_ASSERT( b2IsLeaf( node ) );++	// Caller must ensure this+	B2_VALIDATE( b2AABB_Contains( node->aabb, aabb ) == false );++	node->aabb = aabb;+	node->flagIndex |= B2_MOVED_NODE;++	index = parents[index];+	while ( index != B2_NULL_INDEX )+	{+		node = nodes + index;+		bool changed = b2EnlargeAABB( &node->aabb, aabb );++		// This is marked to ensure the root is marked in this loop or the one below.+		node->flagIndex |= B2_MOVED_NODE;++		index = parents[index];++		if ( changed == false )+		{+			break;+		}+	}++	// Mark all the way up to the root.+	while ( index != B2_NULL_INDEX )+	{+		node = nodes + index;+		if ( node->flagIndex & B2_MOVED_NODE )+		{+			// Early out because this ancestor was previously ascended and marked as moved.+			break;+		}++		node->flagIndex |= B2_MOVED_NODE;+		index = parents[index];+	}+}++void b2DynamicTree_SetCategoryBits( b2DynamicTree* tree, int proxyId, uint64_t categoryBits )+{+	B2_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );+	tree->proxies[proxyId].categoryBits = categoryBits;+}++uint64_t b2DynamicTree_GetCategoryBits( b2DynamicTree* tree, int proxyId )+{+	B2_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );+	return tree->proxies[proxyId].categoryBits;+}++int b2DynamicTree_GetHeight( const b2DynamicTree* tree )+{+	return b2GetNodeHeight( tree->nodes + B2_ROOT_NODE );+}++// The area ratio is the thing that SAH seeks to minimize. SAH+// cannot do anything about leaf boxes or the root box. It seeks+// to minimize the area of all non-root internal nodes. Divide this+// by the root area to make the metric non-dimensional.+// So this becomes a meaningful measure of tree quality.+float b2DynamicTree_GetAreaRatio( const b2DynamicTree* tree )+{+	if ( tree->proxyCount == 0 )+	{+		return 0.0f;+	}++	const b2TreeNode* nodes = tree->nodes;+	float rootArea = b2Perimeter( nodes[B2_ROOT_NODE].aabb );+	if ( rootArea <= 0.0f )+	{+		return 0.0f;+	}++	// Free nodes and the empty node are leaf tagged.+	float internalArea = 0.0f;+	int nodeEnd = tree->nodeEnd;+	for ( int i = 2; i < nodeEnd; ++i )+	{+		if ( b2IsLeaf( nodes + i ) == false )+		{+			internalArea += b2Perimeter( nodes[i].aabb );+		}+	}++	return internalArea / rootArea;+}++b2AABB b2DynamicTree_GetRootBounds( const b2DynamicTree* tree )+{+	if ( tree->proxyCount == 0 )+	{+		return (b2AABB){ b2Vec2_zero, b2Vec2_zero };+	}++	return tree->nodes[B2_ROOT_NODE].aabb;+}++#if B2_ENABLE_VALIDATION++static int b2ValidateSubtree( const b2DynamicTree* tree, int nodeIndex, int* leafCount )+{+	B2_ASSERT( 0 <= nodeIndex && nodeIndex < tree->nodeEnd );+	const b2TreeNode* node = tree->nodes + nodeIndex;+	B2_ASSERT( b2IsEmptyNode( node ) == false );++	if ( b2IsLeaf( node ) )+	{+		int proxyId = b2GetProxyId( node );+		B2_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );+		B2_ASSERT( tree->proxies[proxyId].node == nodeIndex );+		B2_ASSERT( (int32_t)tree->proxies[proxyId].userData == node->shapeIndex );+		*leafCount += 1;+		return 0;+	}++	int pair = b2GetLeftChild( node );+	B2_ASSERT( ( pair & 1 ) == 0 && 2 <= pair && pair < tree->nodeEnd );+	B2_ASSERT( tree->parents[pair] == nodeIndex );+	B2_ASSERT( tree->parents[pair + 1] == nodeIndex );+	B2_ASSERT( tree->dfsOrdered == false || nodeIndex < pair );++	const b2TreeNode* c1 = tree->nodes + pair;+	const b2TreeNode* c2 = tree->nodes + pair + 1;+	B2_ASSERT( b2AABB_Contains( node->aabb, c1->aabb ) );+	B2_ASSERT( b2AABB_Contains( node->aabb, c2->aabb ) );+	B2_ASSERT( b2IsNodeMoved( node ) == ( b2IsNodeMoved( c1 ) || b2IsNodeMoved( c2 ) ) );++	// A bad tree can stack overflow, but that is validation on its own.+	int height1 = b2ValidateSubtree( tree, pair, leafCount );+	int height2 = b2ValidateSubtree( tree, pair + 1, leafCount );+	int height = 1 + b2MaxInt( height1, height2 );+	B2_ASSERT( node->height == height );+	return height;+}++#endif++void b2DynamicTree_Validate( const b2DynamicTree* tree )+{+#if B2_ENABLE_VALIDATION+	B2_ASSERT( 2 <= tree->nodeEnd && tree->nodeEnd <= tree->nodeCapacity );+	B2_ASSERT( ( tree->nodeEnd & 1 ) == 0 );+	B2_ASSERT( tree->parents[B2_ROOT_NODE] == B2_NULL_INDEX );+	B2_ASSERT( b2IsEmptyNode( tree->nodes + B2_ROOT_NODE + 1 ) );++	int freePairCount = 0;+	int pair = tree->pairFreeList;+	while ( pair != B2_NULL_INDEX )+	{+		B2_ASSERT( ( pair & 1 ) == 0 && 2 <= pair && pair < tree->nodeEnd );+		B2_ASSERT( b2IsEmptyNode( tree->nodes + pair ) );+		B2_ASSERT( b2IsEmptyNode( tree->nodes + pair + 1 ) );+		pair = tree->parents[pair];+		++freePairCount;+		B2_ASSERT( 2 * freePairCount < tree->nodeEnd );+	}++	// Validate proxy free list.+	int freeProxyCount = 0;+	int freeIndex = tree->proxyFreeList;+	while ( freeIndex != B2_NULL_INDEX )+	{+		B2_ASSERT( 0 <= freeIndex && freeIndex < tree->proxyCapacity );+		B2_ASSERT( tree->proxies[freeIndex].node == B2_NULL_INDEX );+		freeIndex = tree->proxies[freeIndex].next;+		++freeProxyCount;+	}+	B2_ASSERT( tree->proxyCount + freeProxyCount == tree->proxyCapacity );++	B2_ASSERT( tree->nodeEnd == 2 * b2MaxInt( tree->proxyCount, 1 ) + 2 * freePairCount );++	if ( tree->proxyCount == 0 )+	{+		B2_ASSERT( b2IsEmptyNode( tree->nodes + B2_ROOT_NODE ) );+		return;+	}++	int leafCount = 0;+	b2ValidateSubtree( tree, B2_ROOT_NODE, &leafCount );+	B2_ASSERT( leafCount == tree->proxyCount );++#else+	B2_UNUSED( tree );+#endif+}++void b2DynamicTree_ValidateNoMoved( const b2DynamicTree* tree )+{+#if B2_ENABLE_VALIDATION == 1+	const b2TreeNode* nodes = tree->nodes;+	int nodeEnd = tree->nodeEnd;+	for ( int i = 0; i < nodeEnd; ++i )+	{+		B2_ASSERT( b2IsNodeMoved( nodes + i ) == false );+	}+#else+	B2_UNUSED( tree );+#endif+}++size_t b2DynamicTree_GetByteCount( const b2DynamicTree* tree )+{+	size_t size = sizeof( b2DynamicTree );+	size += tree->nodeCapacity * sizeof( b2TreeNode );+	size += tree->nodeCapacity * sizeof( int32_t );+	size += tree->proxyCapacity * sizeof( b2TreeProxy );+	size += tree->swapNodes == NULL ? 0 : tree->nodeCapacity * sizeof( b2TreeNode );++	// leafIndices+	size += tree->rebuildCapacity * sizeof( int );+	// leafNodes+	size += tree->rebuildCapacity * sizeof( b2TreeNode );+	// leafBoxes+	size += tree->rebuildCapacity * sizeof( b2AABB );+	// leafCenters+	size += tree->rebuildCapacity * sizeof( b2Vec2 );+	// binIndices+	size += tree->rebuildCapacity * sizeof( int );++	return size;+}++uint64_t b2DynamicTree_GetUserData( const b2DynamicTree* tree, int proxyId )+{+	B2_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );+	return tree->proxies[proxyId].userData;+}++b2AABB b2DynamicTree_GetProxyBounds( const b2DynamicTree* tree, int proxyId )+{+	B2_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );+	int nodeIndex = tree->proxies[proxyId].node;+	B2_ASSERT( 0 <= nodeIndex && nodeIndex < tree->nodeEnd );+	return tree->nodes[nodeIndex].aabb;+}++b2TreeStats b2DynamicTree_OverlapAABB( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits, b2TreeQueryCallbackFcn* callback,+								 void* context )+{+	b2TreeStats result = { 0 };++	if ( tree->proxyCount == 0 )+	{+		return result;+	}++	const b2TreeNode* nodes = tree->nodes;++	int stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;+	stack[stackCount++] = b2GetRootPair( nodes );++	b2AABBV boxv = b2LoadAABBV( &aabb );++	while ( stackCount > 0 )+	{+		int pair = stack[--stackCount];+		result.nodeVisits += 1;++		for ( int i = 0; i < 2; ++i )+		{+			const b2TreeNode* node = nodes + pair + i;+			if ( b2OverlapNode( boxv, node ) )+			{+				if ( b2IsLeaf( node ) )+				{+					// callback to user code with proxy id+					int proxyId = b2GetProxyId( node );+					const b2TreeProxy* proxy = tree->proxies + proxyId;+					if ( proxy->categoryBits & maskBits )+					{+						bool proceed = callback( proxyId, proxy->userData, context );+						result.leafVisits += 1;++						if ( proceed == false )+						{+							return result;+						}+					}+				}+				else+				{+					if ( stackCount < B2_TREE_STACK_SIZE - 1 )+					{+						stack[stackCount++] = b2GetLeftChild( node );+					}+					else+					{+						B2_ASSERT( stackCount < B2_TREE_STACK_SIZE - 1 );+					}+				}+			}+		}+	}++	return result;+}++// This ignores the category bits. Useful for debug draw.+b2TreeStats b2DynamicTree_OverlapAABBAll( const b2DynamicTree* tree, b2AABB aabb, b2TreeQueryCallbackFcn* callback, void* context )+{+	b2TreeStats result = { 0 };++	if ( tree->proxyCount == 0 )+	{+		return result;+	}++	const b2TreeNode* nodes = tree->nodes;++	int stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;+	stack[stackCount++] = b2GetRootPair( nodes );++	b2AABBV boxv = b2LoadAABBV( &aabb );++	while ( stackCount > 0 )+	{+		int pair = stack[--stackCount];+		result.nodeVisits += 1;++		for ( int i = 0; i < 2; ++i )+		{+			const b2TreeNode* node = nodes + pair + i;+			if ( b2OverlapNode( boxv, node ) )+			{+				if ( b2IsLeaf( node ) )+				{+					// callback to user code with proxy id+					int proxyId = b2GetProxyId( node );+					const b2TreeProxy* proxy = tree->proxies + proxyId;+					bool proceed = callback( proxyId, proxy->userData, context );+					result.leafVisits += 1;++					if ( proceed == false )+					{+						return result;+					}+				}+				else+				{+					if ( stackCount < B2_TREE_STACK_SIZE - 1 )+					{+						stack[stackCount++] = b2GetLeftChild( node );+					}+					else+					{+						B2_ASSERT( stackCount < B2_TREE_STACK_SIZE - 1 );+					}+				}+			}+		}+	}++	return result;+}++// A lot of optimization work went into this. It beats the slab test by a significant margin.+// It is faster than having category bits in the nodes because of the cache line friendly node+// size (64 bytes).+b2TreeStats b2DynamicTree_CastRay( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits,+								   b2TreeRayCastCallbackFcn* callback, void* context )+{+	b2TreeStats result = { 0 };++	if ( tree->proxyCount == 0 )+	{+		return result;+	}++	b2Vec2 p1 = input->origin;+	b2Vec2 d = input->translation;++	b2Vec2 r = b2Normalize( d );++	// v is perpendicular to the segment.+	b2Vec2 v = b2CrossSV( 1.0f, r );+	b2Vec2 abs_v = b2Abs( v );++	// Separating axis for segment (Gino, p80).+	// |dot(v, p1 - c)| > dot(|v|, h)+	float maxFraction = input->maxFraction;++	b2Vec2 p2 = b2MulAdd( p1, maxFraction, d );++	// Build a bounding box for the segment.+	b2AABB segmentAABB = { b2Min( p1, p2 ), b2Max( p1, p2 ) };++	b2AABBV boxv = b2LoadAABBV( &segmentAABB );++	const b2TreeNode* nodes = tree->nodes;++	int stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;+	stack[stackCount++] = b2GetRootPair( nodes );++	b2RayCastInput subInput = *input;++	while ( stackCount > 0 )+	{+		int pair = stack[--stackCount];+		result.nodeVisits += 1;++		const b2TreeNode* hit[2];+		bool isLeaf[2];+		int hitCount = 0;+		for ( int i = 0; i < 2; ++i )+		{+			const b2TreeNode* node = nodes + pair + i;++			if ( b2OverlapNode( boxv, node ) == false )+			{+				continue;+			}++			// Separating axis for segment (Gino, p80).+			// |dot(v, p1 - c)| > dot(|v|, h)+			b2AABB nodeAABB = node->aabb;+			b2Vec2 c = b2AABB_Center( nodeAABB );+			b2Vec2 h = b2AABB_Extents( nodeAABB );+			float term1 = b2AbsFloat( b2Dot( v, b2Sub( p1, c ) ) );+			float term2 = b2Dot( abs_v, h );+			if ( term2 < term1 )+			{+				continue;+			}++			isLeaf[hitCount] = b2IsLeaf( node );+			hit[hitCount] = node;+			hitCount += 1;+		}++		if ( hitCount == 2 && isLeaf[0] == false && isLeaf[1] == false )+		{+			b2Vec2 center1 = b2AABB_Center( hit[0]->aabb );+			b2Vec2 center2 = b2AABB_Center( hit[1]->aabb );+			float d1 = b2DistanceSquared( center1, p1 );+			float d2 = b2DistanceSquared( center2, p1 );++			// Want to push the closest one last. Both have the same isLeaf, so they don't swap.+			if ( d1 < d2 )+			{+				B2_SWAP( hit[0], hit[1] );+			}+		}++		for ( int i = 0; i < hitCount; ++i )+		{+			if ( isLeaf[i] )+			{+				int proxyId = b2GetProxyId( hit[i] );+				const b2TreeProxy* proxy = tree->proxies + proxyId;++				if ( ( proxy->categoryBits & maskBits ) == 0 )+				{+					continue;+				}++				subInput.maxFraction = maxFraction;++				float value = callback( &subInput, proxyId, proxy->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 = b2MulAdd( p1, maxFraction, d );+					segmentAABB.lowerBound = b2Min( p1, p2 );+					segmentAABB.upperBound = b2Max( p1, p2 );++					boxv = b2LoadAABBV( &segmentAABB );+				}+			}+			else+			{+				if ( stackCount < B2_TREE_STACK_SIZE - 1 )+				{+					stack[stackCount++] = b2GetLeftChild( hit[i] );+				}+				else+				{+					B2_ASSERT( stackCount < B2_TREE_STACK_SIZE - 1 );+				}+			}+		}+	}++	return result;+}++// Follows structure of ray cast with small tweaks to handle a swept box.+b2TreeStats b2DynamicTree_CastBox( const b2DynamicTree* tree, const b2BoxCastInput* input, uint64_t maskBits,+								   b2TreeBoxCastCallbackFcn* callback, void* context )+{+	b2TreeStats result = { 0 };++	if ( tree->proxyCount == 0 )+	{+		return result;+	}++	b2AABB originAABB = input->box;++	b2Vec2 p1 = b2AABB_Center( originAABB );+	b2Vec2 extension = b2AABB_Extents( originAABB );++	// v is perpendicular to the segment.+	b2Vec2 r = input->translation;+	b2Vec2 v = b2CrossSV( 1.0f, r );+	b2Vec2 abs_v = b2Abs( v );++	// Separating axis for segment (Gino, p80).+	// |dot(v, p1 - c)| > dot(|v|, h)++	float maxFraction = input->maxFraction;++	// Build total box for the cast+	b2Vec2 t = b2MulSV( maxFraction, input->translation );+	b2AABB totalAABB = {+		b2Min( originAABB.lowerBound, b2Add( originAABB.lowerBound, t ) ),+		b2Max( originAABB.upperBound, b2Add( originAABB.upperBound, t ) ),+	};++	b2AABBV boxv = b2LoadAABBV( &totalAABB );++	const b2TreeNode* nodes = tree->nodes;++	int stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;+	stack[stackCount++] = b2GetRootPair( nodes );++	b2BoxCastInput subInput = *input;++	while ( stackCount > 0 )+	{+		int pair = stack[--stackCount];+		result.nodeVisits += 1;++		const b2TreeNode* hit[2];+		bool isLeaf[2];+		int hitCount = 0;+		for ( int i = 0; i < 2; ++i )+		{+			const b2TreeNode* node = nodes + pair + i;++			if ( b2OverlapNode( boxv, node ) == false )+			{+				continue;+			}++			// Separating axis for segment (Gino, p80).+			// |dot(v, p1 - c)| > dot(|v|, h)+			// radius extension is added to the node in this case+			b2AABB nodeAABB = node->aabb;+			b2Vec2 c = b2AABB_Center( nodeAABB );+			b2Vec2 h = b2Add( b2AABB_Extents( nodeAABB ), extension );+			float term1 = b2AbsFloat( b2Dot( v, b2Sub( p1, c ) ) );+			float term2 = b2Dot( abs_v, h );+			if ( term2 < term1 )+			{+				continue;+			}++			isLeaf[hitCount] = b2IsLeaf( node );+			hit[hitCount] = node;+			hitCount += 1;+		}++		if ( hitCount == 2 && isLeaf[0] == false && isLeaf[1] == false )+		{+			b2Vec2 center1 = b2AABB_Center( hit[0]->aabb );+			b2Vec2 center2 = b2AABB_Center( hit[1]->aabb );+			float d1 = b2DistanceSquared( center1, p1 );+			float d2 = b2DistanceSquared( center2, p1 );++			// Want to push the closest one last. Both have the same isLeaf, so they don't swap.+			if ( d1 < d2 )+			{+				B2_SWAP( hit[0], hit[1] );+			}+		}++		for ( int i = 0; i < hitCount; ++i )+		{+			if ( isLeaf[i] )+			{+				int proxyId = b2GetProxyId( hit[i] );+				const b2TreeProxy* proxy = tree->proxies + proxyId;++				if ( ( proxy->categoryBits & maskBits ) == 0 )+				{+					continue;+				}++				subInput.maxFraction = maxFraction;++				float value = callback( &subInput, proxyId, proxy->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 cast bounding box.+					maxFraction = value;+					t = b2MulSV( maxFraction, input->translation );+					totalAABB.lowerBound = b2Min( originAABB.lowerBound, b2Add( originAABB.lowerBound, t ) );+					totalAABB.upperBound = b2Max( originAABB.upperBound, b2Add( originAABB.upperBound, t ) );+					boxv = b2LoadAABBV( &totalAABB );+				}+			}+			else+			{+				if ( stackCount < B2_TREE_STACK_SIZE - 1 )+				{+					stack[stackCount++] = b2GetLeftChild( hit[i] );+				}+				else+				{+					B2_ASSERT( stackCount < B2_TREE_STACK_SIZE - 1 );+				}+			}+		}+	}++	return result;+}++// Median split == 0, Surface area heuristic == 1+#define B2_TREE_HEURISTIC 0++#if B2_TREE_HEURISTIC == 0++// Median split heuristic+static int b2PartitionMid( int* indices, b2Vec2* centers, int count )+{+	// Handle trivial case+	if ( count <= 2 )+	{+		return count / 2;+	}++	b2Vec2 lowerBound = centers[0];+	b2Vec2 upperBound = centers[0];++	for ( int i = 1; i < count; ++i )+	{+		lowerBound = b2Min( lowerBound, centers[i] );+		upperBound = b2Max( upperBound, centers[i] );+	}++	b2Vec2 d = b2Sub( upperBound, lowerBound );+	b2Vec2 c = { 0.5f * ( lowerBound.x + upperBound.x ), 0.5f * ( lowerBound.y + upperBound.y ) };++	// 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 )+	{+		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+				{+					b2Vec2 temp = centers[i1];+					centers[i1] = centers[i2 - 1];+					centers[i2 - 1] = temp;+				}++				i1 += 1;+				i2 -= 1;+			}+		}+	}+	else+	{+		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+				{+					b2Vec2 temp = centers[i1];+					centers[i1] = centers[i2 - 1];+					centers[i2 - 1] = temp;+				}++				i1 += 1;+				i2 -= 1;+			}+		}+	}+	B2_ASSERT( i1 == i2 );++	if ( i1 > 0 && i1 < count )+	{+		return i1;+	}++	return count / 2;+}++#else++#define B2_BIN_COUNT 8++typedef struct b2TreeBin+{+	b2AABB aabb;+	int count;+} b2TreeBin;++typedef struct b2TreePlane+{+	b2AABB leftAABB;+	b2AABB rightAABB;+	int leftCount;+	int rightCount;+} b2TreePlane;++// "On Fast Construction of SAH-based Bounding Volume Hierarchies" by Ingo Wald+// Returns the left child count+static int b2PartitionSAH( int* indices, int* binIndices, b2AABB* boxes, int count )+{+	B2_ASSERT( count > 0 );++	b2TreeBin bins[B2_BIN_COUNT];+	b2TreePlane planes[B2_BIN_COUNT - 1];++	b2Vec2 center = b2AABB_Center( boxes[0] );+	b2AABB centroidAABB;+	centroidAABB.lowerBound = center;+	centroidAABB.upperBound = center;++	for ( int i = 1; i < count; ++i )+	{+		center = b2AABB_Center( boxes[i] );+		centroidAABB.lowerBound = b2Min( centroidAABB.lowerBound, center );+		centroidAABB.upperBound = b2Max( centroidAABB.upperBound, center );+	}++	b2Vec2 d = b2Sub( 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 < B2_BIN_COUNT; ++i )+	{+		bins[i].aabb.lowerBound = (b2Vec2){ FLT_MAX, FLT_MAX };+		bins[i].aabb.upperBound = (b2Vec2){ -FLT_MAX, -FLT_MAX };+		bins[i].count = 0;+	}++	// Assign boxes to bins and compute bin boxes+	// TODO_ERIN optimize+	float binCount = B2_BIN_COUNT;+	float lowerBoundArray[2] = { centroidAABB.lowerBound.x, centroidAABB.lowerBound.y };+	float minC = lowerBoundArray[axisIndex];+	for ( int i = 0; i < count; ++i )+	{+		b2Vec2 c = b2AABB_Center( boxes[i] );+		float cArray[2] = { c.x, c.y };+		int binIndex = (int)( binCount * ( cArray[axisIndex] - minC ) * invD );+		binIndex = b2ClampInt( binIndex, 0, B2_BIN_COUNT - 1 );+		binIndices[i] = binIndex;+		bins[binIndex].count += 1;+		bins[binIndex].aabb = b2AABB_Union( bins[binIndex].aabb, boxes[i] );+	}++	int planeCount = B2_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 = b2AABB_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 = b2AABB_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 = b2Perimeter( planes[i].leftAABB );+		float rightArea = b2Perimeter( 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+			{+				b2AABB temp = boxes[i1];+				boxes[i1] = boxes[i2 - 1];+				boxes[i2 - 1] = temp;+			}++			i1 += 1;+			i2 -= 1;+		}+	}+	B2_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.+typedef struct b2RebuildItem+{+	// Where this node is written and the pair its children go in.+	int nodeIndex;+	int pair;+	int childCount;++	// Leaf indices+	int startIndex;+	int splitIndex;+	int endIndex;+} b2RebuildItem;++typedef struct b2CopyItem+{+	int oldPair;+	int newIndex;+} b2CopyItem;++// Bump allocate a pair of sibling nodes. Returns index to the first one.+static inline int b2BumpPair( b2DynamicTree* tree, int parent )+{+	int pair = tree->nodeEnd;+	B2_ASSERT( pair + 2 <= tree->nodeCapacity );+	tree->nodeEnd += 2;+	tree->parents[pair] = parent;+	tree->parents[pair + 1] = parent;+	return pair;+}++static inline void b2SetLeftChild( b2TreeNode* node, int pair )+{+	node->flagIndex = ( node->flagIndex & ~B2_NODE_INDEX_MASK ) | (uint32_t)pair;+}++// Copy a retained subtree from the old array into the rebuilt DFS array using+// a bump allocator.+static void b2CopySubtree( b2DynamicTree* tree, b2TreeNode node, int newIndex )+{+	const b2TreeNode* oldNodes = tree->nodes;+	b2TreeNode* newNodes = tree->swapNodes;+	b2TreeProxy* proxies = tree->proxies;++	if ( b2IsLeaf( &node ) )+	{+		newNodes[newIndex] = node;+		proxies[b2GetProxyId( &node )].node = newIndex;+		return;+	}++	b2CopyItem stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;++	int oldPair = b2GetLeftChild( &node );+	int newPair = b2BumpPair( tree, newIndex );+	b2SetLeftChild( &node, newPair );++	// Copy the subtree root. This can be a left or right sibling (even or odd).+	newNodes[newIndex] = node;++	for ( ;; )+	{+		b2TreeNode* pair = newNodes + newPair;++		// Copy the siblings.+		pair[0] = oldNodes[oldPair];+		pair[1] = oldNodes[oldPair + 1];++		if ( b2IsLeaf( pair + 1 ) )+		{+			// Hook up proxy on right sibling.+			int proxyId = b2GetProxyId( pair + 1 );+			proxies[proxyId].node = newPair + 1;+		}+		else+		{+			// Push the left child of the right sibling.+			B2_ASSERT( stackCount < B2_TREE_STACK_SIZE );+			int leftChild = b2GetLeftChild( pair + 1 );+			stack[stackCount++] = (b2CopyItem){ leftChild, newPair + 1 };+		}++		if ( b2IsLeaf( pair ) == false )+		{+			// Descend into left subtree.+			int leftIndex = newPair;+			oldPair = b2GetLeftChild( pair );+			newPair = b2BumpPair( tree, leftIndex );+			b2SetLeftChild( newNodes + leftIndex, newPair );+			continue;+		}++		// Hook up proxy on left sibling.+		proxies[b2GetProxyId( pair )].node = newPair;++		if ( stackCount == 0 )+		{+			break;+		}++		// Descend a right subtree that was previously pushed.+		b2CopyItem item = stack[--stackCount];+		oldPair = item.oldPair;+		newPair = b2BumpPair( tree, item.newIndex );+		b2SetLeftChild( newNodes + item.newIndex, newPair );+	}+}++static void b2PlaceLeaf( b2DynamicTree* tree, b2TreeNode leaf, int newIndex )+{+	if ( b2IsLeaf( &leaf ) )+	{+		tree->swapNodes[newIndex] = leaf;+		tree->proxies[b2GetProxyId( &leaf )].node = newIndex;+	}+	else+	{+		b2CopySubtree( tree, leaf, newIndex );+	}+}++static void b2BuildTree( b2DynamicTree* tree, int leafCount )+{+	b2TreeNode* nodes = tree->swapNodes;+	const b2TreeNode* leaves = tree->leafNodes;+	int* leafIndices = tree->leafIndices;++#if B2_TREE_HEURISTIC == 0+	b2Vec2* leafCenters = tree->leafCenters;+#else+	b2AABB* leafBoxes = tree->leafBoxes;+	int* binIndices = tree->binIndices;+#endif++	// Bump allocation into the swap nodes.+	tree->nodeEnd = 2;+	nodes[B2_ROOT_NODE + 1] = b2MakeEmptyNode();+	tree->parents[B2_ROOT_NODE] = B2_NULL_INDEX;+	tree->parents[B2_ROOT_NODE + 1] = B2_NULL_INDEX;++	if ( leafCount == 1 )+	{+		b2PlaceLeaf( tree, leaves[leafIndices[0]], B2_ROOT_NODE );+		return;+	}++	b2RebuildItem stack[B2_TREE_STACK_SIZE];+	int top = 0;++	stack[0].nodeIndex = B2_ROOT_NODE;+	stack[0].pair = b2BumpPair( tree, B2_ROOT_NODE );+	stack[0].childCount = -1;+	stack[0].startIndex = 0;+	stack[0].endIndex = leafCount;+#if B2_TREE_HEURISTIC == 0+	stack[0].splitIndex = b2PartitionMid( leafIndices, leafCenters, leafCount );+#else+	stack[0].splitIndex = b2PartitionSAH( leafIndices, binIndices, leafBoxes, leafCount );+#endif++	while ( true )+	{+		b2RebuildItem* item = stack + top;+		item->childCount += 1;++		if ( item->childCount == 2 )+		{+			// Both children written, so the parent node can be finalized.+			nodes[item->nodeIndex] = b2MakeInternalNode( nodes, item->pair );+			if ( top == 0 )+			{+				break;+			}++			top -= 1;+			continue;+		}++		int slot = item->childCount;+		int startIndex = slot == 0 ? item->startIndex : item->splitIndex;+		int endIndex = slot == 0 ? item->splitIndex : item->endIndex;+		int count = endIndex - startIndex;+		B2_ASSERT( count > 0 );++		int nodeIndex = item->pair + slot;+		if ( count == 1 )+		{+			b2PlaceLeaf( tree, leaves[leafIndices[startIndex]], nodeIndex );+			continue;+		}++		// todo fail gracefully if the stack runs out in release+		B2_ASSERT( top < B2_TREE_STACK_SIZE - 1 );++		top += 1;+		b2RebuildItem* newItem = stack + top;+		newItem->nodeIndex = nodeIndex;+		newItem->pair = b2BumpPair( tree, nodeIndex );+		newItem->childCount = -1;+		newItem->startIndex = startIndex;+		newItem->endIndex = endIndex;+		newItem->splitIndex = startIndex;+#if B2_TREE_HEURISTIC == 0+		newItem->splitIndex += b2PartitionMid( leafIndices + startIndex, leafCenters + startIndex, count );+#else+		newItem->splitIndex += b2PartitionSAH( leafIndices + startIndex, binIndices + startIndex, leafBoxes + startIndex, count );+#endif+	}+}++// Rebuild the stale parts of the tree. The entire tree is put into DFS order. This makes+// refitting much faster. This is done async with threading, so the cost is hidden.+// Not safe to access tree during this operation.+int b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild )+{+	int proxyCount = tree->proxyCount;+	if ( proxyCount == 0 )+	{+		return 0;+	}++	// An unordered tree is rebuilt even when nothing moved, the sweep refit needs the order+	const b2TreeNode* nodes = tree->nodes;+	const b2TreeNode* root = nodes + B2_ROOT_NODE;+	if ( fullBuild == false && b2IsNodeMoved( root ) == false && tree->dfsOrdered )+	{+		return 0;+	}++	if ( tree->swapNodes == NULL )+	{+		tree->swapNodes = b2AllocZero( tree->nodeCapacity * sizeof( b2TreeNode ) );+	}++	if ( proxyCount > tree->rebuildCapacity )+	{+		int oldCapacity = tree->rebuildCapacity;+		int newCapacity = proxyCount + proxyCount / 2;++		b2Free( tree->leafIndices, oldCapacity * sizeof( int32_t ) );+		tree->leafIndices = (int32_t*)b2Alloc( newCapacity * sizeof( int32_t ) );++		b2Free( tree->leafNodes, oldCapacity * sizeof( b2TreeNode ) );+		tree->leafNodes = (b2TreeNode*)b2Alloc( newCapacity * sizeof( b2TreeNode ) );++#if B2_TREE_HEURISTIC == 0+		b2Free( tree->leafCenters, oldCapacity * sizeof( b2Vec2 ) );+		tree->leafCenters = (b2Vec2*)b2Alloc( newCapacity * sizeof( b2Vec2 ) );+#else+		b2Free( tree->leafBoxes, oldCapacity * sizeof( b2AABB ) );+		tree->leafBoxes = (b2AABB*)b2Alloc( newCapacity * sizeof( b2AABB ) );+		b2Free( tree->binIndices, oldCapacity * sizeof( int32_t ) );+		tree->binIndices = (int32_t*)b2Alloc( newCapacity * sizeof( int32_t ) );+#endif+		tree->rebuildCapacity = newCapacity;+	}+	int* leafIndices = tree->leafIndices;+	b2TreeNode* leaves = tree->leafNodes;+#if B2_TREE_HEURISTIC == 0+	b2Vec2* leafCenters = tree->leafCenters;+#else+	b2AABB* leafBoxes = tree->leafBoxes;+#endif++	// Gather build leaves. A marked internal node is descended and abandoned in the old array.+	// Everything else is a build leaf, kept subtrees included. A leaf root, or an unmarked root+	// on an unordered array, is the one build leaf.+	int leafCount = 0;+	int stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;+	if ( b2IsLeaf( root ) == false && ( fullBuild || b2IsNodeMoved( root ) ) )+	{+		stack[stackCount++] = b2GetLeftChild( root );+	}+	else+	{+		b2TreeNode node = *root;+		node.flagIndex &= ~B2_MOVED_NODE;+		leafIndices[0] = 0;+		leaves[0] = node;+#if B2_TREE_HEURISTIC == 0+		leafCenters[0] = b2AABB_Center( node.aabb );+#else+		leafBoxes[0] = node.aabb;+#endif+		leafCount = 1;+	}++	while ( stackCount > 0 )+	{+		int pair = stack[--stackCount];+		for ( int i = 0; i < 2; ++i )+		{+			b2TreeNode node = nodes[pair + i];+			if ( b2IsLeaf( &node ) == false && ( fullBuild || b2IsNodeMoved( &node ) ) )+			{+				// todo fail gracefully if the stack runs out in release+				B2_ASSERT( stackCount < B2_TREE_STACK_SIZE );+				stack[stackCount++] = b2GetLeftChild( &node );+				continue;+			}++			node.flagIndex &= ~B2_MOVED_NODE;+			leafIndices[leafCount] = leafCount;+			leaves[leafCount] = node;+#if B2_TREE_HEURISTIC == 0+			leafCenters[leafCount] = b2AABB_Center( node.aabb );+#else+			leafBoxes[leafCount] = node.aabb;+#endif+			leafCount += 1;+		}+	}++	B2_ASSERT( 0 < leafCount && leafCount <= proxyCount );++	b2BuildTree( tree, leafCount );++	// The spare is now the tree and the old array is the spare. The build is dense, so the+	// stale tail of the old array sits above the end and is never read.+	B2_SWAP( tree->nodes, tree->swapNodes );+	tree->pairFreeList = B2_NULL_INDEX;+	tree->dfsOrdered = true;++	b2DynamicTree_Validate( tree );+	b2DynamicTree_ValidateNoMoved( tree );++	return leafCount;+}++// Set the moved flag on the ancestors of the proxy. Serial use case.+void b2DynamicTree_MarkProxyMovedSerial( b2DynamicTree* tree, int proxyId )+{+	B2_VALIDATE( 0 <= proxyId && proxyId < tree->proxyCapacity );++	b2TreeNode* nodes = tree->nodes;+	const int32_t* parents = tree->parents;++	int index = tree->proxies[proxyId].node;+	B2_VALIDATE( 0 <= index && index < tree->nodeEnd );+	B2_VALIDATE( b2IsLeaf( nodes + index ) );++	while ( index != B2_NULL_INDEX )+	{+		nodes[index].flagIndex |= B2_MOVED_NODE;+		index = parents[index];+	}+}++// Update a proxy AABB and flag the ancestors as moved. Thread-safe using atomics.+void b2DynamicTree_MarkProxyMoved( b2DynamicTree* tree, int proxyId, b2AABB aabb )+{+	B2_VALIDATE( 0 <= proxyId && proxyId < tree->proxyCapacity );++	b2TreeNode* nodes = tree->nodes;+	const int32_t* parents = tree->parents;++	int index = tree->proxies[proxyId].node;+	B2_VALIDATE( 0 <= index && index < tree->nodeEnd );++	b2TreeNode* node = nodes + index;+	B2_VALIDATE( b2IsLeaf( node ) );+	B2_VALIDATE( b2AABB_Contains( node->aabb, aabb ) == false );++	// This is not raced since it is the leaf.+	node->aabb = aabb;+	node->flagIndex |= B2_MOVED_NODE;++	index = parents[index];+	while ( index != B2_NULL_INDEX )+	{+		node = nodes + index;++		// Read first to avoid the FetchOr if possible.+		if ( b2AtomicLoadU32Raw( &node->flagIndex ) & B2_MOVED_NODE )+		{+			break;+		}++		uint32_t previousFlags = b2AtomicFetchOrU32( &node->flagIndex, B2_MOVED_NODE );+		if ( previousFlags & B2_MOVED_NODE )+		{+			// Ancestor already visited.+			break;+		}++		index = parents[index];+	}+}++// Clear the moved flags from the entire tree.+void b2DynamicTree_ClearMoved( b2DynamicTree* tree )+{+	b2TreeNode* nodes = tree->nodes;++	b2TreeNode* root = nodes + B2_ROOT_NODE;+	if ( b2IsNodeMoved( root ) == false )+	{+		return;+	}++	root->flagIndex &= ~B2_MOVED_NODE;+	if ( b2IsLeaf( root ) )+	{+		return;+	}++	int stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;+	stack[stackCount++] = b2GetLeftChild( root );++	while ( stackCount > 0 )+	{+		int pair = stack[--stackCount];+		for ( int i = 0; i < 2; ++i )+		{+			b2TreeNode* node = nodes + pair + i;+			if ( node->flagIndex & B2_MOVED_NODE )+			{+				node->flagIndex &= ~B2_MOVED_NODE;+				if ( b2IsLeaf( node ) == false )+				{+					if ( stackCount < B2_TREE_STACK_SIZE )+					{+						stack[stackCount++] = b2GetLeftChild( node );+					}+					else+					{+						// Bad stuff will happen if the moved flags don't get cleared.+						B2_ASSERT( stackCount < B2_TREE_STACK_SIZE );+					}+				}+			}+		}+	}+}++int b2DynamicTree_GatherMovedProxies( const b2DynamicTree* tree, int* proxyIds )+{+	const b2TreeNode* nodes = tree->nodes;+	const b2TreeNode* root = nodes + B2_ROOT_NODE;+	if ( b2IsNodeMoved( root ) == false )+	{+		return 0;+	}++	if ( b2IsLeaf( root ) )+	{+		proxyIds[0] = b2GetProxyId( root );+		return 1;+	}++	int count = 0;+	int stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;+	stack[stackCount++] = b2GetLeftChild( root );++	while ( stackCount > 0 )+	{+		int pair = stack[--stackCount];+		for ( int i = 0; i < 2; ++i )+		{+			const b2TreeNode* node = nodes + pair + i;+			if ( b2IsNodeMoved( node ) == false )+			{+				continue;+			}++			if ( b2IsLeaf( node ) )+			{+				proxyIds[count++] = b2GetProxyId( node );+			}+			else+			{+				if ( stackCount < B2_TREE_STACK_SIZE )+				{+					stack[stackCount++] = b2GetLeftChild( node );+				}+				else+				{+					B2_ASSERT( stackCount < B2_TREE_STACK_SIZE );+				}+			}+		}+	}++	return count;+}++// Slow refit for unit tests.+static void b2RefitSubtree( b2TreeNode* nodes, int rootIndex )+{+	b2TreeNode* root = nodes + rootIndex;+	if ( b2IsLeaf( root ) || b2IsNodeMoved( root ) == false )+	{+		return;+	}++	// Negative entries indicate deferred unions. A parent is pushed before its children so the+	// LIFO order guarantees both children are finished when the union runs.+	int stack[B2_TREE_STACK_SIZE];+	int stackCount = 0;+	// ~0 == -1+	stack[stackCount++] = ~rootIndex;+	stack[stackCount++] = b2GetLeftChild( root );+	stack[stackCount++] = b2GetLeftChild( root ) + 1;++	while ( stackCount > 0 )+	{+		int item = stack[--stackCount];+		if ( item < 0 )+		{+			b2TreeNode* node = nodes + ~item;+			int pair = b2GetLeftChild( node );+			node->aabb = b2UnionV( nodes[pair].aabb, nodes[pair + 1].aabb );+			continue;+		}++		b2TreeNode* node = nodes + item;+		if ( b2IsLeaf( node ) || b2IsNodeMoved( node ) == false )+		{+			continue;+		}++		if ( stackCount + 3 <= B2_TREE_STACK_SIZE )+		{+			int pair = b2GetLeftChild( node );+			stack[stackCount++] = ~item;+			stack[stackCount++] = pair;+			stack[stackCount++] = pair + 1;+		}+		else+		{+			B2_ASSERT( stackCount + 3 <= B2_TREE_STACK_SIZE );+		}+	}+}++void b2DynamicTree_Refit( b2DynamicTree* tree )+{+	if ( b2HasTreeMoved( tree ) == false )+	{+		return;+	}++	b2TreeNode* nodes = tree->nodes;++	if ( tree->dfsOrdered == false )+	{+		// This only happens in unit tests.+		b2RefitSubtree( nodes, B2_ROOT_NODE );+		return;+	}++	for ( int pair = tree->nodeEnd - 2; pair >= 0; pair -= 2 )+	{+		b2TreeNode* node = nodes + pair;+		uint32_t flags1 = node[0].flagIndex;+		uint32_t flags2 = node[1].flagIndex;++		// Did either move?+		if ( ( ( flags1 | flags2 ) & B2_MOVED_NODE ) == 0 )+		{+			continue;+		}++		// Is the node internal and moved?+		bool refit1 = ( flags1 & ( B2_LEAF_NODE | B2_MOVED_NODE ) ) == B2_MOVED_NODE;+		bool refit2 = ( flags2 & ( B2_LEAF_NODE | B2_MOVED_NODE ) ) == B2_MOVED_NODE;++		// If the node needs a refit then get the child index. Otherwise use a valid dummy index.+		// The store function won't actually modify the node at the dummy index. This+		// avoids an unpredictable branch. SIMD FTW.+		int children1 = refit1 ? (int)( flags1 & B2_NODE_INDEX_MASK ) : pair;+		int children2 = refit2 ? (int)( flags2 & B2_NODE_INDEX_MASK ) : pair;++		// Conditionally store the union.+		b2StoreAABBV( &node[0].aabb, b2UnionPairV( nodes + children1 ), refit1 );+		b2StoreAABBV( &node[1].aabb, b2UnionPairV( nodes + children2 ), refit2 );+	}+}
+ upstream/src/dynamic_tree.h view
@@ -0,0 +1,199 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box2d/collision.h"+#include "box2d/math_functions.h"++#define B2_TREE_STACK_SIZE 512++// Used to mark a node as being moved such that pairs need to be generated and the tree+// may need to be rebuilt. This mark comes from a few sources:+// - updating the transform and AABB in the solver from movement+// - a joint disabling collision+// - creating a proxy+// - setting the transform on a body+#define B2_MOVED_NODE ( 1u << 30 )++#define B2_LEAF_NODE ( 1u << 31 )+#define B2_NODE_INDEX_MASK ( 0xFFFFFFFFu & ~( B2_MOVED_NODE | B2_LEAF_NODE ) )++// Used to indicate empty nodes. It also passes as a leaf so that internal node+// processing will naturally skip it.+#define B2_EMPTY_NODE ( B2_NODE_INDEX_MASK | B2_LEAF_NODE )++#define B2_ROOT_NODE 0++/// A node in the dynamic tree. Siblings sit together at an even index so two nodes fit in a 64 byte+/// cache line. The root is at index zero and index one is always empty.+typedef struct b2TreeNode+{+	/// The node bounding box+	b2AABB aabb;++	/// In 3D this space is used by the AABB z components.+	uint64_t padding;++	/// - bit 31 : 1 for leaf node+	/// - bit 30 : 1 for moved flag+	/// - bits 0-29 : index of the sibling pair node or the proxy id for a leaf+	uint32_t flagIndex;++	union+	{+		/// The height of an internal node. A leaf has zero height.+		int32_t height;++		/// The shape index for a leaf. Truncated from proxy user data.+		int32_t shapeIndex;+	};++} b2TreeNode;++/// Separate storage for tree leaves.+typedef struct b2TreeProxy+{+	/// User data is an index instead of void* because it is used internally as a shape index.+	uint64_t userData;++	/// Category bits for collision filtering.+	uint64_t categoryBits;++	/// The leaf node. B2_NULL_INDEX for a free proxy.+	int32_t node;++	/// Next free proxy.+	int32_t next;++} b2TreeProxy;++struct b2DynamicTree+{+	/// Array of nodes. The root is at index zero and index 1 is empty. Otherwise siblings are+	/// paired at even indices. Has holes for free node pairs.+	b2TreeNode* nodes;++	/// Parent index per node. The free list is interwoven.+	int32_t* parents;++	/// Proxy data split from node array as cold data.+	b2TreeProxy* proxies;++	/// Every allocated node has a lower index than this.+	int32_t nodeEnd;++	/// The allocated node space+	int32_t nodeCapacity;++	/// Free pairs below nodeEnd+	int32_t pairFreeList;++	/// Number of proxies created+	int32_t proxyCount;++	/// The allocated proxy space+	int32_t proxyCapacity;++	/// Proxy free list+	int32_t proxyFreeList;++	/// Array of nodes for rebuild.+	b2TreeNode* swapNodes;++	/// Leaf indices for rebuild+	int32_t* leafIndices;++	/// Leaves for the rebuild. May represent a proxy or a retained subtree.+	b2TreeNode* leafNodes;++	/// Leaf bounding boxes for rebuild+	b2AABB* leafBoxes;++	/// Leaf bounding box centers for rebuild+	b2Vec2* leafCenters;++	/// Bins for sorting during rebuild+	int32_t* binIndices;++	/// Allocated space for rebuilding+	int32_t rebuildCapacity;++	/// Rebuild orders the nodes so the children follow parents. Cache friendly for queries and+	/// refitting. The order can be disrupted by proxy creation.+	bool dfsOrdered;++};++B2_FORCE_INLINE bool b2IsLeaf( const b2TreeNode* node )+{+	return ( node->flagIndex & B2_LEAF_NODE ) == B2_LEAF_NODE;+}++B2_FORCE_INLINE bool b2IsNodeMoved( const b2TreeNode* node )+{+	return ( node->flagIndex & B2_MOVED_NODE ) == B2_MOVED_NODE;+}++B2_FORCE_INLINE bool b2IsEmptyNode( const b2TreeNode* node )+{+	return node->flagIndex == B2_EMPTY_NODE;+}++B2_FORCE_INLINE int b2GetLeftChild( const b2TreeNode* node )+{+	return (int)( node->flagIndex & B2_NODE_INDEX_MASK );+}++B2_FORCE_INLINE int b2GetProxyId( const b2TreeNode* node )+{+	return (int)( node->flagIndex & B2_NODE_INDEX_MASK );+}++B2_FORCE_INLINE int b2GetRootPair( const b2TreeNode* nodes )+{+	const b2TreeNode* root = nodes + B2_ROOT_NODE;+	return b2IsLeaf( root ) ? B2_ROOT_NODE : b2GetLeftChild( root );+}++B2_FORCE_INLINE b2TreeNode b2MakeEmptyNode( void )+{+	return (b2TreeNode){+		.aabb =+			{+				.lowerBound = { .x = INFINITY, .y = INFINITY },+				.upperBound = { .x = -INFINITY, .y = -INFINITY },+			},+		.flagIndex = B2_EMPTY_NODE,+		.height = 0,+	};+}++static inline bool b2HasTreeMoved( const b2DynamicTree* tree )+{+	return b2IsNodeMoved( tree->nodes + B2_ROOT_NODE );+}++static inline bool b2NeedsRebuild( const b2DynamicTree* tree )+{+	return b2IsNodeMoved( tree->nodes + B2_ROOT_NODE ) || tree->dfsOrdered == false;+}++void b2CreateDynamicTreeInternal( b2DynamicTree* tree, int proxyCapacity );+void b2DestroyDynamicTreeInternal( b2DynamicTree* tree );++int b2CreateTreeProxyInternal( b2DynamicTree* tree, b2AABB aabb, uint64_t categoryBits, uint64_t userData,+									   bool markMoved );+void b2DynamicTree_MoveProxyInternal( b2DynamicTree* tree, int proxyId, b2AABB aabb, bool markMoved );+void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb );++/// Rebuild the tree while retaining subtrees that haven't changed. Returns the number of boxes sorted.+int b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild );++void b2DynamicTree_MarkProxyMovedSerial( b2DynamicTree* tree, int proxyId );+void b2DynamicTree_MarkProxyMoved( b2DynamicTree* tree, int proxyId, b2AABB aabb );+void b2DynamicTree_ClearMoved( b2DynamicTree* tree );+int b2DynamicTree_GatherMovedProxies( const b2DynamicTree* tree, int* proxyIds );+void b2DynamicTree_Refit( b2DynamicTree* tree );++void b2DynamicTree_ValidateNoMoved( const b2DynamicTree* tree );
+ upstream/src/geometry.c view
@@ -0,0 +1,1097 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "aabb.h"+#include "shape.h"++#include "box2d/collision.h"+#include "box2d/constants.h"+#include "box2d/math_functions.h"++#include <float.h>+#include <stddef.h>++_Static_assert( B2_MAX_POLYGON_VERTICES > 2, "must be 3 or more" );++bool b2IsValidRay( const b2RayCastInput* input )+{+	bool isValid = b2IsValidVec2( input->origin ) && b2IsValidVec2( input->translation ) &&+				   b2IsValidFloat( input->maxFraction ) && 0.0f <= input->maxFraction && input->maxFraction < B2_HUGE;+	return isValid;+}++static b2Vec2 b2ComputePolygonCentroid( const b2Vec2* vertices, int count )+{+	b2Vec2 center = { 0.0f, 0.0f };+	float area = 0.0f;++	// Get a reference point for forming triangles.+	// Use the first vertex to reduce round-off errors.+	b2Vec2 origin = vertices[0];++	const float inv3 = 1.0f / 3.0f;++	for ( int i = 1; i < count - 1; ++i )+	{+		// Triangle edges+		b2Vec2 e1 = b2Sub( vertices[i], origin );+		b2Vec2 e2 = b2Sub( vertices[i + 1], origin );+		float a = 0.5f * b2Cross( e1, e2 );++		// Area weighted centroid+		center = b2MulAdd( center, a * inv3, b2Add( e1, e2 ) );+		area += a;+	}++	B2_ASSERT( area > FLT_EPSILON );+	float invArea = 1.0f / area;+	center.x *= invArea;+	center.y *= invArea;++	// Restore offset+	center = b2Add( origin, center );++	return center;+}++b2Polygon b2MakePolygon( const b2Hull* hull, float radius )+{+	B2_ASSERT( b2ValidateHull( hull ) );++	if ( hull->count < 3 )+	{+		// Handle a bad hull when assertions are disabled+		return b2MakeSquare( 0.5f );+	}++	b2Polygon shape = { 0 };+	shape.count = hull->count;+	shape.radius = radius;++	// Copy vertices+	for ( int i = 0; i < shape.count; ++i )+	{+		shape.vertices[i] = hull->points[i];+	}++	// Compute normals. Ensure the edges have non-zero length.+	for ( int i = 0; i < shape.count; ++i )+	{+		int i1 = i;+		int i2 = i + 1 < shape.count ? i + 1 : 0;+		b2Vec2 edge = b2Sub( shape.vertices[i2], shape.vertices[i1] );+		B2_ASSERT( b2Dot( edge, edge ) > FLT_EPSILON * FLT_EPSILON );+		shape.normals[i] = b2Normalize( b2CrossVS( edge, 1.0f ) );+	}++	shape.centroid = b2ComputePolygonCentroid( shape.vertices, shape.count );++	return shape;+}++b2Polygon b2MakeOffsetPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation )+{+	return b2MakeOffsetRoundedPolygon( hull, position, rotation, 0.0f );+}++b2Polygon b2MakeOffsetRoundedPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation, float radius )+{+	B2_ASSERT( b2ValidateHull( hull ) );++	if ( hull->count < 3 )+	{+		// Handle a bad hull when assertions are disabled+		return b2MakeSquare( 0.5f );+	}++	b2Transform transform = { position, rotation };++	b2Polygon shape = { 0 };+	shape.count = hull->count;+	shape.radius = radius;++	// Copy vertices+	for ( int i = 0; i < shape.count; ++i )+	{+		shape.vertices[i] = b2TransformPoint( transform, hull->points[i] );+	}++	// Compute normals. Ensure the edges have non-zero length.+	for ( int i = 0; i < shape.count; ++i )+	{+		int i1 = i;+		int i2 = i + 1 < shape.count ? i + 1 : 0;+		b2Vec2 edge = b2Sub( shape.vertices[i2], shape.vertices[i1] );+		B2_ASSERT( b2Dot( edge, edge ) > FLT_EPSILON * FLT_EPSILON );+		shape.normals[i] = b2Normalize( b2CrossVS( edge, 1.0f ) );+	}++	shape.centroid = b2ComputePolygonCentroid( shape.vertices, shape.count );++	return shape;+}++b2Polygon b2MakeSquare( float halfWidth )+{+	return b2MakeBox( halfWidth, halfWidth );+}++b2Polygon b2MakeBox( float halfWidth, float halfHeight )+{+	B2_ASSERT( b2IsValidFloat( halfWidth ) && halfWidth > 0.0f );+	B2_ASSERT( b2IsValidFloat( halfHeight ) && halfHeight > 0.0f );++	b2Polygon shape = { 0 };+	shape.count = 4;+	shape.vertices[0] = (b2Vec2){ -halfWidth, -halfHeight };+	shape.vertices[1] = (b2Vec2){ halfWidth, -halfHeight };+	shape.vertices[2] = (b2Vec2){ halfWidth, halfHeight };+	shape.vertices[3] = (b2Vec2){ -halfWidth, halfHeight };+	shape.normals[0] = (b2Vec2){ 0.0f, -1.0f };+	shape.normals[1] = (b2Vec2){ 1.0f, 0.0f };+	shape.normals[2] = (b2Vec2){ 0.0f, 1.0f };+	shape.normals[3] = (b2Vec2){ -1.0f, 0.0f };+	shape.radius = 0.0f;+	shape.centroid = b2Vec2_zero;+	return shape;+}++b2Polygon b2MakeRoundedBox( float halfWidth, float halfHeight, float radius )+{+	B2_ASSERT( b2IsValidFloat( radius ) && radius >= 0.0f );+	b2Polygon shape = b2MakeBox( halfWidth, halfHeight );+	shape.radius = radius;+	return shape;+}++b2Polygon b2MakeOffsetBox( float halfWidth, float halfHeight, b2Vec2 center, b2Rot rotation )+{+	b2Transform xf = { center, rotation };++	b2Polygon shape = { 0 };+	shape.count = 4;+	shape.vertices[0] = b2TransformPoint( xf, (b2Vec2){ -halfWidth, -halfHeight } );+	shape.vertices[1] = b2TransformPoint( xf, (b2Vec2){ halfWidth, -halfHeight } );+	shape.vertices[2] = b2TransformPoint( xf, (b2Vec2){ halfWidth, halfHeight } );+	shape.vertices[3] = b2TransformPoint( xf, (b2Vec2){ -halfWidth, halfHeight } );+	shape.normals[0] = b2RotateVector( xf.q, (b2Vec2){ 0.0f, -1.0f } );+	shape.normals[1] = b2RotateVector( xf.q, (b2Vec2){ 1.0f, 0.0f } );+	shape.normals[2] = b2RotateVector( xf.q, (b2Vec2){ 0.0f, 1.0f } );+	shape.normals[3] = b2RotateVector( xf.q, (b2Vec2){ -1.0f, 0.0f } );+	shape.radius = 0.0f;+	shape.centroid = xf.p;+	return shape;+}++b2Polygon b2MakeOffsetRoundedBox( float halfWidth, float halfHeight, b2Vec2 center, b2Rot rotation, float radius )+{+	B2_ASSERT( b2IsValidFloat( radius ) && radius >= 0.0f );+	b2Transform xf = { center, rotation };++	b2Polygon shape = { 0 };+	shape.count = 4;+	shape.vertices[0] = b2TransformPoint( xf, (b2Vec2){ -halfWidth, -halfHeight } );+	shape.vertices[1] = b2TransformPoint( xf, (b2Vec2){ halfWidth, -halfHeight } );+	shape.vertices[2] = b2TransformPoint( xf, (b2Vec2){ halfWidth, halfHeight } );+	shape.vertices[3] = b2TransformPoint( xf, (b2Vec2){ -halfWidth, halfHeight } );+	shape.normals[0] = b2RotateVector( xf.q, (b2Vec2){ 0.0f, -1.0f } );+	shape.normals[1] = b2RotateVector( xf.q, (b2Vec2){ 1.0f, 0.0f } );+	shape.normals[2] = b2RotateVector( xf.q, (b2Vec2){ 0.0f, 1.0f } );+	shape.normals[3] = b2RotateVector( xf.q, (b2Vec2){ -1.0f, 0.0f } );+	shape.radius = radius;+	shape.centroid = xf.p;+	return shape;+}++b2Polygon b2TransformPolygon( b2Transform transform, const b2Polygon* polygon )+{+	b2Polygon p = *polygon;++	for ( int i = 0; i < p.count; ++i )+	{+		p.vertices[i] = b2TransformPoint( transform, p.vertices[i] );+		p.normals[i] = b2RotateVector( transform.q, p.normals[i] );+	}++	p.centroid = b2TransformPoint( transform, p.centroid );++	return p;+}++b2MassData b2ComputeCircleMass( const b2Circle* shape, float density )+{+	float rr = shape->radius * shape->radius;++	b2MassData massData;+	massData.mass = density * B2_PI * rr;+	massData.center = shape->center;++	// inertia about the center of mass+	massData.rotationalInertia = massData.mass * 0.5f * rr;++	return massData;+}++b2MassData b2ComputeCapsuleMass( const b2Capsule* shape, float density )+{+	float radius = shape->radius;+	float rr = radius * radius;+	b2Vec2 p1 = shape->center1;+	b2Vec2 p2 = shape->center2;+	float length = b2Length( b2Sub( p2, p1 ) );+	float ll = length * length;++	float circleMass = density * ( B2_PI * radius * radius );+	float boxMass = density * ( 2.0f * radius * length );++	b2MassData massData;+	massData.mass = circleMass + boxMass;+	massData.center.x = 0.5f * ( p1.x + p2.x );+	massData.center.y = 0.5f * ( p1.y + p2.y );++	// two offset half circles, both halves add up to full circle and each half is offset by half length+	// semi-circle centroid = 4 r / 3 pi+	// Need to apply parallel-axis theorem twice:+	// 1. shift semi-circle centroid to origin+	// 2. shift semi-circle to box end+	// m * ((h + lc)^2 - lc^2) = m * (h^2 + 2 * h * lc)+	// See: https://en.wikipedia.org/wiki/Parallel_axis_theorem+	// I verified this formula by computing the convex hull of a 128 vertex capsule++	// half circle centroid+	float lc = 4.0f * radius / ( 3.0f * B2_PI );++	// half length of rectangular portion of capsule+	float h = 0.5f * length;++	float circleInertia = circleMass * ( 0.5f * rr + h * h + 2.0f * h * lc );+	float boxInertia = boxMass * ( 4.0f * rr + ll ) / 12.0f;+	massData.rotationalInertia = circleInertia + boxInertia;++	return massData;+}++b2MassData b2ComputePolygonMass( const b2Polygon* shape, float density )+{+	// Polygon mass, centroid, and inertia.+	// Let rho be the polygon density in mass per unit area.+	// Then:+	// mass = rho * int(dA)+	// centroid.x = (1/mass) * rho * int(x * dA)+	// centroid.y = (1/mass) * rho * int(y * dA)+	// I = rho * int((x*x + y*y) * dA)+	//+	// We can compute these integrals by summing all the integrals+	// for each triangle of the polygon. To evaluate the integral+	// for a single triangle, we make a change of variables to+	// the (u,v) coordinates of the triangle:+	// x = x0 + e1x * u + e2x * v+	// y = y0 + e1y * u + e2y * v+	// where 0 <= u && 0 <= v && u + v <= 1.+	//+	// We integrate u from [0,1-v] and then v from [0,1].+	// We also need to use the Jacobian of the transformation:+	// D = cross(e1, e2)+	//+	// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)+	//+	// The rest of the derivation is handled by computer algebra.++	B2_ASSERT( shape->count > 0 );++	if ( shape->count == 1 )+	{+		b2Circle circle;+		circle.center = shape->vertices[0];+		circle.radius = shape->radius;+		return b2ComputeCircleMass( &circle, density );+	}++	if ( shape->count == 2 )+	{+		b2Capsule capsule;+		capsule.center1 = shape->vertices[0];+		capsule.center2 = shape->vertices[1];+		capsule.radius = shape->radius;+		return b2ComputeCapsuleMass( &capsule, density );+	}++	b2Vec2 vertices[B2_MAX_POLYGON_VERTICES] = { 0 };+	int count = shape->count;+	float radius = shape->radius;++	if ( radius > 0.0f )+	{+		// Approximate mass of rounded polygons by pushing out the vertices.+		float sqrt2 = 1.412f;+		for ( int i = 0; i < count; ++i )+		{+			int j = i == 0 ? count - 1 : i - 1;+			b2Vec2 n1 = shape->normals[j];+			b2Vec2 n2 = shape->normals[i];++			b2Vec2 mid = b2Normalize( b2Add( n1, n2 ) );+			vertices[i] = b2MulAdd( shape->vertices[i], sqrt2 * radius, mid );+		}+	}+	else+	{+		for ( int i = 0; i < count; ++i )+		{+			vertices[i] = shape->vertices[i];+		}+	}++	b2Vec2 center = { 0.0f, 0.0f };+	float area = 0.0f;+	float rotationalInertia = 0.0f;++	// Get a reference point for forming triangles.+	// Use the first vertex to reduce round-off errors.+	b2Vec2 r = vertices[0];++	const float inv3 = 1.0f / 3.0f;++	for ( int i = 1; i < count - 1; ++i )+	{+		// Triangle edges+		b2Vec2 e1 = b2Sub( vertices[i], r );+		b2Vec2 e2 = b2Sub( vertices[i + 1], r );++		float D = b2Cross( e1, e2 );++		float triangleArea = 0.5f * D;+		area += triangleArea;++		// Area weighted centroid, r at origin+		center = b2MulAdd( center, triangleArea * inv3, b2Add( e1, e2 ) );++		float ex1 = e1.x, ey1 = e1.y;+		float ex2 = e2.x, ey2 = e2.y;++		float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;+		float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;++		rotationalInertia += ( 0.25f * inv3 * D ) * ( intx2 + inty2 );+	}++	b2MassData massData;++	// Total mass+	massData.mass = density * area;++	// Center of mass, shift back from origin at r+	B2_ASSERT( area > FLT_EPSILON );+	float invArea = 1.0f / area;+	center.x *= invArea;+	center.y *= invArea;+	massData.center = b2Add( r, center );++	// Inertia tensor relative to the local origin (point s).+	massData.rotationalInertia = density * rotationalInertia;++	// Shift inertia to center of mass+	massData.rotationalInertia -= massData.mass * b2Dot( center, center );++	// If this goes negative we are hosed+	B2_ASSERT( massData.rotationalInertia >= 0.0f );++	return massData;+}++// AABBs are built in double and narrowed to float once. In large world mode the narrowing rounds+// outward so the box always contains the shape, and the inflation (speculative margin) folds into+// the double step, otherwise it vanishes into a float ULP far from the origin. In float mode the+// rounding helpers are plain casts.++static b2AABB b2ComputeCircleFatAABB( const b2Circle* shape, b2WorldTransform xf, float extra )+{+	b2Pos c = b2TransformWorldPoint( xf, shape->center );+	double r = (double)shape->radius + (double)extra;+	b2AABB aabb = {+		{ b2RoundDownFloat( c.x - r ), b2RoundDownFloat( c.y - r ) },+		{ b2RoundUpFloat( c.x + r ), b2RoundUpFloat( c.y + r ) },+	};+	return aabb;+}++static b2AABB b2ComputeCapsuleFatAABB( const b2Capsule* shape, b2WorldTransform xf, float extra )+{+	b2Pos v1 = b2TransformWorldPoint( xf, shape->center1 );+	b2Pos v2 = b2TransformWorldPoint( xf, shape->center2 );+	double r = (double)shape->radius + (double)extra;+	b2AABB aabb = {+		{ b2RoundDownFloat( ( v1.x < v2.x ? v1.x : v2.x ) - r ), b2RoundDownFloat( ( v1.y < v2.y ? v1.y : v2.y ) - r ) },+		{ b2RoundUpFloat( ( v1.x > v2.x ? v1.x : v2.x ) + r ), b2RoundUpFloat( ( v1.y > v2.y ? v1.y : v2.y ) + r ) },+	};+	return aabb;+}++static b2AABB b2ComputePolygonFatAABB( const b2Polygon* shape, b2WorldTransform xf, float extra )+{+	B2_ASSERT( shape->count > 0 );+	b2Pos v = b2TransformWorldPoint( xf, shape->vertices[0] );+	double lx = v.x, ly = v.y, ux = v.x, uy = v.y;++	for ( int i = 1; i < shape->count; ++i )+	{+		v = b2TransformWorldPoint( xf, shape->vertices[i] );+		lx = v.x < lx ? v.x : lx;+		ly = v.y < ly ? v.y : ly;+		ux = v.x > ux ? v.x : ux;+		uy = v.y > uy ? v.y : uy;+	}++	double r = (double)shape->radius + (double)extra;+	b2AABB aabb = {+		{ b2RoundDownFloat( lx - r ), b2RoundDownFloat( ly - r ) },+		{ b2RoundUpFloat( ux + r ), b2RoundUpFloat( uy + r ) },+	};+	return aabb;+}++static b2AABB b2ComputeSegmentFatAABB( const b2Segment* shape, b2WorldTransform xf, float extra )+{+	b2Pos v1 = b2TransformWorldPoint( xf, shape->point1 );+	b2Pos v2 = b2TransformWorldPoint( xf, shape->point2 );+	double e = (double)extra;+	b2AABB aabb = {+		{ b2RoundDownFloat( ( v1.x < v2.x ? v1.x : v2.x ) - e ), b2RoundDownFloat( ( v1.y < v2.y ? v1.y : v2.y ) - e ) },+		{ b2RoundUpFloat( ( v1.x > v2.x ? v1.x : v2.x ) + e ), b2RoundUpFloat( ( v1.y > v2.y ? v1.y : v2.y ) + e ) },+	};+	return aabb;+}++b2AABB b2ComputeCircleBounds( const b2Circle* shape, b2WorldTransform xf )+{+	return b2ComputeCircleFatAABB( shape, xf, 0.0f );+}++b2AABB b2ComputeCapsuleBounds( const b2Capsule* shape, b2WorldTransform xf )+{+	return b2ComputeCapsuleFatAABB( shape, xf, 0.0f );+}++b2AABB b2ComputePolygonBounds( const b2Polygon* shape, b2WorldTransform xf )+{+	return b2ComputePolygonFatAABB( shape, xf, 0.0f );+}++b2AABB b2ComputeSegmentBounds( const b2Segment* shape, b2WorldTransform xf )+{+	return b2ComputeSegmentFatAABB( shape, xf, 0.0f );+}++b2AABB b2ComputeFatShapeAABB( const b2Shape* shape, b2WorldTransform xf, float extra )+{+	switch ( shape->type )+	{+		case b2_capsuleShape:+			return b2ComputeCapsuleFatAABB( &shape->capsule, xf, extra );+		case b2_circleShape:+			return b2ComputeCircleFatAABB( &shape->circle, xf, extra );+		case b2_polygonShape:+			return b2ComputePolygonFatAABB( &shape->polygon, xf, extra );+		case b2_segmentShape:+			return b2ComputeSegmentFatAABB( &shape->segment, xf, extra );+		case b2_chainSegmentShape:+			return b2ComputeSegmentFatAABB( &shape->chainSegment.segment, xf, extra );+		default:+		{+			B2_ASSERT( false );+			b2AABB empty = { { 0.0f, 0.0f }, { 0.0f, 0.0f } };+			return empty;+		}+	}+}++bool b2PointInCircle( const b2Circle* shape, b2Vec2 point )+{+	b2Vec2 center = shape->center;+	return b2DistanceSquared( point, center ) <= shape->radius * shape->radius;+}++bool b2PointInCapsule( const b2Capsule* shape, b2Vec2 point )+{+	float rr = shape->radius * shape->radius;+	b2Vec2 p1 = shape->center1;+	b2Vec2 p2 = shape->center2;++	b2Vec2 d = b2Sub( p2, p1 );+	float dd = b2Dot( d, d );+	if ( dd == 0.0f )+	{+		// Capsule is really a circle+		return b2DistanceSquared( point, p1 ) <= rr;+	}++	// Get closest point on capsule segment+	// c = p1 + t * d+	// dot(point - c, d) = 0+	// dot(point - p1 - t * d, d) = 0+	// t = dot(point - p1, d) / dot(d, d)+	float t = b2Dot( b2Sub( point, p1 ), d ) / dd;+	t = b2ClampFloat( t, 0.0f, 1.0f );+	b2Vec2 c = b2MulAdd( p1, t, d );++	// Is query point within radius around closest point?+	return b2DistanceSquared( point, c ) <= rr;+}++bool b2PointInPolygon( const b2Polygon* shape, b2Vec2 point )+{+	b2DistanceInput input = { 0 };+	input.proxyA = b2MakeProxy( shape->vertices, shape->count, 0.0f );+	input.proxyB = b2MakeProxy( &point, 1, 0.0f );+	input.transform = b2Transform_identity;+	input.useRadii = false;++	b2SimplexCache cache = { 0 };+	b2DistanceOutput output = b2ComputeShapeDistance( &input, &cache, NULL, 0 );++	return output.distance <= shape->radius;+}++// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019+// http://www.codercorner.com/blog/?p=321+b2CastOutput b2CastRayCircle( const b2Circle* shape, const b2RayCastInput* input )+{+	B2_ASSERT( b2IsValidRay( input ) );++	b2Vec2 p = shape->center;++	b2CastOutput output = { 0 };++	// Shift ray so circle center is the origin+	b2Vec2 s = b2Sub( input->origin, p );++	float r = shape->radius;+	float rr = r * r;++	float length;+	b2Vec2 d = b2GetLengthAndNormalize( &length, input->translation );+	if ( length == 0.0f )+	{+		// zero length ray++		if ( b2LengthSquared( 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 = -b2Dot( s, d );++	// c is the closest point on the line to the origin+	b2Vec2 c = b2MulAdd( s, t, d );++	float cc = b2Dot( c, c );++	if ( cc > rr )+	{+		// closest point is outside the circle+		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 ( b2LengthSquared( s ) < rr )+		{+			// initial overlap+			output.point = input->origin;+			output.hit = true;+		}++		return output;+	}++	// hit point relative to center+	b2Vec2 hitPoint = b2MulAdd( s, fraction, d );++	output.fraction = fraction / length;+	output.normal = b2Normalize( hitPoint );+	output.point = b2MulAdd( p, shape->radius, output.normal );+	output.hit = true;++	return output;+}++b2CastOutput b2CastRayCapsule( const b2Capsule* shape, const b2RayCastInput* input )+{+	B2_ASSERT( b2IsValidRay( input ) );++	b2CastOutput output = { 0 };++	b2Vec2 v1 = shape->center1;+	b2Vec2 v2 = shape->center2;++	b2Vec2 e = b2Sub( v2, v1 );++	float capsuleLength;+	b2Vec2 a = b2GetLengthAndNormalize( &capsuleLength, e );++	if ( capsuleLength < FLT_EPSILON )+	{+		// Capsule is really a circle+		b2Circle circle = { v1, shape->radius };+		return b2CastRayCircle( &circle, input );+	}++	b2Vec2 p1 = input->origin;+	b2Vec2 d = input->translation;++	// Ray from capsule start to ray start+	b2Vec2 q = b2Sub( p1, v1 );+	float qa = b2Dot( q, a );++	// Vector to ray start that is perpendicular to capsule axis+	b2Vec2 qp = b2MulAdd( q, -qa, a );++	float radius = shape->radius;++	// Does the ray start within the infinite length capsule?+	if ( b2Dot( qp, qp ) < radius * radius )+	{+		if ( qa < 0.0f )+		{+			// start point behind capsule segment+			b2Circle circle = { v1, shape->radius };+			return b2CastRayCircle( &circle, input );+		}++		if ( qa > capsuleLength )+		{+			// start point ahead of capsule segment+			b2Circle circle = { v2, shape->radius };+			return b2CastRayCircle( &circle, input );+		}++		// ray starts inside capsule -> no hit+		output.point = input->origin;+		output.hit = true;+		return output;+	}++	// Perpendicular to capsule axis, pointing right+	b2Vec2 n = { a.y, -a.x };++	float rayLength;+	b2Vec2 u = b2GetLengthAndNormalize( &rayLength, d );++	// Intersect ray with infinite length capsule+	// v1 + radius * n + s1 * a = p1 + s2 * u+	// v1 - radius * n + s1 * a = p1 + s2 * u++	// s1 * a - s2 * u = b+	// b = q - radius * ap+	// or+	// b = q + radius * ap++	// Cramer's rule [a -u]+	float den = -a.x * u.y + u.x * a.y;+	if ( -FLT_EPSILON < den && den < FLT_EPSILON )+	{+		// Ray is parallel to capsule and outside infinite length capsule+		return output;+	}++	b2Vec2 b1 = b2MulSub( q, radius, n );+	b2Vec2 b2 = b2MulAdd( q, radius, n );++	float invDen = 1.0f / den;++	// Cramer's rule [a b1]+	float s21 = ( a.x * b1.y - b1.x * a.y ) * invDen;++	// Cramer's rule [a b2]+	float s22 = ( a.x * b2.y - b2.x * a.y ) * invDen;++	float s2;+	b2Vec2 b;+	if ( s21 < s22 )+	{+		s2 = s21;+		b = b1;+	}+	else+	{+		s2 = s22;+		b = b2;+		n = b2Neg( n );+	}++	if ( s2 < 0.0f || input->maxFraction * rayLength < s2 )+	{+		return output;+	}++	// Cramer's rule [b -u]+	float s1 = ( -b.x * u.y + u.x * b.y ) * invDen;++	if ( s1 < 0.0f )+	{+		// ray passes behind capsule segment+		b2Circle circle = { v1, shape->radius };+		return b2CastRayCircle( &circle, input );+	}+	else if ( capsuleLength < s1 )+	{+		// ray passes ahead of capsule segment+		b2Circle circle = { v2, shape->radius };+		return b2CastRayCircle( &circle, input );+	}+	else+	{+		// ray hits capsule side+		output.fraction = s2 / rayLength;+		output.point = b2Add( b2Lerp( v1, v2, s1 / capsuleLength ), b2MulSV( shape->radius, n ) );+		output.normal = n;+		output.hit = true;+		return output;+	}+}++// Ray vs line segment+b2CastOutput b2CastRaySegment( const b2Segment* shape, const b2RayCastInput* input, bool oneSided )+{+	if ( oneSided )+	{+		// Skip left-side collision+		float offset = b2Cross( b2Sub( input->origin, shape->point1 ), b2Sub( shape->point2, shape->point1 ) );+		if ( offset < 0.0f )+		{+			b2CastOutput output = { 0 };+			return output;+		}+	}++	// Put the ray into the edge's frame of reference.+	b2Vec2 p1 = input->origin;+	b2Vec2 d = input->translation;++	b2Vec2 v1 = shape->point1;+	b2Vec2 v2 = shape->point2;+	b2Vec2 e = b2Sub( v2, v1 );++	b2CastOutput output = { 0 };++	float length;+	b2Vec2 eUnit = b2GetLengthAndNormalize( &length, e );+	if ( length == 0.0f )+	{+		return output;+	}++	// Normal points to the right, looking from v1 towards v2+	b2Vec2 normal = b2RightPerp( eUnit );++	// Intersect ray with infinite segment using normal+	// Similar to intersecting a ray with an infinite plane+	// p = p1 + t * d+	// dot(normal, p - v1) = 0+	// dot(normal, p1 - v1) + t * dot(normal, d) = 0+	float numerator = b2Dot( normal, b2Sub( v1, p1 ) );+	float denominator = b2Dot( normal, d );++	if ( denominator == 0.0f )+	{+		// parallel+		return output;+	}++	float t = numerator / denominator;+	if ( t < 0.0f || input->maxFraction < t )+	{+		// out of ray range+		return output;+	}++	// Intersection point on infinite segment+	b2Vec2 p = b2MulAdd( p1, t, d );++	// Compute position of p along segment+	// p = v1 + s * e+	// s = dot(p - v1, e) / dot(e, e)++	float s = b2Dot( b2Sub( p, v1 ), eUnit );+	if ( s < 0.0f || length < s )+	{+		// out of segment range+		return output;+	}++	if ( numerator > 0.0f )+	{+		normal = b2Neg( normal );+	}++	output.fraction = t;+	output.point = p;+	output.normal = normal;+	output.hit = true;++	return output;+}++b2CastOutput b2CastRayPolygon( const b2Polygon* shape, const b2RayCastInput* input )+{+	B2_ASSERT( b2IsValidRay( input ) );++	if ( shape->radius == 0.0f )+	{+		// Shift all math to first vertex since the polygon may be far+		// from the origin.+		b2Vec2 base = shape->vertices[0];++		b2Vec2 p1 = b2Sub( input->origin, base );+		b2Vec2 d = input->translation;++		float lower = 0.0f, upper = input->maxFraction;++		int index = -1;++		b2CastOutput output = { 0 };++		for ( int edgeIndex = 0; edgeIndex < shape->count; ++edgeIndex )+		{+			// p = p1 + a * d+			// dot(normal, p - v) = 0+			// dot(normal, p1 - v) + a * dot(normal, d) = 0+			b2Vec2 vertex = b2Sub( shape->vertices[edgeIndex], base );+			float numerator = b2Dot( shape->normals[edgeIndex], b2Sub( vertex, p1 ) );+			float denominator = b2Dot( shape->normals[edgeIndex], d );++			if ( denominator == 0.0f )+			{+				// Parallel and runs outside edge+				if ( numerator < 0.0f )+				{+					return output;+				}+			}+			else+			{+				// Note: we want this predicate without division:+				// lower < numerator / denominator, where denominator < 0+				// Since denominator < 0, we have to flip the inequality:+				// lower < numerator / denominator <==> denominator * lower > numerator.+				if ( denominator < 0.0f && numerator < lower * denominator )+				{+					// Increase lower.+					// The segment enters this half-space.+					lower = numerator / denominator;+					index = edgeIndex;+				}+				else if ( denominator > 0.0f && numerator < upper * denominator )+				{+					// Decrease upper.+					// The segment exits this half-space.+					upper = numerator / denominator;+				}+			}++			if ( upper < lower )+			{+				// Ray misses+				return output;+			}+		}++		B2_ASSERT( 0.0f <= lower && lower <= input->maxFraction );++		if ( index >= 0 )+		{+			output.fraction = lower;+			output.normal = shape->normals[index];+			output.point = b2MulAdd( input->origin, lower, d );+			output.hit = true;+		}+		else+		{+			// initial overlap+			output.point = input->origin;+			output.hit = true;+		}++		return output;+	}++	b2ShapeCastPairInput castInput;+	castInput.proxyA = b2MakeProxy( shape->vertices, shape->count, shape->radius );+	castInput.proxyB = b2MakeProxy( &input->origin, 1, 0.0f );+	castInput.transform = b2Transform_identity;+	castInput.translationB = input->translation;+	castInput.maxFraction = input->maxFraction;+	castInput.canEncroach = false;+	return b2CastShape( &castInput );+}++b2CastOutput b2CastShapeCircle( const b2Circle* shape, const b2ShapeCastInput* input )+{+	b2ShapeCastPairInput pairInput;+	pairInput.proxyA = b2MakeProxy( &shape->center, 1, shape->radius );+	pairInput.proxyB = input->proxy;+	pairInput.transform = b2Transform_identity;+	pairInput.translationB = input->translation;+	pairInput.maxFraction = input->maxFraction;+	pairInput.canEncroach = input->canEncroach;++	return b2CastShape( &pairInput );+}++b2CastOutput b2CastShapeCapsule( const b2Capsule* shape, const b2ShapeCastInput* input )+{+	b2ShapeCastPairInput pairInput;+	pairInput.proxyA = b2MakeProxy( &shape->center1, 2, shape->radius );+	pairInput.proxyB = input->proxy;+	pairInput.transform = b2Transform_identity;+	pairInput.translationB = input->translation;+	pairInput.maxFraction = input->maxFraction;+	pairInput.canEncroach = input->canEncroach;++	return b2CastShape( &pairInput );+}++b2CastOutput b2CastShapeSegment( const b2Segment* shape, const b2ShapeCastInput* input )+{+	b2ShapeCastPairInput pairInput;+	pairInput.proxyA = b2MakeProxy( &shape->point1, 2, 0.0f );+	pairInput.proxyB = input->proxy;+	pairInput.transform = b2Transform_identity;+	pairInput.translationB = input->translation;+	pairInput.maxFraction = input->maxFraction;+	pairInput.canEncroach = input->canEncroach;++	return b2CastShape( &pairInput );+}++b2CastOutput b2CastShapePolygon( const b2Polygon* shape, const b2ShapeCastInput* input )+{+	b2ShapeCastPairInput pairInput;+	pairInput.proxyA = b2MakeProxy( shape->vertices, shape->count, shape->radius );+	pairInput.proxyB = input->proxy;+	pairInput.transform = b2Transform_identity;+	pairInput.translationB = input->translation;+	pairInput.maxFraction = input->maxFraction;+	pairInput.canEncroach = input->canEncroach;++	return b2CastShape( &pairInput );+}++b2PlaneResult b2CollideMoverAndCircle( const b2Capsule* mover, const b2Circle* shape )+{+	b2DistanceInput distanceInput;+	distanceInput.proxyA = b2MakeProxy( &shape->center, 1, 0.0f );+	distanceInput.proxyB = b2MakeProxy( &mover->center1, 2, mover->radius );+	distanceInput.transform = b2Transform_identity;+	distanceInput.useRadii = false;++	float totalRadius = mover->radius + shape->radius;++	b2SimplexCache cache = { 0 };+	b2DistanceOutput distanceOutput = b2ComputeShapeDistance( &distanceInput, &cache, NULL, 0 );++	if ( distanceOutput.distance <= totalRadius )+	{+		b2Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance };+		return (b2PlaneResult){+			.plane = plane,+			.point = distanceOutput.pointA,+			.hit = true,+		};+	}++	return (b2PlaneResult){ 0 };+}++b2PlaneResult b2CollideMoverAndCapsule( const b2Capsule* mover, const b2Capsule* shape )+{+	b2DistanceInput distanceInput;+	distanceInput.proxyA = b2MakeProxy( &shape->center1, 2, 0.0f );+	distanceInput.proxyB = b2MakeProxy( &mover->center1, 2, mover->radius );+	distanceInput.transform = b2Transform_identity;+	distanceInput.useRadii = false;++	float totalRadius = mover->radius + shape->radius;++	b2SimplexCache cache = { 0 };+	b2DistanceOutput distanceOutput = b2ComputeShapeDistance( &distanceInput, &cache, NULL, 0 );++	if ( distanceOutput.distance <= totalRadius )+	{+		b2Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance };+		return (b2PlaneResult){+			.plane = plane,+			.point = distanceOutput.pointA,+			.hit = true,+		};+	}++	return (b2PlaneResult){ 0 };+}++b2PlaneResult b2CollideMoverAndPolygon( const b2Capsule* mover, const b2Polygon* shape )+{+	b2DistanceInput distanceInput;+	distanceInput.proxyA = b2MakeProxy( shape->vertices, shape->count, shape->radius );+	distanceInput.proxyB = b2MakeProxy( &mover->center1, 2, mover->radius );+	distanceInput.transform = b2Transform_identity;+	distanceInput.useRadii = false;++	float totalRadius = mover->radius + shape->radius;++	b2SimplexCache cache = { 0 };+	b2DistanceOutput distanceOutput = b2ComputeShapeDistance( &distanceInput, &cache, NULL, 0 );++	if ( distanceOutput.distance <= totalRadius )+	{+		b2Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance };+		return (b2PlaneResult){+			.plane = plane,+			.point = distanceOutput.pointA,+			.hit = true,+		};+	}++	return (b2PlaneResult){ 0 };+}++b2PlaneResult b2CollideMoverAndSegment( const b2Capsule* mover, const b2Segment* shape )+{+	b2DistanceInput distanceInput;+	distanceInput.proxyA = b2MakeProxy( &shape->point1, 2, 0.0f );+	distanceInput.proxyB = b2MakeProxy( &mover->center1, 2, mover->radius );+	distanceInput.transform = b2Transform_identity;+	distanceInput.useRadii = false;++	float totalRadius = mover->radius;++	b2SimplexCache cache = { 0 };+	b2DistanceOutput distanceOutput = b2ComputeShapeDistance( &distanceInput, &cache, NULL, 0 );++	if ( distanceOutput.distance <= totalRadius )+	{+		b2Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance };+		return (b2PlaneResult){+			.plane = plane,+			.point = distanceOutput.pointA,+			.hit = true,+		};+	}++	return (b2PlaneResult){ 0 };+}
+ upstream/src/hull.c view
@@ -0,0 +1,328 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "core.h"++#include "box2d/collision.h"+#include "box2d/constants.h"+#include "box2d/math_functions.h"++#include <float.h>++// quickhull recursion+static b2Hull b2RecurseHull( b2Vec2 p1, b2Vec2 p2, b2Vec2* ps, int count )+{+	b2Hull hull;+	hull.count = 0;++	if ( count == 0 )+	{+		return hull;+	}++	// create an edge vector pointing from p1 to p2+	b2Vec2 e = b2Normalize( b2Sub( p2, p1 ) );++	// discard points left of e and find point furthest to the right of e+	b2Vec2 rightPoints[B2_MAX_POLYGON_VERTICES];+	int rightCount = 0;++	int bestIndex = 0;+	float bestDistance = b2Cross( b2Sub( ps[bestIndex], p1 ), e );+	if ( bestDistance > 0.0f )+	{+		rightPoints[rightCount++] = ps[bestIndex];+	}++	for ( int i = 1; i < count; ++i )+	{+		float distance = b2Cross( b2Sub( ps[i], p1 ), e );+		if ( distance > bestDistance )+		{+			bestIndex = i;+			bestDistance = distance;+		}++		if ( distance > 0.0f )+		{+			rightPoints[rightCount++] = ps[i];+		}+	}++	if ( bestDistance < 2.0f * B2_LINEAR_SLOP )+	{+		return hull;+	}++	b2Vec2 bestPoint = ps[bestIndex];++	// compute hull to the right of p1-bestPoint+	b2Hull hull1 = b2RecurseHull( p1, bestPoint, rightPoints, rightCount );++	// compute hull to the right of bestPoint-p2+	b2Hull hull2 = b2RecurseHull( bestPoint, p2, rightPoints, rightCount );++	// stitch together hulls+	for ( int i = 0; i < hull1.count; ++i )+	{+		hull.points[hull.count++] = hull1.points[i];+	}++	hull.points[hull.count++] = bestPoint;++	for ( int i = 0; i < hull2.count; ++i )+	{+		hull.points[hull.count++] = hull2.points[i];+	}++	B2_ASSERT( hull.count < B2_MAX_POLYGON_VERTICES );++	return hull;+}++// quickhull algorithm+// - merges vertices based on B2_LINEAR_SLOP+// - removes collinear points using B2_LINEAR_SLOP+// - returns an empty hull if it fails+b2Hull b2ComputeHull( const b2Vec2* points, int count )+{+	b2Hull hull = { 0 };+	hull.count = 0;++	if ( count < 3 || count > B2_MAX_POLYGON_VERTICES )+	{+		// check your data+		return hull;+	}++	count = b2MinInt( count, B2_MAX_POLYGON_VERTICES );++	b2AABB aabb = { { FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX } };++	// Perform aggressive point welding. First point always remains.+	// Also compute the bounding box for later.+	b2Vec2 ps[B2_MAX_POLYGON_VERTICES];+	int n = 0;+	const float linearSlop = B2_LINEAR_SLOP;+	const float tolSqr = 0.25f * linearSlop * linearSlop;+	for ( int i = 0; i < count; ++i )+	{+		aabb.lowerBound = b2Min( aabb.lowerBound, points[i] );+		aabb.upperBound = b2Max( aabb.upperBound, points[i] );++		b2Vec2 vi = points[i];++		bool unique = true;+		for ( int j = 0; j < n; ++j )+		{+			b2Vec2 vj = ps[j];++			float distSqr = b2DistanceSquared( vi, vj );+			if ( distSqr < tolSqr )+			{+				unique = false;+				break;+			}+		}++		if ( unique )+		{+			ps[n++] = vi;+		}+	}++	if ( n < 3 )+	{+		// all points very close together, check your data and check your scale+		return hull;+	}++	// Find an extreme point as the first point on the hull+	b2Vec2 c = b2AABB_Center( aabb );+	int f1 = 0;+	float dsq1 = b2DistanceSquared( c, ps[f1] );+	for ( int i = 1; i < n; ++i )+	{+		float dsq = b2DistanceSquared( c, ps[i] );+		if ( dsq > dsq1 )+		{+			f1 = i;+			dsq1 = dsq;+		}+	}++	// remove p1 from working set+	b2Vec2 p1 = ps[f1];+	ps[f1] = ps[n - 1];+	n = n - 1;++	int f2 = 0;+	float dsq2 = b2DistanceSquared( p1, ps[f2] );+	for ( int i = 1; i < n; ++i )+	{+		float dsq = b2DistanceSquared( p1, ps[i] );+		if ( dsq > dsq2 )+		{+			f2 = i;+			dsq2 = dsq;+		}+	}++	// remove p2 from working set+	b2Vec2 p2 = ps[f2];+	ps[f2] = ps[n - 1];+	n = n - 1;++	// split the points into points that are left and right of the line p1-p2.+	b2Vec2 rightPoints[B2_MAX_POLYGON_VERTICES - 2];+	int rightCount = 0;++	b2Vec2 leftPoints[B2_MAX_POLYGON_VERTICES - 2];+	int leftCount = 0;++	b2Vec2 e = b2Normalize( b2Sub( p2, p1 ) );++	for ( int i = 0; i < n; ++i )+	{+		float d = b2Cross( b2Sub( ps[i], p1 ), e );++		// slop used here to skip points that are very close to the line p1-p2+		if ( d >= 2.0f * linearSlop )+		{+			rightPoints[rightCount++] = ps[i];+		}+		else if ( d <= -2.0f * linearSlop )+		{+			leftPoints[leftCount++] = ps[i];+		}+	}++	// compute hulls on right and left+	b2Hull hull1 = b2RecurseHull( p1, p2, rightPoints, rightCount );+	b2Hull hull2 = b2RecurseHull( p2, p1, leftPoints, leftCount );++	if ( hull1.count == 0 && hull2.count == 0 )+	{+		// all points collinear+		return hull;+	}++	// stitch hulls together, preserving CCW winding order+	hull.points[hull.count++] = p1;++	for ( int i = 0; i < hull1.count; ++i )+	{+		hull.points[hull.count++] = hull1.points[i];+	}++	hull.points[hull.count++] = p2;++	for ( int i = 0; i < hull2.count; ++i )+	{+		hull.points[hull.count++] = hull2.points[i];+	}++	B2_ASSERT( hull.count <= B2_MAX_POLYGON_VERTICES );++	// merge collinear+	bool searching = true;+	while ( searching && hull.count > 2 )+	{+		searching = false;++		for ( int i = 0; i < hull.count; ++i )+		{+			int i1 = i;+			int i2 = ( i + 1 ) % hull.count;+			int i3 = ( i + 2 ) % hull.count;++			b2Vec2 s1 = hull.points[i1];+			b2Vec2 s2 = hull.points[i2];+			b2Vec2 s3 = hull.points[i3];++			// unit edge vector for s1-s3+			b2Vec2 r = b2Normalize( b2Sub( s3, s1 ) );++			float distance = b2Cross( b2Sub( s2, s1 ), r );+			if ( distance <= 2.0f * linearSlop )+			{+				// remove midpoint from hull+				for ( int j = i2; j < hull.count - 1; ++j )+				{+					hull.points[j] = hull.points[j + 1];+				}+				hull.count -= 1;++				// continue searching for collinear points+				searching = true;++				break;+			}+		}+	}++	if ( hull.count < 3 )+	{+		// all points collinear, shouldn't be reached since this was validated above+		hull.count = 0;+	}++	return hull;+}++bool b2ValidateHull( const b2Hull* hull )+{+	if ( hull->count < 3 || B2_MAX_POLYGON_VERTICES < hull->count )+	{+		return false;+	}++	// test that every point is behind every edge+	for ( int i = 0; i < hull->count; ++i )+	{+		// create an edge vector+		int i1 = i;+		int i2 = i < hull->count - 1 ? i1 + 1 : 0;+		b2Vec2 p = hull->points[i1];+		b2Vec2 e = b2Normalize( b2Sub( hull->points[i2], p ) );++		for ( int j = 0; j < hull->count; ++j )+		{+			// skip points that subtend the current edge+			if ( j == i1 || j == i2 )+			{+				continue;+			}++			float distance = b2Cross( b2Sub( hull->points[j], p ), e );+			if ( distance >= 0.0f )+			{+				return false;+			}+		}+	}++	// test for collinear points+	const float linearSlop = B2_LINEAR_SLOP;+	for ( int i = 0; i < hull->count; ++i )+	{+		int i1 = i;+		int i2 = ( i + 1 ) % hull->count;+		int i3 = ( i + 2 ) % hull->count;++		b2Vec2 p1 = hull->points[i1];+		b2Vec2 p2 = hull->points[i2];+		b2Vec2 p3 = hull->points[i3];++		b2Vec2 e = b2Normalize( b2Sub( p3, p1 ) );++		float distance = b2Cross( b2Sub( p2, p1 ), e );+		if ( distance <= linearSlop )+		{+			// p1-p2-p3 are collinear+			return false;+		}+	}++	return true;+}
+ upstream/src/id_pool.c view
@@ -0,0 +1,79 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "id_pool.h"++b2IdPool b2CreateIdPool( void )+{+	b2IdPool pool = { 0 };+	b2Array_CreateN( pool.freeArray, 32 );+	return pool;+}++void b2DestroyIdPool( b2IdPool* pool )+{+	b2Array_Destroy( pool->freeArray );+	*pool = ( b2IdPool ){ 0 };+}++int b2AllocId( b2IdPool* pool )+{+	int count = pool->freeArray.count;+	if ( count > 0 )+	{+		int id = b2Array_Pop( pool->freeArray );+		return id;+	}++	int id = pool->nextIndex;+	pool->nextIndex += 1;+	return id;+}++void b2FreeId( b2IdPool* pool, int id )+{+	B2_ASSERT( pool->nextIndex > 0 );+	B2_ASSERT( 0 <= id && id < pool->nextIndex );+	b2Array_Push( pool->freeArray, id );+}++#if B2_ENABLE_VALIDATION++void b2ValidateFreeId( b2IdPool* pool, int id )+{+	int freeCount = pool->freeArray.count;+	for ( int i = 0; i < freeCount; ++i )+	{+		if ( pool->freeArray.data[i] == id )+		{+			return;+		}+	}++	B2_ASSERT( 0 );+}++void b2ValidateUsedId( b2IdPool* pool, int id )+{+	int freeCount = pool->freeArray.count;+	for ( int i = 0; i < freeCount; ++i )+	{+		if ( pool->freeArray.data[i] == id )+		{+			B2_ASSERT( 0 );+		}+	}+}++#else++void b2ValidateFreeId( b2IdPool* pool, int id )+{+	B2_UNUSED( pool, id );+}++void b2ValidateUsedId( b2IdPool* pool, int id )+{+	B2_UNUSED( pool, id );+}+#endif
+ upstream/src/id_pool.h view
@@ -0,0 +1,35 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"++typedef struct b2IdPool+{+	b2Array( int ) freeArray;+	int nextIndex;+} b2IdPool;++b2IdPool b2CreateIdPool( void );+void b2DestroyIdPool( b2IdPool* pool );++int b2AllocId( b2IdPool* pool );+void b2FreeId( b2IdPool* pool, int id );+void b2ValidateFreeId( b2IdPool* pool, int id );+void b2ValidateUsedId( b2IdPool* pool, int id );++static inline int b2GetIdCount( b2IdPool* pool )+{+	return pool->nextIndex - pool->freeArray.count;+}++static inline int b2GetIdCapacity( b2IdPool* pool )+{+	return pool->nextIndex;+}++static inline int b2GetIdBytes( b2IdPool* pool )+{+	return b2Array_ByteCount( pool->freeArray );+}
+ upstream/src/island.c view
@@ -0,0 +1,736 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "island.h"++#include "body.h"+#include "contact.h"+#include "core.h"+#include "joint.h"+#include "physics_world.h"+#include "solver_set.h"++#include <stddef.h>++b2Island* b2CreateIsland( b2World* world, int setIndex )+{+	B2_ASSERT( setIndex == b2_awakeSet || setIndex >= b2_firstSleepingSet );++	int islandId = b2AllocId( &world->islandIdPool );++	if ( islandId == world->islands.count )+	{+		b2Island emptyIsland = { 0 };+		b2Array_Push( world->islands, emptyIsland );+	}+	else+	{+		B2_ASSERT( world->islands.data[islandId].setIndex == B2_NULL_INDEX );+	}++	b2SolverSet* set = b2Array_Get( world->solverSets, setIndex );++	b2Island* island = b2Array_Get( world->islands, islandId );+	island->setIndex = setIndex;+	island->localIndex = set->islandSims.count;+	island->islandId = islandId;+	b2Array_Create( island->bodies );+	b2Array_Create( island->contacts );+	b2Array_Create( island->joints );+	island->constraintRemoveCount = 0;++	b2IslandSim* islandSim = b2Array_Emplace( set->islandSims );+	islandSim->islandId = islandId;++	return island;+}++void b2DestroyIsland( b2World* world, int islandId )+{+	if ( world->splitIslandId == islandId )+	{+		world->splitIslandId = B2_NULL_INDEX;+	}++	// assume island is empty+	b2Island* island = b2Array_Get( world->islands, islandId );+	b2SolverSet* set = b2Array_Get( world->solverSets, island->setIndex );+	{+		int localIndex = island->localIndex;+		int lastIndex = set->islandSims.count - 1;+		B2_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 revision)+	b2Array_Destroy( island->bodies );+	b2Array_Destroy( island->contacts );+	b2Array_Destroy( island->joints );+	island->constraintRemoveCount = 0;+	island->localIndex = B2_NULL_INDEX;+	island->islandId = B2_NULL_INDEX;+	island->setIndex = B2_NULL_INDEX;++	B2_VALIDATE( island->localIndex == B2_NULL_INDEX );++	b2FreeId( &world->islandIdPool, islandId );+}++static int b2MergeIslands( b2World* world, int islandIdA, int islandIdB )+{+	if ( islandIdA == islandIdB )+	{+		return islandIdA;+	}++	if ( islandIdA == B2_NULL_INDEX )+	{+		B2_ASSERT( islandIdB != B2_NULL_INDEX );+		return islandIdB;+	}++	if ( islandIdB == B2_NULL_INDEX )+	{+		B2_ASSERT( islandIdA != B2_NULL_INDEX );+		return islandIdA;+	}++	b2Island* smallIsland;+	b2Island* bigIsland;+	{+		b2Island* islandA = b2Array_Get( world->islands, islandIdA );+		b2Island* islandB = b2Array_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;+	b2Array_Grow( 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];+		b2Body* body = b2Array_Get( world->bodies, bodyId );+		B2_VALIDATE( body->islandId == smallIsland->islandId );+		body->islandId = bigIslandId;+		body->islandIndex = bigIsland->bodies.count;+		b2Array_Push( bigIsland->bodies, bodyId );+	}++	// Migrate contacts from smaller island to larger island+	if ( smallIsland->contacts.count > 0 )+	{+		b2Array_Grow( bigIsland->contacts, bigIsland->contacts.count + smallIsland->contacts.count );++		for ( int i = 0; i < smallIsland->contacts.count; ++i )+		{+			b2ContactLink* link = smallIsland->contacts.data + i;+			b2Contact* contact = b2Array_Get( world->contacts, link->contactId );+			contact->islandId = bigIslandId;+			contact->islandIndex = bigIsland->contacts.count;+			b2Array_Push( bigIsland->contacts, *link );+		}+	}++	// Migrate joints from smaller island to larger island+	if ( smallIsland->joints.count > 0 )+	{+		b2Array_Grow( bigIsland->joints, bigIsland->joints.count + smallIsland->joints.count );++		for ( int i = 0; i < smallIsland->joints.count; ++i )+		{+			b2JointLink* link = smallIsland->joints.data + i;+			b2Joint* joint = b2Array_Get( world->joints, link->jointId );+			joint->islandId = bigIslandId;+			joint->islandIndex = bigIsland->joints.count;+			b2Array_Push( bigIsland->joints, *link );+		}+	}++	// Track removed constraints+	bigIsland->constraintRemoveCount += smallIsland->constraintRemoveCount;++	b2DestroyIsland( world, smallIsland->islandId );++	b2ValidateIsland( world, bigIslandId );++	return bigIslandId;+}++static void b2AddContactToIsland( b2World* world, int islandId, b2Contact* contact )+{+	B2_ASSERT( contact->islandId == B2_NULL_INDEX );+	B2_ASSERT( contact->islandIndex == B2_NULL_INDEX );++	b2Island* island = b2Array_Get( world->islands, islandId );++	contact->islandId = islandId;+	contact->islandIndex = island->contacts.count;++	b2ContactLink link;+	link.contactId = contact->contactId;+	link.bodyIdA = contact->edges[0].bodyId;+	link.bodyIdB = contact->edges[1].bodyId;+	b2Array_Push( island->contacts, link );++	b2ValidateIsland( world, islandId );+}++// Link a contact into an island.+void b2LinkContact( b2World* world, b2Contact* contact )+{+	B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) != 0 );++	int bodyIdA = contact->edges[0].bodyId;+	int bodyIdB = contact->edges[1].bodyId;++	b2Body* bodyA = b2Array_Get( world->bodies, bodyIdA );+	b2Body* bodyB = b2Array_Get( world->bodies, bodyIdB );++	B2_ASSERT( bodyA->setIndex != b2_disabledSet && bodyB->setIndex != b2_disabledSet );+	B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet );++	// Wake bodyB if bodyA is awake and bodyB is sleeping+	if ( bodyA->setIndex == b2_awakeSet && bodyB->setIndex >= b2_firstSleepingSet )+	{+		b2WakeSolverSet( world, bodyB->setIndex );+	}++	// Wake bodyA if bodyB is awake and bodyA is sleeping+	if ( bodyB->setIndex == b2_awakeSet && bodyA->setIndex >= b2_firstSleepingSet )+	{+		b2WakeSolverSet( world, bodyA->setIndex );+	}++	int islandIdA = bodyA->islandId;+	int islandIdB = bodyB->islandId;++	// Static bodies have null island indices.+	B2_ASSERT( bodyA->setIndex != b2_staticSet || islandIdA == B2_NULL_INDEX );+	B2_ASSERT( bodyB->setIndex != b2_staticSet || islandIdB == B2_NULL_INDEX );+	B2_ASSERT( islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX );++	// Merge islands. This will destroy one of the islands.+	int finalIslandId = b2MergeIslands( world, islandIdA, islandIdB );++	// Add contact to the island that survived+	b2AddContactToIsland( world, finalIslandId, contact );+}++// This is called when a contact no longer has contact points or when a contact is destroyed.+void b2UnlinkContact( b2World* world, b2Contact* contact )+{+	B2_ASSERT( contact->islandId != B2_NULL_INDEX );++	// remove from island+	int islandId = contact->islandId;+	b2Island* island = b2Array_Get( world->islands, islandId );++	int removeIndex = contact->islandIndex;+	B2_ASSERT( 0 <= removeIndex && removeIndex < island->contacts.count );+	B2_ASSERT( island->contacts.data[removeIndex].contactId == contact->contactId );++	int movedIndex = b2Array_RemoveSwap( island->contacts, removeIndex );+	if ( movedIndex != B2_NULL_INDEX )+	{+		// Fix islandIndex on the contact that was swapped into removeIndex+		b2ContactLink* movedLink = island->contacts.data + removeIndex;+		b2Contact* movedContact = b2Array_Get( world->contacts, movedLink->contactId );+		B2_ASSERT( movedContact->islandIndex == movedIndex );+		movedContact->islandIndex = removeIndex;+	}++	contact->islandId = B2_NULL_INDEX;+	contact->islandIndex = B2_NULL_INDEX;+	island->constraintRemoveCount += 1;++	b2ValidateIsland( world, islandId );+}++static void b2AddJointToIsland( b2World* world, int islandId, b2Joint* joint )+{+	B2_ASSERT( joint->islandId == B2_NULL_INDEX );+	B2_ASSERT( joint->islandIndex == B2_NULL_INDEX );++	b2Island* island = b2Array_Get( world->islands, islandId );++	joint->islandId = islandId;+	joint->islandIndex = island->joints.count;++	b2JointLink link;+	link.jointId = joint->jointId;+	link.bodyIdA = joint->edges[0].bodyId;+	link.bodyIdB = joint->edges[1].bodyId;+	b2Array_Push( island->joints, link );++	b2ValidateIsland( world, islandId );+}++void b2LinkJoint( b2World* world, b2Joint* joint )+{+	b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId );+	b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId );++	B2_ASSERT( bodyA->type == b2_dynamicBody || bodyB->type == b2_dynamicBody );++	if ( bodyA->setIndex == b2_awakeSet && bodyB->setIndex >= b2_firstSleepingSet )+	{+		b2WakeSolverSet( world, bodyB->setIndex );+	}+	else if ( bodyB->setIndex == b2_awakeSet && bodyA->setIndex >= b2_firstSleepingSet )+	{+		b2WakeSolverSet( world, bodyA->setIndex );+	}++	int islandIdA = bodyA->islandId;+	int islandIdB = bodyB->islandId;++	B2_ASSERT( islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX );++	// Merge islands. This will destroy one of the islands.+	int finalIslandId = b2MergeIslands( world, islandIdA, islandIdB );++	// Add joint the island that survived+	b2AddJointToIsland( world, finalIslandId, joint );+}++void b2UnlinkJoint( b2World* world, b2Joint* joint )+{+	if ( joint->islandId == B2_NULL_INDEX )+	{+		return;+	}++	// remove from island+	int islandId = joint->islandId;+	b2Island* island = b2Array_Get( world->islands, islandId );++	int removeIndex = joint->islandIndex;+	B2_ASSERT( 0 <= removeIndex && removeIndex < island->joints.count );+	B2_ASSERT( island->joints.data[removeIndex].jointId == joint->jointId );++	int movedIndex = b2Array_RemoveSwap( island->joints, removeIndex );+	if ( movedIndex != B2_NULL_INDEX )+	{+		// Fix islandIndex on the joint that was swapped into removeIndex+		b2JointLink* movedLink = island->joints.data + removeIndex;+		b2Joint* movedJoint = b2Array_Get( world->joints, movedLink->jointId );+		B2_ASSERT( movedJoint->islandIndex == movedIndex );+		movedJoint->islandIndex = removeIndex;+	}++	joint->islandId = B2_NULL_INDEX;+	joint->islandIndex = B2_NULL_INDEX;+	island->constraintRemoveCount += 1;++	b2ValidateIsland( world, islandId );+}++// Find parent of a node. Use path halving to speed up further queries.+static inline int b2IslandFindParent( 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 b2IslandUnion( int* parents, int* ranks, int node1, int node2, int* contactCounts, int* jointCounts )+{+	int root1 = b2IslandFindParent( parents, node1 );+	int root2 = b2IslandFindParent( 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 b2SplitIsland( b2World* world, int baseId )+{+	b2Island* baseIsland = b2Array_Get( world->islands, baseId );+	B2_ASSERT( baseIsland->constraintRemoveCount > 0 );+	B2_ASSERT( baseIsland->setIndex == b2_awakeSet );++	b2ValidateIsland( world, baseId );++	// Cache base island fields before b2CreateIsland, 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;+	b2ContactLink* baseContacts = baseIsland->contacts.data;+	int baseContactCapacity = baseIsland->contacts.capacity;++	int baseJointCount = baseIsland->joints.count;+	b2JointLink* baseJoints = baseIsland->joints.data;+	int baseJointCapacity = baseIsland->joints.capacity;++	b2Stack* 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 = b2StackAlloc( alloc, baseBodyCount * sizeof( int ), "parents" );+	int* contactCounts = b2StackAlloc( alloc, baseBodyCount * sizeof( int ), "contact counts" );+	int* jointCounts = b2StackAlloc( alloc, baseBodyCount * sizeof( int ), "joint counts" );+	int* ranks = b2StackAlloc( alloc, baseBodyCount * sizeof( int ), "ranks" );+	for ( int i = 0; i < baseBodyCount; ++i )+	{+		parents[i] = i;+		ranks[i] = 0;+		contactCounts[i] = 0;+		jointCounts[i] = 0;+	}++	b2Body* 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;+		B2_VALIDATE( 0 <= bodyIdA && bodyIdA < world->bodies.count );+		B2_VALIDATE( 0 <= bodyIdB && bodyIdB < world->bodies.count );+		b2Body* bodyA = bodies + bodyIdA;+		b2Body* bodyB = bodies + bodyIdB;+		int islandIndexA = bodyA->islandIndex;+		int islandIndexB = bodyB->islandIndex;++		// Only connect non-static bodies+		if ( islandIndexA != B2_NULL_INDEX && islandIndexB != B2_NULL_INDEX )+		{+			B2_VALIDATE( 0 <= islandIndexA && islandIndexA < baseBodyCount );+			B2_VALIDATE( 0 <= islandIndexB && islandIndexB < baseBodyCount );+			b2IslandUnion( parents, ranks, islandIndexA, islandIndexB, contactCounts, jointCounts );+			int root = b2IslandFindParent( parents, islandIndexA );+			contactCounts[root] += 1;+		}+		else+		{+			int islandIndex = islandIndexA != B2_NULL_INDEX ? islandIndexA : islandIndexB;+			int root = b2IslandFindParent( 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;+		B2_VALIDATE( 0 <= bodyIdA && bodyIdA < world->bodies.count );+		B2_VALIDATE( 0 <= bodyIdB && bodyIdB < world->bodies.count );+		b2Body* bodyA = bodies + bodyIdA;+		b2Body* bodyB = bodies + bodyIdB;+		int islandIndexA = bodyA->islandIndex;+		int islandIndexB = bodyB->islandIndex;++		// Only connect non-static bodies+		if ( islandIndexA != B2_NULL_INDEX && islandIndexB != B2_NULL_INDEX )+		{+			B2_VALIDATE( 0 <= islandIndexA && islandIndexA < baseBodyCount );+			B2_VALIDATE( 0 <= islandIndexB && islandIndexB < baseBodyCount );+			b2IslandUnion( parents, ranks, islandIndexA, islandIndexB, contactCounts, jointCounts );+			int root = b2IslandFindParent( parents, islandIndexA );+			jointCounts[root] += 1;+		}+		else+		{+			int islandIndex = islandIndexA != B2_NULL_INDEX ? islandIndexA : islandIndexB;+			int root = b2IslandFindParent( parents, islandIndex );+			jointCounts[root] += 1;+		}+	}++	// Done with ranks+	b2StackFree( alloc, ranks );+	ranks = NULL;++	// Flatten all parent indices and count connected components.+	int componentCount = 0;+	for ( int i = 0; i < baseBodyCount; ++i )+	{+		parents[i] = b2IslandFindParent( parents, i );+		if ( parents[i] == i )+		{+			componentCount += 1;+		}+	}++	// Early return — island is still fully connected, no split needed.+	if ( componentCount == 1 )+	{+		baseIsland->constraintRemoveCount = 0;+		b2StackFree( alloc, jointCounts );+		b2StackFree( alloc, contactCounts );+		b2StackFree( alloc, parents );+		return;+	}++	// Detach body/contact/joint arrays from base island so b2DestroyIsland 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 = b2StackAlloc( alloc, baseBodyCount * sizeof( int ), "root map" );+	for ( int i = 0; i < baseBodyCount; ++i )+	{+		rootMap[i] = B2_NULL_INDEX;+	}++	int* componentBodyCounts = b2StackAlloc( alloc, componentCount * sizeof( int ), "component body counts" );+	int* componentContactCounts = b2StackAlloc( alloc, componentCount * sizeof( int ), "component contact counts" );+	int* componentJointCounts = b2StackAlloc( 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] == B2_NULL_INDEX )+		{+			rootMap[rootIndex] = islandCount;+			componentBodyCounts[islandCount] = 0;+			componentContactCounts[islandCount] = contactCounts[rootIndex];+			componentJointCounts[islandCount] = jointCounts[rootIndex];+			islandCount += 1;+		}++		componentBodyCounts[rootMap[rootIndex]] += 1;+	}++	B2_ASSERT( islandCount == componentCount );++	// Map from new island index to island id+	int* islandIds = b2StackAlloc( 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+		b2Island* newIsland = b2CreateIsland( world, b2_awakeSet );+		islandIds[i] = newIsland->islandId;++		// Reserve arrays to avoid wasteful growth and memcpy.+		b2Array_Reserve( newIsland->bodies, componentBodyCounts[i] );+		b2Array_Reserve( newIsland->contacts, componentContactCounts[i] );+		b2Array_Reserve( newIsland->joints, componentJointCounts[i] );+	}++	// Assign bodies to new islands+	for ( int i = 0; i < baseBodyCount; ++i )+	{+		int bodyId = baseBodyIds[i];+		int root = b2IslandFindParent( parents, i );+		int newIslandId = islandIds[rootMap[root]];++		b2Body* body = b2Array_Get( world->bodies, bodyId );+		b2Island* newIsland = b2Array_Get( world->islands, newIslandId );++		body->islandId = newIslandId;+		body->islandIndex = newIsland->bodies.count;++		// Ensure the array has the correct capacity+		B2_VALIDATE( newIsland->bodies.count < newIsland->bodies.capacity );+		b2Array_Push( newIsland->bodies, bodyId );+	}++	// Assign contacts to the island of their bodies+	for ( int i = 0; i < baseContactCount; ++i )+	{+		b2ContactLink* link = baseContacts + i;+		b2Contact* contact = b2Array_Get( world->contacts, link->contactId );++		// Static bodies don't have an island id.+		b2Body* bodyA = b2Array_Get( world->bodies, link->bodyIdA );+		b2Body* bodyB = b2Array_Get( world->bodies, link->bodyIdB );+		int targetIslandId = bodyA->islandId != B2_NULL_INDEX ? bodyA->islandId : bodyB->islandId;++		b2Island* targetIsland = b2Array_Get( world->islands, targetIslandId );+		contact->islandId = targetIslandId;+		contact->islandIndex = targetIsland->contacts.count;++		// Ensure the array has the correct capacity+		B2_VALIDATE( targetIsland->contacts.count < targetIsland->contacts.capacity );+		b2Array_Push( targetIsland->contacts, *link );+	}++	// Assign joints to the island of their bodies+	for ( int i = 0; i < baseJointCount; ++i )+	{+		b2JointLink* link = baseJoints + i;+		b2Joint* joint = b2Array_Get( world->joints, link->jointId );++		// Static bodies don't have an island id.+		b2Body* bodyA = b2Array_Get( world->bodies, link->bodyIdA );+		b2Body* bodyB = b2Array_Get( world->bodies, link->bodyIdB );+		int targetIslandId = bodyA->islandId != B2_NULL_INDEX ? bodyA->islandId : bodyB->islandId;++		b2Island* targetIsland = b2Array_Get( world->islands, targetIslandId );+		joint->islandId = targetIslandId;+		joint->islandIndex = targetIsland->joints.count;++		// Ensure the array has the correct capacity+		B2_VALIDATE( targetIsland->joints.count < targetIsland->joints.capacity );+		b2Array_Push( targetIsland->joints, *link );+	}++	// Destroy the base island+	b2DestroyIsland( world, baseId );++	// Free the detached arrays manually+	b2Free( baseBodyIds, baseBodyCapacity * sizeof( int ) );+	b2Free( baseContacts, baseContactCapacity * sizeof( b2ContactLink ) );+	b2Free( baseJoints, baseJointCapacity * sizeof( b2JointLink ) );++	// Free arena items in LIFO order+	b2StackFree( alloc, islandIds );+	b2StackFree( alloc, componentJointCounts );+	b2StackFree( alloc, componentContactCounts );+	b2StackFree( alloc, componentBodyCounts );+	b2StackFree( alloc, rootMap );+	b2StackFree( alloc, jointCounts );+	b2StackFree( alloc, contactCounts );+	b2StackFree( 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 b2SplitIslandTask( void* context )+{+	b2TracyCZoneNC( split, "Split Island", b2_colorOlive, true );++	uint64_t ticks = b2GetTicks();+	b2World* world = context;++	B2_ASSERT( world->splitIslandId != B2_NULL_INDEX );++	b2SplitIsland( world, world->splitIslandId );++	world->splitIslandId = B2_NULL_INDEX;+	world->profile.splitIslands += b2GetMilliseconds( ticks );+	b2TracyCZoneEnd( split );+}++#if B2_ENABLE_VALIDATION+void b2ValidateIsland( b2World* world, int islandId )+{+	if ( islandId == B2_NULL_INDEX )+	{+		return;+	}++	b2Island* island = b2Array_Get( world->islands, islandId );+	B2_ASSERT( island->islandId == islandId );+	B2_ASSERT( island->setIndex != B2_NULL_INDEX );++	{+		B2_ASSERT( island->bodies.count > 0 );+		B2_ASSERT( island->bodies.count <= b2GetIdCount( &world->bodyIdPool ) );++		for ( int i = 0; i < island->bodies.count; ++i )+		{+			b2Body* body = b2Array_Get( world->bodies, island->bodies.data[i] );+			B2_ASSERT( body->islandId == islandId );+			B2_ASSERT( body->islandIndex == i );+			B2_ASSERT( body->setIndex == island->setIndex );+		}+	}++	if ( island->contacts.count > 0 )+	{+		B2_ASSERT( island->contacts.count <= b2GetIdCount( &world->contactIdPool ) );++		for ( int i = 0; i < island->contacts.count; ++i )+		{+			b2ContactLink* link = island->contacts.data + i;+			b2Contact* contact = b2Array_Get( world->contacts, link->contactId );+			B2_ASSERT( contact->setIndex == island->setIndex );+			B2_ASSERT( contact->islandId == islandId );+			B2_ASSERT( contact->islandIndex == i );+		}+	}++	if ( island->joints.count > 0 )+	{+		B2_ASSERT( island->joints.count <= b2GetIdCount( &world->jointIdPool ) );++		for ( int i = 0; i < island->joints.count; ++i )+		{+			b2JointLink* link = island->joints.data + i;+			b2Joint* joint = b2Array_Get( world->joints, link->jointId );+			B2_ASSERT( joint->setIndex == island->setIndex );+			B2_ASSERT( joint->islandId == islandId );+			B2_ASSERT( joint->islandIndex == i );+		}+	}+}++#else++void b2ValidateIsland( b2World* world, int islandId )+{+	B2_UNUSED( world );+	B2_UNUSED( islandId );+}+#endif
+ upstream/src/island.h view
@@ -0,0 +1,107 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"++#include <stdbool.h>+#include <stdint.h>++typedef struct b2Contact b2Contact;+typedef struct b2Joint b2Joint;+typedef struct b2World b2World;++// Cached contact data stored in the island for fast contiguous iteration.+// Avoids touching b2Contact during union-find in b2SplitIsland.+typedef struct b2ContactLink+{+	int contactId;+	int bodyIdA;+	int bodyIdB;+} b2ContactLink;++b2DeclareArray( b2ContactLink );++// Cached joint data stored in the island for fast contiguous iteration.+typedef struct b2JointLink+{+	int jointId;+	int bodyIdA;+	int bodyIdB;+} b2JointLink;++b2DeclareArray( b2JointLink );++// 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 b2Island+{+	// index of solver set stored in b2World+	// may be B2_NULL_INDEX+	int setIndex;++	// island index within set+	// may be B2_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.+	b2Array( 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 b2SplitIsland's union-find pass+	// never needs to touch b2Contact/b2Joint.+	b2Array( b2ContactLink ) contacts;+	b2Array( b2JointLink ) joints;++} b2Island;++b2DeclareArray( b2Island );++// This is used to move islands across solver sets+typedef struct b2IslandSim+{+	int islandId;+} b2IslandSim;++b2DeclareArray( b2IslandSim );++b2Island* b2CreateIsland( b2World* world, int setIndex );+void b2DestroyIsland( b2World* world, int islandId );++// Link contacts into the island graph when it starts having contact points+void b2LinkContact( b2World* world, b2Contact* contact );++// Unlink contact from the island graph when it stops having contact points+void b2UnlinkContact( b2World* world, b2Contact* contact );++// Link a joint into the island graph when it is created+void b2LinkJoint( b2World* world, b2Joint* joint );++// Unlink a joint from the island graph when it is destroyed+void b2UnlinkJoint( b2World* world, b2Joint* joint );++void b2SplitIsland( b2World* world, int baseId );+void b2SplitIslandTask( void* context );++void b2ValidateIsland( b2World* world, int islandId );+
+ upstream/src/joint.c view
@@ -0,0 +1,1854 @@+// SPDX-FileCopyrightText: 2023 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 "box2d/box2d.h"++#include <stddef.h>+#include <stdio.h>+#include <string.h>++static b2JointDef b2DefaultJointDef( void )+{+	b2JointDef def = { 0 };+	def.localFrameA.q = b2Rot_identity;+	def.localFrameB.q = b2Rot_identity;+	def.forceThreshold = FLT_MAX;+	def.torqueThreshold = FLT_MAX;+	def.constraintHertz = 60.0f;+	def.constraintDampingRatio = 2.0f;+	def.drawScale = b2GetLengthUnitsPerMeter();+	return def;+}++b2DistanceJointDef b2DefaultDistanceJointDef( void )+{+	b2DistanceJointDef def = { 0 };+	def.base = b2DefaultJointDef();+	def.lowerSpringForce = -FLT_MAX;+	def.upperSpringForce = FLT_MAX;+	def.length = 1.0f;+	def.maxLength = B2_HUGE;+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2FilterJointDef b2DefaultFilterJointDef( void )+{+	b2FilterJointDef def = { 0 };+	def.base = b2DefaultJointDef();+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2MotorJointDef b2DefaultMotorJointDef( void )+{+	b2MotorJointDef def = { 0 };+	def.base = b2DefaultJointDef();+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2MoverJointDef b2DefaultMoverJointDef( void )+{+	b2MoverJointDef def = { 0 };+	def.base = b2DefaultJointDef();+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2PogoJointDef b2DefaultPogoJointDef( void )+{+	b2PogoJointDef def = { 0 };+	def.base = b2DefaultJointDef();+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2PrismaticJointDef b2DefaultPrismaticJointDef( void )+{+	b2PrismaticJointDef def = { 0 };+	def.base = b2DefaultJointDef();+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2RevoluteJointDef b2DefaultRevoluteJointDef( void )+{+	b2RevoluteJointDef def = { 0 };+	def.base = b2DefaultJointDef();+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2WeldJointDef b2DefaultWeldJointDef( void )+{+	b2WeldJointDef def = { 0 };+	def.base = b2DefaultJointDef();+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2WheelJointDef b2DefaultWheelJointDef( void )+{+	b2WheelJointDef def = { 0 };+	def.base = b2DefaultJointDef();+	def.enableSpring = true;+	def.hertz = 1.0f;+	def.dampingRatio = 0.7f;+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2ExplosionDef b2DefaultExplosionDef( void )+{+	b2ExplosionDef def = { 0 };+	def.maskBits = B2_DEFAULT_MASK_BITS;+	return def;+}++b2Joint* b2GetJointFullId( b2World* world, b2JointId jointId )+{+	int id = jointId.index1 - 1;+	b2Joint* joint = b2Array_Get( world->joints, id );+	B2_ASSERT( joint->jointId == id && joint->generation == jointId.generation );+	return joint;+}++b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint )+{+	if ( joint->setIndex == b2_awakeSet )+	{+		B2_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < B2_GRAPH_COLOR_COUNT );+		b2GraphColor* color = world->constraintGraph.colors + joint->colorIndex;+		return b2Array_Get( color->jointSims, joint->localIndex );+	}++	b2SolverSet* set = b2Array_Get( world->solverSets, joint->setIndex );+	return b2Array_Get( set->jointSims, joint->localIndex );+}++b2JointSim* b2GetJointSimCheckType( b2JointId jointId, b2JointType type )+{+	B2_UNUSED( type );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return NULL;+	}++	b2Joint* joint = b2GetJointFullId( world, jointId );+	B2_ASSERT( joint->type == type );+	b2JointSim* jointSim = b2GetJointSim( world, joint );+	B2_ASSERT( jointSim->type == type );+	return jointSim;+}++static void b2DestroyContactsBetweenBodies( b2World* world, b2Body* bodyA, b2Body* 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;+	}++	// destroy the contacts+	while ( contactKey != B2_NULL_INDEX )+	{+		int contactId = contactKey >> 1;+		int edgeIndex = contactKey & 1;++		b2Contact* contact = b2Array_Get( world->contacts, contactId );+		contactKey = contact->edges[edgeIndex].nextKey;++		int otherEdgeIndex = edgeIndex ^ 1;+		if ( contact->edges[otherEdgeIndex].bodyId == otherBodyId )+		{+			// Careful, this removes the contact from the current doubly linked list+			b2DestroyContact( world, contact );+		}+	}++	b2ValidateSolverSets( world );+}++static bool b2IsValidJointDef( const b2JointDef* def )+{+	return b2IsValidTransform( def->localFrameA ) && b2IsValidTransform( def->localFrameB ) &&+		   b2IsValidFloat( def->forceThreshold ) && def->forceThreshold >= 0.0f && b2IsValidFloat( def->torqueThreshold ) &&+		   def->torqueThreshold >= 0.0f && b2IsValidFloat( def->constraintHertz ) && def->constraintHertz >= 0.0f &&+		   b2IsValidFloat( def->constraintDampingRatio ) && def->constraintDampingRatio >= 0.0f &&+		   b2IsValidFloat( def->drawScale );+}++typedef struct b2JointPair+{+	b2Joint* joint;+	b2JointSim* jointSim;+} b2JointPair;++static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2JointType type )+{+	B2_ASSERT( b2IsValidTransform( def->localFrameA ) );+	B2_ASSERT( b2IsValidTransform( def->localFrameB ) );+	B2_ASSERT( world->worldId == def->bodyIdA.world0 );+	B2_ASSERT( world->worldId == def->bodyIdB.world0 );+	B2_ASSERT( B2_ID_EQUALS( def->bodyIdA, def->bodyIdB ) == false );++	b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA );+	b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB );++	// If the joint prevents collisions, then destroy all contacts between attached bodies.+	// Do this first so world arrays are not disturbed in the following code.+	if ( def->collideConnected == false )+	{+		b2DestroyContactsBetweenBodies( world, bodyA, bodyB );+	}++	int bodyIdA = bodyA->id;+	int bodyIdB = bodyB->id;+	int maxSetIndex = b2MaxInt( bodyA->setIndex, bodyB->setIndex );++	// Create joint id and joint+	int jointId = b2AllocId( &world->jointIdPool );+	if ( jointId == world->joints.count )+	{+		b2Array_Push( world->joints, (b2Joint){ 0 } );+	}++	b2Joint* joint = b2Array_Get( world->joints, jointId );+	joint->jointId = jointId;+	joint->userData = def->userData;+	joint->generation += 1;+	joint->setIndex = B2_NULL_INDEX;+	joint->colorIndex = B2_NULL_INDEX;+	joint->localIndex = B2_NULL_INDEX;+	joint->islandId = B2_NULL_INDEX;+	joint->islandIndex = B2_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 = B2_NULL_INDEX;+	joint->edges[0].nextKey = bodyA->headJointKey;++	int keyA = ( jointId << 1 ) | 0;+	if ( bodyA->headJointKey != B2_NULL_INDEX )+	{+		b2Joint* jointA = b2Array_Get( world->joints, bodyA->headJointKey >> 1 );+		b2JointEdge* 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 = B2_NULL_INDEX;+	joint->edges[1].nextKey = bodyB->headJointKey;++	int keyB = ( jointId << 1 ) | 1;+	if ( bodyB->headJointKey != B2_NULL_INDEX )+	{+		b2Joint* jointB = b2Array_Get( world->joints, bodyB->headJointKey >> 1 );+		b2JointEdge* edgeB = jointB->edges + ( bodyB->headJointKey & 1 );+		edgeB->prevKey = keyB;+	}+	bodyB->headJointKey = keyB;+	bodyB->jointCount += 1;++	b2JointSim* jointSim;++	if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet )+	{+		// if either body is disabled, create in disabled set+		b2SolverSet* set = b2Array_Get( world->solverSets, b2_disabledSet );+		joint->setIndex = b2_disabledSet;+		joint->localIndex = set->jointSims.count;++		jointSim = b2Array_Emplace( set->jointSims );+		memset( jointSim, 0, sizeof( b2JointSim ) );++		jointSim->jointId = jointId;+		jointSim->bodyIdA = bodyIdA;+		jointSim->bodyIdB = bodyIdB;+	}+	else if ( bodyA->type != b2_dynamicBody && bodyB->type != b2_dynamicBody )+	{+		// joint is not attached to a dynamic body+		b2SolverSet* set = b2Array_Get( world->solverSets, b2_staticSet );+		joint->setIndex = b2_staticSet;+		joint->localIndex = set->jointSims.count;++		jointSim = b2Array_Emplace( set->jointSims );+		memset( jointSim, 0, sizeof( b2JointSim ) );++		jointSim->jointId = jointId;+		jointSim->bodyIdA = bodyIdA;+		jointSim->bodyIdB = bodyIdB;+	}+	else if ( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet )+	{+		// if either body is sleeping, wake it+		if ( maxSetIndex >= b2_firstSleepingSet )+		{+			b2WakeSolverSet( world, maxSetIndex );+		}++		joint->setIndex = b2_awakeSet;++		jointSim = b2CreateJointInGraph( world, joint );+		jointSim->jointId = jointId;+		jointSim->bodyIdA = bodyIdA;+		jointSim->bodyIdB = bodyIdB;+	}+	else+	{+		// joint connected between sleeping and/or static bodies+		B2_ASSERT( bodyA->setIndex >= b2_firstSleepingSet || bodyB->setIndex >= b2_firstSleepingSet );+		B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet );++		// joint should go into the sleeping set (not static set)+		int setIndex = maxSetIndex;++		b2SolverSet* set = b2Array_Get( world->solverSets, setIndex );+		joint->setIndex = setIndex;+		joint->localIndex = set->jointSims.count;++		jointSim = b2Array_Emplace( set->jointSims );+		memset( jointSim, 0, sizeof( b2JointSim ) );++		// These must be set to accommodate the merge below+		jointSim->jointId = jointId;+		jointSim->bodyIdA = bodyIdA;+		jointSim->bodyIdB = bodyIdB;++		if ( bodyA->setIndex != bodyB->setIndex && bodyA->setIndex >= b2_firstSleepingSet &&+			 bodyB->setIndex >= b2_firstSleepingSet )+		{+			// merge sleeping sets+			b2MergeSolverSets( world, bodyA->setIndex, bodyB->setIndex );+			B2_ASSERT( bodyA->setIndex == bodyB->setIndex );++			// fix potentially invalid set index+			setIndex = bodyA->setIndex;++			b2SolverSet* mergedSet = b2Array_Get( world->solverSets, setIndex );++			// Careful! The joint sim pointer was orphaned by the set merge.+			jointSim = b2Array_Get( mergedSet->jointSims, joint->localIndex );+		}++		B2_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 = (b2Softness){+		.biasRate = 0.0f,+		.massScale = 1.0f,+		.impulseScale = 0.0f,+	};++	B2_ASSERT( b2IsValidFloat( def->forceThreshold ) && def->forceThreshold >= 0.0f );+	B2_ASSERT( b2IsValidFloat( def->torqueThreshold ) && def->torqueThreshold >= 0.0f );++	jointSim->forceThreshold = def->forceThreshold;+	jointSim->torqueThreshold = def->torqueThreshold;++	B2_ASSERT( jointSim->jointId == jointId );+	B2_ASSERT( jointSim->bodyIdA == bodyIdA );+	B2_ASSERT( jointSim->bodyIdB == bodyIdB );++	if ( joint->setIndex > b2_disabledSet )+	{+		// Add edge to island graph+		b2LinkJoint( world, joint );+	}++	b2ValidateSolverSets( world );++	return (b2JointPair){ joint, jointSim };+}++b2JointId b2CreateDistanceJoint( b2WorldId worldId, const b2DistanceJointDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidJointDef( &def->base ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->length ) && def->length > 0.0f, (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->hertz ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->dampingRatio ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->minLength ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxLength ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxMotorForce ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->motorSpeed ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->lowerSpringForce ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->upperSpringForce ), (b2JointId){ 0 } );+	B2_ASSERT( def->lowerSpringForce <= def->upperSpringForce );++	b2World* world = b2GetWorldFromId( worldId );++	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return (b2JointId){ 0 };+	}++	b2JointPair pair = b2CreateJoint( world, &def->base, b2_distanceJoint );++	b2JointSim* joint = pair.jointSim;++	b2DistanceJoint empty = { 0 };+	joint->distanceJoint = empty;+	joint->distanceJoint.length = b2MaxFloat( def->length, B2_LINEAR_SLOP );+	joint->distanceJoint.hertz = def->hertz;+	joint->distanceJoint.dampingRatio = def->dampingRatio;+	joint->distanceJoint.minLength = b2MaxFloat( def->minLength, B2_LINEAR_SLOP );+	joint->distanceJoint.maxLength = b2MaxFloat( def->minLength, def->maxLength );+	joint->distanceJoint.maxMotorForce = def->maxMotorForce;+	joint->distanceJoint.motorSpeed = def->motorSpeed;+	joint->distanceJoint.enableSpring = def->enableSpring;+	joint->distanceJoint.lowerSpringForce = b2MinFloat( def->lowerSpringForce, def->upperSpringForce );+	joint->distanceJoint.upperSpringForce = b2MaxFloat( def->lowerSpringForce, def->upperSpringForce );+	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;++	b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation };++	B2_REC_CREATE( world, CreateDistanceJoint, jointId, worldId, *def );++	return jointId;+}++b2JointId b2CreateFilterJoint( b2WorldId worldId, const b2FilterJointDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidJointDef( &def->base ), (b2JointId){ 0 } );++	b2World* world = b2GetWorldFromId( worldId );++	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return (b2JointId){ 0 };+	}++	b2JointPair pair = b2CreateJoint( world, &def->base, b2_filterJoint );++	b2JointSim* joint = pair.jointSim;++	b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation };++	B2_REC_CREATE( world, CreateFilterJoint, jointId, worldId, *def );++	return jointId;+}++b2JointId b2CreateMotorJoint( b2WorldId worldId, const b2MotorJointDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidJointDef( &def->base ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->linearVelocity ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxVelocityForce ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->angularVelocity ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxVelocityTorque ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->linearHertz ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->linearDampingRatio ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxSpringForce ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->angularHertz ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->angularDampingRatio ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxSpringTorque ), (b2JointId){ 0 } );++	b2World* world = b2GetWorldFromId( worldId );++	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return (b2JointId){ 0 };+	}++	b2JointPair pair = b2CreateJoint( world, &def->base, b2_motorJoint );+	b2JointSim* joint = pair.jointSim;++	joint->motorJoint = (b2MotorJoint){ 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;++	b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation };++	B2_REC_CREATE( world, CreateMotorJoint, jointId, worldId, *def );++	return jointId;+}++b2JointId b2CreateMoverJoint( b2WorldId worldId, const b2MoverJointDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidJointDef( &def->base ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->linearVelocity ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->maxVelocityForce ), (b2JointId){ 0 } );++	b2World* world = b2GetWorldFromId( worldId );++	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return (b2JointId){ 0 };+	}++	b2JointPair pair = b2CreateJoint( world, &def->base, b2_moverJoint );+	b2JointSim* joint = pair.jointSim;++	joint->moverJoint = (b2MoverJoint){ 0 };+	joint->moverJoint.linearVelocity = def->linearVelocity;+	joint->moverJoint.maxVelocityForce = def->maxVelocityForce;++	b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation };++	B2_REC_CREATE( world, CreateMoverJoint, jointId, worldId, *def );++	return jointId;+}++b2JointId b2CreatePogoJoint( b2WorldId worldId, const b2PogoJointDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidJointDef( &def->base ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->normal ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->restLength ) && def->restLength >= 0.0f, (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->hertz ) && def->hertz >= 0.0f, (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->dampingRatio ) && def->dampingRatio >= 0.0f, (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxTensionForce ) && def->maxTensionForce >= 0.0f, (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxCompressionForce ) && def->maxCompressionForce >= 0.0f, (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->impulse ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->velocity ), (b2JointId){ 0 } );++	b2World* world = b2GetWorldFromId( worldId );++	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return (b2JointId){ 0 };+	}++	b2JointPair pair = b2CreateJoint( world, &def->base, b2_pogoJoint );++	b2JointSim* joint = pair.jointSim;++	joint->pogoJoint = (b2PogoJoint){ 0 };+	joint->pogoJoint.restLength = def->restLength;+	joint->pogoJoint.hertz = def->hertz;+	joint->pogoJoint.dampingRatio = def->dampingRatio;+	joint->pogoJoint.maxTensionForce = def->maxTensionForce;+	joint->pogoJoint.maxCompressionForce = def->maxCompressionForce;+	joint->pogoJoint.normal = def->normal;+	joint->pogoJoint.impulse = def->impulse;+	joint->pogoJoint.velocity = def->velocity;++	b2JointId jointId = {+		.index1 = joint->jointId + 1,+		.world0 = world->worldId,+		.generation = pair.joint->generation,+	};++	B2_REC_CREATE( world, CreatePogoJoint, jointId, worldId, *def );++	return jointId;+}++b2JointId b2CreatePrismaticJoint( b2WorldId worldId, const b2PrismaticJointDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidJointDef( &def->base ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->hertz ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->dampingRatio ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->targetTranslation ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->lowerTranslation ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->upperTranslation ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxMotorForce ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->motorSpeed ), (b2JointId){ 0 } );+	B2_ASSERT( def->lowerTranslation <= def->upperTranslation );++	b2World* world = b2GetWorldFromId( worldId );++	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return (b2JointId){ 0 };+	}++	b2JointPair pair = b2CreateJoint( world, &def->base, b2_prismaticJoint );++	b2JointSim* joint = pair.jointSim;++	joint->prismaticJoint = (b2PrismaticJoint){ 0 };+	joint->prismaticJoint.hertz = def->hertz;+	joint->prismaticJoint.dampingRatio = def->dampingRatio;+	joint->prismaticJoint.targetTranslation = def->targetTranslation;+	joint->prismaticJoint.lowerTranslation = b2MinFloat( def->lowerTranslation, def->upperTranslation );+	joint->prismaticJoint.upperTranslation = b2MaxFloat( def->lowerTranslation, 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;++	b2JointId jointId = {+		.index1 = joint->jointId + 1,+		.world0 = world->worldId,+		.generation = pair.joint->generation,+	};++	B2_REC_CREATE( world, CreatePrismaticJoint, jointId, worldId, *def );++	return jointId;+}++b2JointId b2CreateRevoluteJoint( b2WorldId worldId, const b2RevoluteJointDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidJointDef( &def->base ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->targetAngle ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->hertz ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->dampingRatio ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->lowerAngle ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->upperAngle ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxMotorTorque ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->motorSpeed ), (b2JointId){ 0 } );+	B2_ASSERT( def->lowerAngle <= def->upperAngle );++	b2World* world = b2GetWorldFromId( worldId );++	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return (b2JointId){ 0 };+	}++	b2JointPair pair = b2CreateJoint( world, &def->base, b2_revoluteJoint );++	b2JointSim* joint = pair.jointSim;++	b2RevoluteJoint empty = { 0 };+	joint->revoluteJoint = empty;++	joint->revoluteJoint.targetAngle = b2ClampFloat( def->targetAngle, -B2_PI, B2_PI );+	joint->revoluteJoint.hertz = def->hertz;+	joint->revoluteJoint.dampingRatio = def->dampingRatio;++	float lowerAngle = b2MinFloat( def->lowerAngle, def->upperAngle );+	float upperAngle = b2MaxFloat( def->lowerAngle, def->upperAngle );+	joint->revoluteJoint.lowerAngle = b2ClampFloat( lowerAngle, -0.99f * B2_PI, 0.99f * B2_PI );+	joint->revoluteJoint.upperAngle = b2ClampFloat( upperAngle, -0.99f * B2_PI, 0.99f * B2_PI );++	joint->revoluteJoint.maxMotorTorque = def->maxMotorTorque;+	joint->revoluteJoint.motorSpeed = def->motorSpeed;+	joint->revoluteJoint.enableSpring = def->enableSpring;+	joint->revoluteJoint.enableLimit = def->enableLimit;+	joint->revoluteJoint.enableMotor = def->enableMotor;++	b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation };++	B2_REC_CREATE( world, CreateRevoluteJoint, jointId, worldId, *def );++	return jointId;+}++b2JointId b2CreateWeldJoint( b2WorldId worldId, const b2WeldJointDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidJointDef( &def->base ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->linearHertz ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->angularHertz ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->linearDampingRatio ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->angularDampingRatio ), (b2JointId){ 0 } );++	b2World* world = b2GetWorldFromId( worldId );++	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return (b2JointId){ 0 };+	}++	b2JointPair pair = b2CreateJoint( world, &def->base, b2_weldJoint );++	b2JointSim* joint = pair.jointSim;++	b2WeldJoint empty = { 0 };+	joint->weldJoint = empty;+	joint->weldJoint.linearHertz = def->linearHertz;+	joint->weldJoint.linearDampingRatio = def->linearDampingRatio;+	joint->weldJoint.angularHertz = def->angularHertz;+	joint->weldJoint.angularDampingRatio = def->angularDampingRatio;+	joint->weldJoint.linearImpulse = b2Vec2_zero;+	joint->weldJoint.angularImpulse = 0.0f;++	b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation };++	B2_REC_CREATE( world, CreateWeldJoint, jointId, worldId, *def );++	return jointId;+}++b2JointId b2CreateWheelJoint( b2WorldId worldId, const b2WheelJointDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidJointDef( &def->base ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->hertz ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->dampingRatio ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->lowerTranslation ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->upperTranslation ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maxMotorTorque ), (b2JointId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->motorSpeed ), (b2JointId){ 0 } );+	B2_ASSERT( def->lowerTranslation <= def->upperTranslation );++	b2World* world = b2GetWorldFromId( worldId );++	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return (b2JointId){ 0 };+	}++	b2JointPair pair = b2CreateJoint( world, &def->base, b2_wheelJoint );++	b2JointSim* joint = pair.jointSim;++	joint->wheelJoint = (b2WheelJoint){ 0 };+	joint->wheelJoint.perpMass = 0.0f;+	joint->wheelJoint.axialMass = 0.0f;+	joint->wheelJoint.motorImpulse = 0.0f;+	joint->wheelJoint.lowerImpulse = 0.0f;+	joint->wheelJoint.upperImpulse = 0.0f;+	joint->wheelJoint.lowerTranslation = b2MinFloat( def->lowerTranslation, def->upperTranslation );+	joint->wheelJoint.upperTranslation = b2MaxFloat( def->lowerTranslation, def->upperTranslation );+	joint->wheelJoint.maxMotorTorque = def->maxMotorTorque;+	joint->wheelJoint.motorSpeed = def->motorSpeed;+	joint->wheelJoint.hertz = def->hertz;+	joint->wheelJoint.dampingRatio = def->dampingRatio;+	joint->wheelJoint.enableSpring = def->enableSpring;+	joint->wheelJoint.enableLimit = def->enableLimit;+	joint->wheelJoint.enableMotor = def->enableMotor;++	b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation };++	B2_REC_CREATE( world, CreateWheelJoint, jointId, worldId, *def );++	return jointId;+}++void b2DestroyJointInternal( b2World* world, b2Joint* joint )+{+	int jointId = joint->jointId;++	b2JointEdge* edgeA = joint->edges + 0;+	b2JointEdge* edgeB = joint->edges + 1;++	int idA = edgeA->bodyId;+	int idB = edgeB->bodyId;+	b2Body* bodyA = b2Array_Get( world->bodies, idA );+	b2Body* bodyB = b2Array_Get( world->bodies, idB );++	// Remove from body A+	if ( edgeA->prevKey != B2_NULL_INDEX )+	{+		b2Joint* prevJoint = b2Array_Get( world->joints, edgeA->prevKey >> 1 );+		b2JointEdge* prevEdge = prevJoint->edges + ( edgeA->prevKey & 1 );+		prevEdge->nextKey = edgeA->nextKey;+	}++	if ( edgeA->nextKey != B2_NULL_INDEX )+	{+		b2Joint* nextJoint = b2Array_Get( world->joints, edgeA->nextKey >> 1 );+		b2JointEdge* 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 != B2_NULL_INDEX )+	{+		b2Joint* prevJoint = b2Array_Get( world->joints, edgeB->prevKey >> 1 );+		b2JointEdge* prevEdge = prevJoint->edges + ( edgeB->prevKey & 1 );+		prevEdge->nextKey = edgeB->nextKey;+	}++	if ( edgeB->nextKey != B2_NULL_INDEX )+	{+		b2Joint* nextJoint = b2Array_Get( world->joints, edgeB->nextKey >> 1 );+		b2JointEdge* 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 != B2_NULL_INDEX )+	{+		B2_ASSERT( joint->setIndex > b2_disabledSet );+		b2UnlinkJoint( world, joint );+	}+	else+	{+		B2_ASSERT( joint->setIndex <= b2_disabledSet );+	}++	// Remove joint from solver set that owns it+	int setIndex = joint->setIndex;+	int localIndex = joint->localIndex;++	if ( setIndex == b2_awakeSet )+	{+		b2RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, joint->colorIndex, localIndex );+	}+	else+	{+		b2SolverSet* set = b2Array_Get( world->solverSets, setIndex );+		int movedIndex = b2Array_RemoveSwap( set->jointSims, localIndex );+		if ( movedIndex != B2_NULL_INDEX )+		{+			// Fix moved joint+			b2JointSim* movedJointSim = set->jointSims.data + localIndex;+			int movedId = movedJointSim->jointId;+			b2Joint* movedJoint = b2Array_Get( world->joints, movedId );+			B2_ASSERT( movedJoint->localIndex == movedIndex );+			movedJoint->localIndex = localIndex;+		}+	}++	// Free joint and id (preserve joint generation)+	joint->setIndex = B2_NULL_INDEX;+	joint->localIndex = B2_NULL_INDEX;+	joint->colorIndex = B2_NULL_INDEX;+	joint->jointId = B2_NULL_INDEX;+	b2FreeId( &world->jointIdPool, jointId );++	b2WakeBody( world, bodyA );+	b2WakeBody( world, bodyB );++	b2ValidateSolverSets( world );+}++void b2DestroyJoint( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_ASSERT( world->locked == false );++	if ( world->locked )+	{+		return;+	}++	B2_REC( world, DestroyJoint, jointId );++	b2Joint* joint = b2GetJointFullId( world, jointId );++	b2DestroyJointInternal( world, joint );+}++b2JointType b2Joint_GetType( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	return joint->type;+}++b2BodyId b2Joint_GetBodyA( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	return b2MakeBodyId( world, joint->edges[0].bodyId );+}++b2BodyId b2Joint_GetBodyB( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	return b2MakeBodyId( world, joint->edges[1].bodyId );+}++b2WorldId b2Joint_GetWorld( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	return (b2WorldId){ jointId.world0 + 1, world->generation };+}++void b2Joint_SetLocalFrameA( b2JointId jointId, b2Transform localFrame )+{+	B2_CHECK_INPUT( b2IsValidTransform( localFrame ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, JointSetLocalFrameA, jointId, localFrame );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* jointSim = b2GetJointSim( world, joint );+	jointSim->localFrameA = localFrame;+}++b2Transform b2Joint_GetLocalFrameA( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* jointSim = b2GetJointSim( world, joint );+	return jointSim->localFrameA;+}++void b2Joint_SetLocalFrameB( b2JointId jointId, b2Transform localFrame )+{+	B2_CHECK_INPUT( b2IsValidTransform( localFrame ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, JointSetLocalFrameB, jointId, localFrame );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* jointSim = b2GetJointSim( world, joint );+	jointSim->localFrameB = localFrame;+}++b2Transform b2Joint_GetLocalFrameB( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* jointSim = b2GetJointSim( world, joint );+	return jointSim->localFrameB;+}++void b2Joint_SetCollideConnected( b2JointId jointId, bool shouldCollide )+{+	b2World* world = b2GetWorldLocked( jointId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, JointSetCollideConnected, jointId, shouldCollide );++	b2Joint* joint = b2GetJointFullId( world, jointId );+	if ( joint->collideConnected == shouldCollide )+	{+		return;+	}++	joint->collideConnected = shouldCollide;++	b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId );+	b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId );++	if ( shouldCollide )+	{+		b2BodyType typeA = bodyA->type;+		b2BodyType typeB = bodyB->type;++		int shapeId = B2_NULL_INDEX;+		if (typeA == b2_dynamicBody && typeB == b2_dynamicBody)+		{+			shapeId = bodyA->shapeCount < bodyB->shapeCount ? bodyA->headShapeId : bodyB->headShapeId;+		}+		else if ( typeA == b2_dynamicBody )+		{+			shapeId = bodyA->headShapeId;+		}+		else if ( typeB == b2_dynamicBody )+		{+			shapeId = bodyB->headShapeId;+		}++		while ( shapeId != B2_NULL_INDEX )+		{+			b2Shape* shape = b2Array_Get( world->shapes, shapeId );++			if ( shape->proxyKey != B2_NULL_INDEX )+			{+				b2BroadPhase_MarkProxyMovedSerial( &world->broadPhase, shape->proxyKey );+			}++			shapeId = shape->nextShapeId;+		}+	}+	else+	{+		b2DestroyContactsBetweenBodies( world, bodyA, bodyB );+	}+}++bool b2Joint_GetCollideConnected( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	return joint->collideConnected;+}++void b2Joint_SetUserData( b2JointId jointId, void* userData )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	joint->userData = userData;+}++void* b2Joint_GetUserData( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	return joint->userData;+}++void b2Joint_WakeBodies( b2JointId jointId )+{+	b2World* world = b2GetWorldLocked( jointId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, JointWakeBodies, jointId );++	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId );+	b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId );++	b2WakeBody( world, bodyA );+	b2WakeBody( world, bodyB );+}++void b2GetJointReaction( b2JointSim* sim, float invTimeStep, float* force, float* torque )+{+	float linearImpulse = 0.0f;+	float angularImpulse = 0.0f;++	switch ( sim->type )+	{+		case b2_distanceJoint:+		{+			b2DistanceJoint* joint = &sim->distanceJoint;+			linearImpulse = b2AbsFloat( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse );+		}+		break;++		case b2_motorJoint:+		{+			b2MotorJoint* joint = &sim->motorJoint;+			linearImpulse = b2Length( b2Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) );+			angularImpulse = b2AbsFloat( joint->angularVelocityImpulse + joint->angularSpringImpulse );+		}+		break;++		case b2_moverJoint:+		{+			b2MoverJoint* joint = &sim->moverJoint;+			linearImpulse = b2Length( joint->linearVelocityImpulse );+		}+		break;++		case b2_pogoJoint:+		{+			b2PogoJoint* joint = &sim->pogoJoint;+			linearImpulse = b2AbsFloat( joint->impulse );+		}+		break;++		case b2_prismaticJoint:+		{+			b2PrismaticJoint* joint = &sim->prismaticJoint;+			float perpImpulse = joint->impulse.x;+			float axialImpulse = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;+			linearImpulse = sqrtf( perpImpulse * perpImpulse + axialImpulse * axialImpulse );+			angularImpulse = b2AbsFloat( joint->impulse.y );+		}+		break;++		case b2_revoluteJoint:+		{+			b2RevoluteJoint* joint = &sim->revoluteJoint;++			linearImpulse = b2Length( joint->linearImpulse );+			angularImpulse = b2AbsFloat( joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse );+		}+		break;++		case b2_weldJoint:+		{+			b2WeldJoint* joint = &sim->weldJoint;+			linearImpulse = b2Length( joint->linearImpulse );+			angularImpulse = b2AbsFloat( joint->angularImpulse );+		}+		break;++		case b2_wheelJoint:+		{+			b2WheelJoint* joint = &sim->wheelJoint;+			float perpImpulse = joint->perpImpulse;+			float axialImpulse = joint->springImpulse + joint->lowerImpulse - joint->upperImpulse;+			linearImpulse = sqrtf( perpImpulse * perpImpulse + axialImpulse * axialImpulse );+			angularImpulse = b2AbsFloat( joint->motorImpulse );+		}+		break;++		default:+			break;+	}++	*force = linearImpulse * invTimeStep;+	*torque = angularImpulse * invTimeStep;+}++static b2Vec2 b2GetJointConstraintForce( b2World* world, b2Joint* joint )+{+	b2JointSim* base = b2GetJointSim( world, joint );++	switch ( joint->type )+	{+		case b2_distanceJoint:+			return b2GetDistanceJointForce( world, base );++		case b2_motorJoint:+			return b2GetMotorJointForce( world, base );++		case b2_moverJoint:+			return b2GetMoverJointForce( world, base );++		case b2_filterJoint:+			return b2Vec2_zero;++		case b2_pogoJoint:+			return b2GetPogoJointForce( world, base );++		case b2_prismaticJoint:+			return b2GetPrismaticJointForce( world, base );++		case b2_revoluteJoint:+			return b2GetRevoluteJointForce( world, base );++		case b2_weldJoint:+			return b2GetWeldJointForce( world, base );++		case b2_wheelJoint:+			return b2GetWheelJointForce( world, base );++		default:+			B2_ASSERT( false );+			return b2Vec2_zero;+	}+}++static float b2GetJointConstraintTorque( b2World* world, b2Joint* joint )+{+	b2JointSim* base = b2GetJointSim( world, joint );++	switch ( joint->type )+	{+		case b2_distanceJoint:+			return 0.0f;++		case b2_motorJoint:+			return b2GetMotorJointTorque( world, base );++		case b2_filterJoint:+			return 0.0f;++		case b2_prismaticJoint:+			return b2GetPrismaticJointTorque( world, base );++		case b2_revoluteJoint:+			return b2GetRevoluteJointTorque( world, base );++		case b2_weldJoint:+			return b2GetWeldJointTorque( world, base );++		case b2_wheelJoint:+			return b2GetWheelJointTorque( world, base );++		default:+			B2_ASSERT( false );+			return 0.0f;+	}+}++b2Vec2 b2Joint_GetConstraintForce( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	return b2GetJointConstraintForce( world, joint );+}++float b2Joint_GetConstraintTorque( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	return b2GetJointConstraintTorque( world, joint );+}++float b2Joint_GetLinearSeparation( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* base = b2GetJointSim( world, joint );++	// Relative to body A so the difference stays in float precision far from the origin+	b2WorldTransform wxfA = b2GetBodyTransform( world, joint->edges[0].bodyId );+	b2Transform xfA = b2ToRelativeTransform( wxfA, wxfA.p );+	b2Transform xfB = b2ToRelativeTransform( b2GetBodyTransform( world, joint->edges[1].bodyId ), wxfA.p );++	b2Vec2 pA = b2TransformPoint( xfA, base->localFrameA.p );+	b2Vec2 pB = b2TransformPoint( xfB, base->localFrameB.p );+	b2Vec2 dp = b2Sub( pB, pA );++	switch ( joint->type )+	{+		case b2_distanceJoint:+		{+			b2DistanceJoint* distanceJoint = &base->distanceJoint;+			float length = b2Length( 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 b2AbsFloat( length - distanceJoint->length );+		}++		case b2_motorJoint:+			return 0.0f;++		case b2_filterJoint:+			return 0.0f;++		case b2_prismaticJoint:+		{+			b2PrismaticJoint* prismaticJoint = &base->prismaticJoint;+			b2Vec2 axisA = b2RotateVector( xfA.q, (b2Vec2){ 1.0f, 0.0f } );+			b2Vec2 perpA = b2LeftPerp( axisA );+			float perpendicularSeparation = b2AbsFloat( b2Dot( perpA, dp ) );+			float limitSeparation = 0.0f;++			if ( prismaticJoint->enableLimit )+			{+				float translation = b2Dot( 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 b2_revoluteJoint:+			return b2Length( dp );++		case b2_weldJoint:+		{+			b2WeldJoint* weldJoint = &base->weldJoint;+			if ( weldJoint->linearHertz == 0.0f )+			{+				return b2Length( dp );+			}++			return 0.0f;+		}++		case b2_wheelJoint:+		{+			b2WheelJoint* wheelJoint = &base->wheelJoint;+			b2Vec2 axisA = b2RotateVector( xfA.q, (b2Vec2){ 1.0f, 0.0f } );+			b2Vec2 perpA = b2LeftPerp( axisA );+			float perpendicularSeparation = b2AbsFloat( b2Dot( perpA, dp ) );+			float limitSeparation = 0.0f;++			if ( wheelJoint->enableLimit )+			{+				float translation = b2Dot( axisA, dp );+				if ( translation < wheelJoint->lowerTranslation )+				{+					limitSeparation = wheelJoint->lowerTranslation - translation;+				}++				if ( wheelJoint->upperTranslation < translation )+				{+					limitSeparation = translation - wheelJoint->upperTranslation;+				}+			}++			return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation );+		}++		default:+			B2_ASSERT( false );+			return 0.0f;+	}+}++float b2Joint_GetAngularSeparation( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* base = b2GetJointSim( world, joint );++	b2Rot qA = b2GetBodyTransform( world, joint->edges[0].bodyId ).q;+	b2Rot qB = b2GetBodyTransform( world, joint->edges[1].bodyId ).q;+	float relativeAngle = b2RelativeAngle( qA, qB );++	switch ( joint->type )+	{+		case b2_distanceJoint:+			return 0.0f;++		case b2_motorJoint:+			return 0.0f;++		case b2_filterJoint:+			return 0.0f;++		case b2_prismaticJoint:+		{+			return relativeAngle;+		}++		case b2_revoluteJoint:+		{+			b2RevoluteJoint* revoluteJoint = &base->revoluteJoint;+			if ( revoluteJoint->enableLimit )+			{+				float angle = relativeAngle;+				if ( angle < revoluteJoint->lowerAngle )+				{+					return revoluteJoint->lowerAngle - angle;+				}++				if ( revoluteJoint->upperAngle < angle )+				{+					return angle - revoluteJoint->upperAngle;+				}+			}++			return 0.0f;+		}++		case b2_weldJoint:+		{+			b2WeldJoint* weldJoint = &base->weldJoint;+			if ( weldJoint->angularHertz == 0.0f )+			{+				return relativeAngle;+			}++			return 0.0f;+		}++		case b2_wheelJoint:+			return 0.0f;++		default:+			B2_ASSERT( false );+			return 0.0f;+	}+}++void b2Joint_SetConstraintTuning( b2JointId jointId, float hertz, float dampingRatio )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) && hertz >= 0.0f );+	B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, JointSetConstraintTuning, jointId, hertz, dampingRatio );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* base = b2GetJointSim( world, joint );+	base->constraintHertz = hertz;+	base->constraintDampingRatio = dampingRatio;+}++void b2Joint_GetConstraintTuning( b2JointId jointId, float* hertz, float* dampingRatio )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* base = b2GetJointSim( world, joint );+	*hertz = base->constraintHertz;+	*dampingRatio = base->constraintDampingRatio;+}++void b2Joint_SetForceThreshold( b2JointId jointId, float threshold )+{+	B2_CHECK_INPUT( b2IsValidFloat( threshold ) && threshold >= 0.0f );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, JointSetForceThreshold, jointId, threshold );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* base = b2GetJointSim( world, joint );+	base->forceThreshold = threshold;+}++float b2Joint_GetForceThreshold( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* base = b2GetJointSim( world, joint );+	return base->forceThreshold;+}++void b2Joint_SetTorqueThreshold( b2JointId jointId, float threshold )+{+	B2_CHECK_INPUT( b2IsValidFloat( threshold ) && threshold >= 0.0f );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, JointSetTorqueThreshold, jointId, threshold );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* base = b2GetJointSim( world, joint );+	base->torqueThreshold = threshold;+}++float b2Joint_GetTorqueThreshold( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	b2JointSim* base = b2GetJointSim( world, joint );+	return base->torqueThreshold;+}++void b2PrepareJoint( b2JointSim* joint, b2StepContext* context )+{+	// Clamp joint hertz based on the time step to reduce jitter.+	float hertz = b2MinFloat( joint->constraintHertz, 0.25f * context->inv_h );+	joint->constraintSoftness = b2MakeSoft( hertz, joint->constraintDampingRatio, context->h );++	switch ( joint->type )+	{+		case b2_distanceJoint:+			b2PrepareDistanceJoint( joint, context );+			break;++		case b2_motorJoint:+			b2PrepareMotorJoint( joint, context );+			break;++		case b2_moverJoint:+			b2PrepareMoverJoint( joint, context );+			break;++		case b2_filterJoint:+			break;++		case b2_pogoJoint:+			b2PreparePogoJoint( joint, context );+			break;++		case b2_prismaticJoint:+			b2PreparePrismaticJoint( joint, context );+			break;++		case b2_revoluteJoint:+			b2PrepareRevoluteJoint( joint, context );+			break;++		case b2_weldJoint:+			b2PrepareWeldJoint( joint, context );+			break;++		case b2_wheelJoint:+			b2PrepareWheelJoint( joint, context );+			break;++		default:+			B2_ASSERT( false );+	}+}++void b2WarmStartJoint( b2JointSim* joint, b2StepContext* context )+{+	switch ( joint->type )+	{+		case b2_distanceJoint:+			b2WarmStartDistanceJoint( joint, context );+			break;++		case b2_motorJoint:+			b2WarmStartMotorJoint( joint, context );+			break;++		case b2_moverJoint:+			b2WarmStartMoverJoint( joint, context );+			break;++		case b2_filterJoint:+			break;++		case b2_pogoJoint:+			b2WarmStartPogoJoint( joint, context );+			break;++		case b2_prismaticJoint:+			b2WarmStartPrismaticJoint( joint, context );+			break;++		case b2_revoluteJoint:+			b2WarmStartRevoluteJoint( joint, context );+			break;++		case b2_weldJoint:+			b2WarmStartWeldJoint( joint, context );+			break;++		case b2_wheelJoint:+			b2WarmStartWheelJoint( joint, context );+			break;++		default:+			B2_ASSERT( false );+	}+}++void b2SolveJoint( b2JointSim* joint, b2StepContext* context, bool useBias )+{+	switch ( joint->type )+	{+		case b2_distanceJoint:+			b2SolveDistanceJoint( joint, context, useBias );+			break;++		case b2_motorJoint:+			b2SolveMotorJoint( joint, context );+			break;++		case b2_moverJoint:+			b2SolveMoverJoint( joint, context );+			break;++		case b2_filterJoint:+			break;++		case b2_pogoJoint:+			b2SolvePogoJoint( joint, context, useBias );+			break;++		case b2_prismaticJoint:+			b2SolvePrismaticJoint( joint, context, useBias );+			break;++		case b2_revoluteJoint:+			b2SolveRevoluteJoint( joint, context, useBias );+			break;++		case b2_weldJoint:+			b2SolveWeldJoint( joint, context, useBias );+			break;++		case b2_wheelJoint:+			b2SolveWheelJoint( joint, context, useBias );+			break;++		default:+			B2_ASSERT( false );+	}+}++void b2PrepareJoints_Overflow( b2StepContext* context )+{+	b2TracyCZoneNC( prepare_joints, "PrepJoints", b2_colorOldLace, true );++	b2ConstraintGraph* graph = context->graph;+	b2JointSim* joints = graph->colors[B2_OVERFLOW_INDEX].jointSims.data;+	int jointCount = graph->colors[B2_OVERFLOW_INDEX].jointSims.count;++	for ( int i = 0; i < jointCount; ++i )+	{+		b2JointSim* joint = joints + i;+		b2PrepareJoint( joint, context );+	}++	b2TracyCZoneEnd( prepare_joints );+}++void b2WarmStartJoints_Overflow( b2StepContext* context )+{+	b2TracyCZoneNC( prepare_joints, "PrepJoints", b2_colorOldLace, true );++	b2ConstraintGraph* graph = context->graph;+	b2JointSim* joints = graph->colors[B2_OVERFLOW_INDEX].jointSims.data;+	int jointCount = graph->colors[B2_OVERFLOW_INDEX].jointSims.count;++	for ( int i = 0; i < jointCount; ++i )+	{+		b2JointSim* joint = joints + i;+		b2WarmStartJoint( joint, context );+	}++	b2TracyCZoneEnd( prepare_joints );+}++void b2SolveJoints_Overflow( b2StepContext* context, bool useBias )+{+	b2TracyCZoneNC( solve_joints, "Solve Overflow Joints", b2_colorLemonChiffon, true );++	b2ConstraintGraph* graph = context->graph;+	b2JointSim* joints = graph->colors[B2_OVERFLOW_INDEX].jointSims.data;+	int jointCount = graph->colors[B2_OVERFLOW_INDEX].jointSims.count;++	for ( int i = 0; i < jointCount; ++i )+	{+		b2JointSim* joint = joints + i;+		b2SolveJoint( joint, context, useBias );+	}++	b2TracyCZoneEnd( solve_joints );+}++void b2PrepareJointsTask( b2SolverBlock block, b2StepContext* context )+{+	b2TracyCZoneNC( prepare_joints, "PrepJoints", b2_colorOldLace, true );++	b2JointPrepareSpan* 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 = b2MinInt( spans[colorIndex + 1].start, endIndex );+		b2JointSim* joints = spans[colorIndex].joints;++		// Loop over color+		for ( ; index < colorEndIndex; ++index )+		{+			B2_ASSERT( 0 <= index - colorStart && index - colorStart < spans[colorIndex].count );+			b2JointSim* joint = joints + ( index - colorStart );+			b2PrepareJoint( joint, context );+		}++		// Advance to next color+		colorIndex += 1;+	}++	b2TracyCZoneEnd( prepare_joints );+}++void b2WarmStartJointsTask( b2SolverBlock block, b2StepContext* context )+{+	b2TracyCZoneNC( warm_joints, "WarmJoints", b2_colorGold, true );++	b2GraphColor* color = context->graph->colors + block.colorIndex;+	b2JointSim* joints = color->jointSims.data;++	for ( int i = block.startIndex; i < block.startIndex + block.count; ++i )+	{+		b2JointSim* joint = joints + i;+		b2WarmStartJoint( joint, context );+	}++	b2TracyCZoneEnd( warm_joints );+}++void b2SolveJointsTask( b2SolverBlock block, b2StepContext* context, bool useBias, int workerIndex )+{+	b2TracyCZoneNC( solve_joints, "SolveJoints", b2_colorLemonChiffon, true );++	b2GraphColor* color = context->graph->colors + block.colorIndex;+	b2JointSim* joints = color->jointSims.data;++	B2_ASSERT( 0 <= block.startIndex && block.startIndex + block.count <= color->jointSims.count );++	b2BitSet* jointStateBitSet = &context->world->taskContexts.data[workerIndex].jointStateBitSet;++	for ( int i = block.startIndex; i < block.startIndex + block.count; ++i )+	{+		b2JointSim* joint = joints + i;+		b2SolveJoint( joint, context, useBias );++		if ( useBias && ( joint->forceThreshold < FLT_MAX || joint->torqueThreshold < FLT_MAX ) &&+			 b2GetBit( jointStateBitSet, joint->jointId ) == false )+		{+			float force, torque;+			b2GetJointReaction( 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.+				b2SetBit( jointStateBitSet, joint->jointId );+			}+		}+	}++	b2TracyCZoneEnd( solve_joints );+}++void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint )+{+	b2Body* bodyA = b2Array_Get( world->bodies, joint->edges[0].bodyId );+	b2Body* bodyB = b2Array_Get( world->bodies, joint->edges[1].bodyId );+	if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet )+	{+		return;+	}++	b2JointSim* jointSim = b2GetJointSim( world, joint );++	b2WorldTransform xfA = b2GetBodyTransformQuick( world, bodyA );+	b2WorldTransform xfB = b2GetBodyTransformQuick( world, bodyB );++	b2Pos pA = b2TransformWorldPoint( xfA, jointSim->localFrameA.p );+	b2Pos pB = b2TransformWorldPoint( xfB, jointSim->localFrameB.p );++	float scale = b2MaxFloat( 0.0001f, draw->jointScale * joint->drawScale );++	switch ( joint->type )+	{+		case b2_distanceJoint:+			b2DrawDistanceJoint( draw, jointSim, xfA, xfB );+			break;++		case b2_filterJoint:+			draw->DrawPointFcn( pA, 8.0f, b2_colorLightSkyBlue, draw->context );+			draw->DrawPointFcn( pB, 8.0f, b2_colorLightSkyBlue, draw->context );+			if (draw->drawJointExtras)+			{+				draw->DrawLineFcn( pA, pB, b2_colorGold, draw->context );+			}+			break;++		case b2_motorJoint:+			draw->DrawPointFcn( pA, 8.0f, b2_colorYellowGreen, draw->context );+			draw->DrawPointFcn( pB, 8.0f, b2_colorPlum, draw->context );+			if (draw->drawJointExtras)+			{+				draw->DrawLineFcn( pA, pB, b2_colorLightGray, draw->context );+			}+			break;++		case b2_moverJoint:+			draw->DrawPointFcn( pB, 8.0f, b2_colorPlum, draw->context );+			break;++		case b2_pogoJoint:+			draw->DrawLineFcn( pA, pB, b2_colorLightGray, draw->context );+			break;++		case b2_prismaticJoint:+			b2DrawPrismaticJoint( draw, jointSim, xfA, xfB, scale );+			break;++		case b2_revoluteJoint:+			b2DrawRevoluteJoint( draw, jointSim, xfA, xfB, scale );+			break;++		case b2_weldJoint:+			b2DrawWeldJoint( draw, jointSim, xfA, xfB, scale );+			break;++		case b2_wheelJoint:+			b2DrawWheelJoint( draw, jointSim, xfA, xfB, scale );+			break;++		default:+			break;+	}++	if ( draw->drawGraphColors )+	{+		int colorIndex = joint->colorIndex;+		if ( colorIndex != B2_NULL_INDEX )+		{+			b2Pos p = b2LerpPos( pA, pB, 0.5f );+			draw->DrawPointFcn( p, 5.0f, b2GetGraphColor( colorIndex ), draw->context );+		}+	}++	if ( draw->drawJointExtras )+	{+		b2Vec2 force = b2GetJointConstraintForce( world, joint );+		float torque = b2GetJointConstraintTorque( world, joint );+		b2Pos p = b2LerpPos( pA, pB, 0.5f );++		draw->DrawLineFcn( p, b2OffsetPos( p, b2MulSV( 0.001f, force ) ), b2_colorAzure, draw->context );++		char buffer[64];+		snprintf( buffer, 64, "f = [%g, %g], t = %g", force.x, force.y, torque );+		draw->DrawStringFcn( p, buffer, b2_colorAzure, draw->context );+	}+}
+ upstream/src/joint.h view
@@ -0,0 +1,378 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT+#pragma once++#include "container.h"+#include "solver.h"++#include "box2d/types.h"++typedef struct b2DebugDraw b2DebugDraw;+typedef struct b2StepContext b2StepContext;+typedef struct b2World b2World;++/// 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 b2JointEdge+{+	int bodyId;+	int prevKey;+	int nextKey;+} b2JointEdge;++// Map from b2JointId to b2Joint in the solver sets+typedef struct b2Joint+{+	void* userData;++	// index of simulation set stored in b2World+	// B2_NULL_INDEX when slot is free+	int setIndex;++	// index into the constraint graph color array, may be B2_NULL_INDEX for sleeping/disabled joints+	// B2_NULL_INDEX when slot is free+	int colorIndex;++	// joint index within set or graph color+	// B2_NULL_INDEX when slot is free+	int localIndex;++	b2JointEdge edges[2];++	int jointId;+	int islandId;++	// Index into the island's joints array for O(1) swap-removal.+	// B2_NULL_INDEX when not in an island.+	int islandIndex;++	float drawScale;++	b2JointType type;++	// This is monotonically advanced when a body is allocated in this slot+	// Used to check for invalid b2JointId+	uint16_t generation;++	bool collideConnected;++} b2Joint;++// The pogo joint is a hybrid between a contact and a joint+typedef struct b2PogoJoint+{+	// Settings+	b2Vec2 normal;+	float restLength;+	float hertz;+	float dampingRatio;+	float maxTensionForce;+	float maxCompressionForce;+	+	// Runtime+	float impulse;+	int indexA;+	int indexB;+	b2Transform frameA;+	b2Transform frameB;+	b2Vec2 deltaCenter;+	float linearMass;+	float velocity;+} b2PogoJoint;++typedef struct b2DistanceJoint+{+	float length;+	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;+	b2Vec2 anchorA;+	b2Vec2 anchorB;+	b2Vec2 deltaCenter;+	b2Softness distanceSoftness;+	float axialMass;++	bool enableSpring;+	bool enableLimit;+	bool enableMotor;+} b2DistanceJoint;++typedef struct b2MotorJoint+{+	b2Vec2 linearVelocity;+	float maxVelocityForce;+	float angularVelocity;+	float maxVelocityTorque;+	float linearHertz;+	float linearDampingRatio;+	float maxSpringForce;+	float angularHertz;+	float angularDampingRatio;+	float maxSpringTorque;++	b2Vec2 linearVelocityImpulse;+	float angularVelocityImpulse;+	b2Vec2 linearSpringImpulse;+	float angularSpringImpulse;++	b2Softness linearSpring;+	b2Softness angularSpring;++	int indexA;+	int indexB;+	b2Transform frameA;+	b2Transform frameB;+	b2Vec2 deltaCenter;+	b2Mat22 linearMass;+	float angularMass;+} b2MotorJoint;++typedef struct b2MoverJoint+{+	b2Vec2 linearVelocity;+	b2Vec2 maxVelocityForce;++	b2Vec2 linearVelocityImpulse;++	int indexA;+	int indexB;+	b2Transform frameA;+	b2Transform frameB;+	float linearMass;+} b2MoverJoint;++typedef struct b2PrismaticJoint+{+	b2Vec2 impulse;+	float springImpulse;+	float motorImpulse;+	float lowerImpulse;+	float upperImpulse;+	float hertz;+	float dampingRatio;+	float targetTranslation;+	float maxMotorForce;+	float motorSpeed;+	float lowerTranslation;+	float upperTranslation;++	int indexA;+	int indexB;+	b2Transform frameA;+	b2Transform frameB;+	b2Vec2 deltaCenter;+	b2Softness springSoftness;++	bool enableSpring;+	bool enableLimit;+	bool enableMotor;+} b2PrismaticJoint;++typedef struct b2RevoluteJoint+{+	b2Vec2 linearImpulse;+	float springImpulse;+	float motorImpulse;+	float lowerImpulse;+	float upperImpulse;+	float hertz;+	float dampingRatio;+	float targetAngle;+	float maxMotorTorque;+	float motorSpeed;+	float lowerAngle;+	float upperAngle;++	int indexA;+	int indexB;+	b2Transform frameA;+	b2Transform frameB;+	b2Vec2 deltaCenter;+	float axialMass;+	b2Softness springSoftness;++	bool enableSpring;+	bool enableMotor;+	bool enableLimit;+} b2RevoluteJoint;++typedef struct b2WeldJoint+{+	float linearHertz;+	float linearDampingRatio;+	float angularHertz;+	float angularDampingRatio;++	b2Softness linearSpring;+	b2Softness angularSpring;+	b2Vec2 linearImpulse;+	float angularImpulse;++	int indexA;+	int indexB;+	b2Transform frameA;+	b2Transform frameB;+	b2Vec2 deltaCenter;+	float axialMass;+} b2WeldJoint;++typedef struct b2WheelJoint+{+	float perpImpulse;+	float motorImpulse;+	float springImpulse;+	float lowerImpulse;+	float upperImpulse;+	float maxMotorTorque;+	float motorSpeed;+	float lowerTranslation;+	float upperTranslation;+	float hertz;+	float dampingRatio;++	int indexA;+	int indexB;+	b2Transform frameA;+	b2Transform frameB;+	b2Vec2 deltaCenter;+	float perpMass;+	float motorMass;+	float axialMass;+	b2Softness springSoftness;++	bool enableSpring;+	bool enableMotor;+	bool enableLimit;+} b2WheelJoint;++/// The base joint class. Joints are used to constraint two bodies together in+/// various fashions. Some joints also feature limits and motors.+typedef struct b2JointSim+{+	int jointId;++	int bodyIdA;+	int bodyIdB;++	b2JointType type;++	b2Transform localFrameA;+	b2Transform localFrameB;++	float invMassA, invMassB;+	float invIA, invIB;++	float constraintHertz;+	float constraintDampingRatio;++	b2Softness constraintSoftness;++	float forceThreshold;+	float torqueThreshold;++	union+	{+		b2DistanceJoint distanceJoint;+		b2MotorJoint motorJoint;+		b2MoverJoint moverJoint;+		b2PogoJoint pogoJoint;+		b2RevoluteJoint revoluteJoint;+		b2PrismaticJoint prismaticJoint;+		b2WeldJoint weldJoint;+		b2WheelJoint wheelJoint;+	};+} b2JointSim;++void b2DestroyJointInternal( b2World* world, b2Joint* joint );++b2Joint* b2GetJointFullId( b2World* world, b2JointId jointId );+b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint );+b2JointSim* b2GetJointSimCheckType( b2JointId jointId, b2JointType type );++void b2PrepareJoint( b2JointSim* joint, b2StepContext* context );+void b2WarmStartJoint( b2JointSim* joint, b2StepContext* context );+void b2SolveJoint( b2JointSim* joint, b2StepContext* context, bool useBias );++void b2PrepareJoints_Overflow( b2StepContext* context );+void b2WarmStartJoints_Overflow( b2StepContext* context );+void b2SolveJoints_Overflow( b2StepContext* context, bool useBias );++void b2PrepareJointsTask( b2SolverBlock block, b2StepContext* context );+void b2WarmStartJointsTask( b2SolverBlock block, b2StepContext* context );+void b2SolveJointsTask( b2SolverBlock block, b2StepContext* context, bool useBias, int workerIndex );++void b2GetJointReaction( b2JointSim* sim, float invTimeStep, float* force, float* torque );++void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint );++b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base );+b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base );+b2Vec2 b2GetMoverJointForce( b2World* world, b2JointSim* base );+b2Vec2 b2GetPogoJointForce( b2World* world, b2JointSim* base );+b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base );+b2Vec2 b2GetRevoluteJointForce( b2World* world, b2JointSim* base );+b2Vec2 b2GetWeldJointForce( b2World* world, b2JointSim* base );+b2Vec2 b2GetWheelJointForce( b2World* world, b2JointSim* base );++float b2GetMotorJointTorque( b2World* world, b2JointSim* base );+float b2GetPrismaticJointTorque( b2World* world, b2JointSim* base );+float b2GetRevoluteJointTorque( b2World* world, b2JointSim* base );+float b2GetWeldJointTorque( b2World* world, b2JointSim* base );+float b2GetWheelJointTorque( b2World* world, b2JointSim* base );++void b2PrepareDistanceJoint( b2JointSim* base, b2StepContext* context );+void b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context );+void b2PrepareMoverJoint( b2JointSim* base, b2StepContext* context );+void b2PreparePogoJoint( b2JointSim* base, b2StepContext* context );+void b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context );+void b2PrepareRevoluteJoint( b2JointSim* base, b2StepContext* context );+void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context );+void b2PrepareWheelJoint( b2JointSim* base, b2StepContext* context );++void b2WarmStartDistanceJoint( b2JointSim* base, b2StepContext* context );+void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context );+void b2WarmStartMoverJoint( b2JointSim* base, b2StepContext* context );+void b2WarmStartPogoJoint( b2JointSim* base, b2StepContext* context );+void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context );+void b2WarmStartRevoluteJoint( b2JointSim* base, b2StepContext* context );+void b2WarmStartWeldJoint( b2JointSim* base, b2StepContext* context );+void b2WarmStartWheelJoint( b2JointSim* base, b2StepContext* context );++void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBias );+void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context );+void b2SolveMoverJoint( b2JointSim* base, b2StepContext* context );+void b2SolvePogoJoint( b2JointSim* base, b2StepContext* context, bool useBias );+void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias );+void b2SolveRevoluteJoint( b2JointSim* base, b2StepContext* context, bool useBias );+void b2SolveWeldJoint( b2JointSim* base, b2StepContext* context, bool useBias );+void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias );++void b2DrawDistanceJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB );+void b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB,+						   float drawScale );+void b2DrawRevoluteJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB,+						  float drawScale );+void b2DrawWeldJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB,+					  float drawScale );+void b2DrawWheelJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB,+					   float drawScale );++b2DeclareArray( b2Joint );+b2DeclareArray( b2JointSim );
+ upstream/src/manifold.c view
@@ -0,0 +1,1711 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "core.h"++#include "box2d/collision.h"+#include "box2d/constants.h"+#include "box2d/math_functions.h"++#include <float.h>+#include <stddef.h>+#include <stdlib.h>++#if defined( B2_SIMD_SSE2 ) || defined( B2_SIMD_AVX2 )+	#include <emmintrin.h>+#endif++#define B2_MAKE_ID( A, B ) ( (uint8_t)( A ) << 8 | (uint8_t)( B ) )++static b2Polygon b2MakeCapsule( b2Vec2 p1, b2Vec2 p2, float radius )+{+	b2Polygon shape = { 0 };+	shape.vertices[0] = p1;+	shape.vertices[1] = p2;+	shape.centroid = b2Lerp( p1, p2, 0.5f );++	b2Vec2 d = b2Sub( p2, p1 );+	B2_ASSERT( b2LengthSquared( d ) > FLT_EPSILON );+	b2Vec2 axis = b2Normalize( d );+	b2Vec2 normal = b2RightPerp( axis );++	shape.normals[0] = normal;+	shape.normals[1] = b2Neg( normal );+	shape.count = 2;+	shape.radius = radius;++	return shape;+}++b2LocalManifold b2CollideCircles( const b2Circle* circleA, const b2Circle* circleB, b2Transform xf )+{+	b2LocalManifold manifold = { 0 };++	b2Vec2 pointA = circleA->center;+	b2Vec2 pointB = b2TransformPoint( xf, circleB->center );++	float distance;+	b2Vec2 normal = b2GetLengthAndNormalize( &distance, b2Sub( pointB, pointA ) );++	float radiusA = circleA->radius;+	float radiusB = circleB->radius;++	float separation = distance - radiusA - radiusB;+	if ( separation > B2_SPECULATIVE_DISTANCE )+	{+		return manifold;+	}++	b2Vec2 cA = b2MulAdd( pointA, radiusA, normal );+	b2Vec2 cB = b2MulAdd( pointB, -radiusB, normal );++	manifold.normal = normal;+	b2LocalManifoldPoint* mp = manifold.points + 0;+	mp->point = b2Lerp( cA, cB, 0.5f );+	mp->separation = separation;+	mp->id = 0;+	manifold.pointCount = 1;+	return manifold;+}++/// Compute the collision manifold between a capsule and circle+b2LocalManifold b2CollideCapsuleAndCircle( const b2Capsule* capsuleA, const b2Circle* circleB, b2Transform xf )+{+	b2LocalManifold manifold = { 0 };++	// Compute circle position in the frame of the capsule.+	b2Vec2 pB = b2TransformPoint( xf, circleB->center );++	// Compute closest point+	b2Vec2 p1 = capsuleA->center1;+	b2Vec2 p2 = capsuleA->center2;++	b2Vec2 e = b2Sub( p2, p1 );++	// dot(p - pA, e) = 0+	// dot(p - (p1 + s1 * e), e) = 0+	// s1 = dot(p - p1, e)+	b2Vec2 pA;+	float s1 = b2Dot( b2Sub( pB, p1 ), e );+	float s2 = b2Dot( b2Sub( p2, pB ), e );+	if ( s1 < 0.0f )+	{+		// p1 region+		pA = p1;+	}+	else if ( s2 < 0.0f )+	{+		// p2 region+		pA = p2;+	}+	else+	{+		// circle colliding with segment interior+		float s = s1 / b2Dot( e, e );+		pA = b2MulAdd( p1, s, e );+	}++	float distance;+	b2Vec2 normal = b2GetLengthAndNormalize( &distance, b2Sub( pB, pA ) );++	float radiusA = capsuleA->radius;+	float radiusB = circleB->radius;+	float separation = distance - radiusA - radiusB;+	if ( separation > B2_SPECULATIVE_DISTANCE )+	{+		return manifold;+	}++	b2Vec2 cA = b2MulAdd( pA, radiusA, normal );+	b2Vec2 cB = b2MulAdd( pB, -radiusB, normal );++	manifold.normal = normal;+	b2LocalManifoldPoint* mp = manifold.points + 0;+	mp->point = b2Lerp( cA, cB, 0.5f );+	mp->separation = separation;+	mp->id = 0;+	manifold.pointCount = 1;+	return manifold;+}++b2LocalManifold b2CollidePolygonAndCircle( const b2Polygon* polygonA, const b2Circle* circleB, b2Transform xf )+{+	b2LocalManifold manifold = { 0 };+	const float speculativeDistance = B2_SPECULATIVE_DISTANCE;++	// Compute circle position in the frame of the polygon.+	b2Vec2 center = b2TransformPoint( xf, circleB->center );+	float radiusA = polygonA->radius;+	float radiusB = circleB->radius;+	float radius = radiusA + radiusB;++	// Find the min separating edge.+	int normalIndex = 0;+	float separation = -FLT_MAX;+	int vertexCount = polygonA->count;+	const b2Vec2* vertices = polygonA->vertices;+	const b2Vec2* normals = polygonA->normals;++	for ( int i = 0; i < vertexCount; ++i )+	{+		float s = b2Dot( normals[i], b2Sub( center, vertices[i] ) );+		if ( s > separation )+		{+			separation = s;+			normalIndex = i;+		}+	}++	if ( separation > radius + speculativeDistance )+	{+		return manifold;+	}++	// Vertices of the reference edge.+	int vertIndex1 = normalIndex;+	int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;+	b2Vec2 v1 = vertices[vertIndex1];+	b2Vec2 v2 = vertices[vertIndex2];++	// Compute barycentric coordinates+	float u1 = b2Dot( b2Sub( center, v1 ), b2Sub( v2, v1 ) );+	float u2 = b2Dot( b2Sub( center, v2 ), b2Sub( v1, v2 ) );++	if ( u1 < 0.0f && separation > FLT_EPSILON )+	{+		// Circle center is closest to v1 and safely outside the polygon+		b2Vec2 normal = b2Normalize( b2Sub( center, v1 ) );+		separation = b2Dot( b2Sub( center, v1 ), normal );+		if ( separation > radius + speculativeDistance )+		{+			return manifold;+		}++		b2Vec2 cA = b2MulAdd( v1, radiusA, normal );+		b2Vec2 cB = b2MulSub( center, radiusB, normal );+		b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f );++		manifold.normal = normal;+		b2LocalManifoldPoint* mp = manifold.points + 0;+		mp->point = contactPointA;+		mp->separation = b2Dot( b2Sub( cB, cA ), normal );+		mp->id = 0;+		manifold.pointCount = 1;+	}+	else if ( u2 < 0.0f && separation > FLT_EPSILON )+	{+		// Circle center is closest to v2 and safely outside the polygon+		b2Vec2 normal = b2Normalize( b2Sub( center, v2 ) );+		separation = b2Dot( b2Sub( center, v2 ), normal );+		if ( separation > radius + speculativeDistance )+		{+			return manifold;+		}++		b2Vec2 cA = b2MulAdd( v2, radiusA, normal );+		b2Vec2 cB = b2MulSub( center, radiusB, normal );+		b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f );++		manifold.normal = normal;+		b2LocalManifoldPoint* mp = manifold.points + 0;+		mp->point = contactPointA;+		mp->separation = b2Dot( b2Sub( cB, cA ), normal );+		mp->id = 0;+		manifold.pointCount = 1;+	}+	else+	{+		// Circle center is between v1 and v2. Center may be inside polygon+		b2Vec2 normal = normals[normalIndex];+		manifold.normal = normal;++		// cA is the projection of the circle center onto to the reference edge+		b2Vec2 cA = b2MulAdd( center, radiusA - b2Dot( b2Sub( center, v1 ), normal ), normal );++		// cB is the deepest point on the circle with respect to the reference edge+		b2Vec2 cB = b2MulSub( center, radiusB, normal );++		// The contact point is the midpoint+		b2LocalManifoldPoint* mp = manifold.points + 0;+		mp->point = b2Lerp( cA, cB, 0.5f );+		mp->separation = separation - radius;+		mp->id = 0;+		manifold.pointCount = 1;+	}++	return manifold;+}++// Follows Ericson 5.1.9 Closest Points of Two Line Segments+// Adds some logic to support clipping to get two contact points+b2LocalManifold b2CollideCapsules( const b2Capsule* capsuleA, const b2Capsule* capsuleB, b2Transform xf )+{+	b2Vec2 origin = capsuleA->center1;++	// Shift to the origin in frame A for round-off, a pure translation in A's frame+	b2Transform xfs = { b2Sub( xf.p, origin ), xf.q };++	b2Vec2 p1 = b2Vec2_zero;+	b2Vec2 q1 = b2Sub( capsuleA->center2, origin );++	b2Vec2 p2 = b2TransformPoint( xfs, capsuleB->center1 );+	b2Vec2 q2 = b2TransformPoint( xfs, capsuleB->center2 );++	b2Vec2 d1 = b2Sub( q1, p1 );+	b2Vec2 d2 = b2Sub( q2, p2 );++	float dd1 = b2Dot( d1, d1 );+	float dd2 = b2Dot( d2, d2 );++	const float epsSqr = FLT_EPSILON * FLT_EPSILON;+	B2_ASSERT( dd1 > epsSqr && dd2 > epsSqr );++	b2Vec2 r = b2Sub( p1, p2 );+	float rd1 = b2Dot( r, d1 );+	float rd2 = b2Dot( r, d2 );++	float d12 = b2Dot( d1, d2 );++	float denom = dd1 * dd2 - d12 * d12;++	// Fraction on segment 1+	float f1 = 0.0f;+	if ( denom != 0.0f )+	{+		// not parallel+		f1 = b2ClampFloat( ( d12 * rd2 - rd1 * dd2 ) / denom, 0.0f, 1.0f );+	}++	// Compute point on segment 2 closest to p1 + f1 * d1+	float f2 = ( d12 * f1 + rd2 ) / dd2;++	// Clamping of segment 2 requires a do over on segment 1+	if ( f2 < 0.0f )+	{+		f2 = 0.0f;+		f1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f );+	}+	else if ( f2 > 1.0f )+	{+		f2 = 1.0f;+		f1 = b2ClampFloat( ( d12 - rd1 ) / dd1, 0.0f, 1.0f );+	}++	b2Vec2 closest1 = b2MulAdd( p1, f1, d1 );+	b2Vec2 closest2 = b2MulAdd( p2, f2, d2 );+	float distanceSquared = b2DistanceSquared( closest1, closest2 );++	b2LocalManifold manifold = { 0 };+	float radiusA = capsuleA->radius;+	float radiusB = capsuleB->radius;+	float radius = radiusA + radiusB;+	float maxDistance = radius + B2_SPECULATIVE_DISTANCE;++	if ( distanceSquared > maxDistance * maxDistance )+	{+		return manifold;+	}++	float distance = sqrtf( distanceSquared );++	float length1, length2;+	b2Vec2 u1 = b2GetLengthAndNormalize( &length1, d1 );+	b2Vec2 u2 = b2GetLengthAndNormalize( &length2, d2 );++	// Does segment B project outside segment A?+	float fp2 = b2Dot( b2Sub( p2, p1 ), u1 );+	float fq2 = b2Dot( b2Sub( q2, p1 ), u1 );+	bool outsideA = ( fp2 <= 0.0f && fq2 <= 0.0f ) || ( fp2 >= length1 && fq2 >= length1 );++	// Does segment A project outside segment B?+	float fp1 = b2Dot( b2Sub( p1, p2 ), u2 );+	float fq1 = b2Dot( b2Sub( q1, p2 ), u2 );+	bool outsideB = ( fp1 <= 0.0f && fq1 <= 0.0f ) || ( fp1 >= length2 && fq1 >= length2 );++	if ( outsideA == false && outsideB == false )+	{+		// attempt to clip+		// this may yield contact points with excessive separation+		// in that case the algorithm falls back to single point collision++		// find reference edge using SAT+		b2Vec2 normalA;+		float separationA;++		{+			normalA = b2LeftPerp( u1 );+			float ss1 = b2Dot( b2Sub( p2, p1 ), normalA );+			float ss2 = b2Dot( b2Sub( q2, p1 ), normalA );+			float s1p = ss1 < ss2 ? ss1 : ss2;+			float s1n = -ss1 < -ss2 ? -ss1 : -ss2;++			if ( s1p > s1n )+			{+				separationA = s1p;+			}+			else+			{+				separationA = s1n;+				normalA = b2Neg( normalA );+			}+		}++		b2Vec2 normalB;+		float separationB;+		{+			normalB = b2LeftPerp( u2 );+			float ss1 = b2Dot( b2Sub( p1, p2 ), normalB );+			float ss2 = b2Dot( b2Sub( q1, p2 ), normalB );+			float s1p = ss1 < ss2 ? ss1 : ss2;+			float s1n = -ss1 < -ss2 ? -ss1 : -ss2;++			if ( s1p > s1n )+			{+				separationB = s1p;+			}+			else+			{+				separationB = s1n;+				normalB = b2Neg( normalB );+			}+		}++		// biased to avoid feature flip-flop+		// todo more testing?+		if ( separationA + 0.1f * B2_LINEAR_SLOP >= separationB )+		{+			manifold.normal = normalA;++			b2Vec2 cp = p2;+			b2Vec2 cq = q2;++			// clip to p1+			if ( fp2 < 0.0f && fq2 > 0.0f )+			{+				cp = b2Lerp( p2, q2, ( 0.0f - fp2 ) / ( fq2 - fp2 ) );+			}+			else if ( fq2 < 0.0f && fp2 > 0.0f )+			{+				cq = b2Lerp( q2, p2, ( 0.0f - fq2 ) / ( fp2 - fq2 ) );+			}++			// clip to q1+			if ( fp2 > length1 && fq2 < length1 )+			{+				cp = b2Lerp( p2, q2, ( fp2 - length1 ) / ( fp2 - fq2 ) );+			}+			else if ( fq2 > length1 && fp2 < length1 )+			{+				cq = b2Lerp( q2, p2, ( fq2 - length1 ) / ( fq2 - fp2 ) );+			}++			float sp = b2Dot( b2Sub( cp, p1 ), normalA );+			float sq = b2Dot( b2Sub( cq, p1 ), normalA );++			if ( sp <= distance + B2_LINEAR_SLOP || sq <= distance + B2_LINEAR_SLOP )+			{+				b2LocalManifoldPoint* mp;+				mp = manifold.points + 0;+				mp->point = b2MulAdd( cp, 0.5f * ( radiusA - radiusB - sp ), normalA );+				mp->separation = sp - radius;+				mp->id = B2_MAKE_ID( 0, 0 );++				mp = manifold.points + 1;+				mp->point = b2MulAdd( cq, 0.5f * ( radiusA - radiusB - sq ), normalA );+				mp->separation = sq - radius;+				mp->id = B2_MAKE_ID( 0, 1 );+				manifold.pointCount = 2;+			}+		}+		else+		{+			// normal always points from A to B+			manifold.normal = b2Neg( normalB );++			b2Vec2 cp = p1;+			b2Vec2 cq = q1;++			// clip to p2+			if ( fp1 < 0.0f && fq1 > 0.0f )+			{+				cp = b2Lerp( p1, q1, ( 0.0f - fp1 ) / ( fq1 - fp1 ) );+			}+			else if ( fq1 < 0.0f && fp1 > 0.0f )+			{+				cq = b2Lerp( q1, p1, ( 0.0f - fq1 ) / ( fp1 - fq1 ) );+			}++			// clip to q2+			if ( fp1 > length2 && fq1 < length2 )+			{+				cp = b2Lerp( p1, q1, ( fp1 - length2 ) / ( fp1 - fq1 ) );+			}+			else if ( fq1 > length2 && fp1 < length2 )+			{+				cq = b2Lerp( q1, p1, ( fq1 - length2 ) / ( fq1 - fp1 ) );+			}++			float sp = b2Dot( b2Sub( cp, p2 ), normalB );+			float sq = b2Dot( b2Sub( cq, p2 ), normalB );++			if ( sp <= distance + B2_LINEAR_SLOP || sq <= distance + B2_LINEAR_SLOP )+			{+				b2LocalManifoldPoint* mp;+				mp = manifold.points + 0;+				mp->point = b2MulAdd( cp, 0.5f * ( radiusB - radiusA - sp ), normalB );+				mp->separation = sp - radius;+				mp->id = B2_MAKE_ID( 0, 0 );+				mp = manifold.points + 1;+				mp->point = b2MulAdd( cq, 0.5f * ( radiusB - radiusA - sq ), normalB );+				mp->separation = sq - radius;+				mp->id = B2_MAKE_ID( 1, 0 );+				manifold.pointCount = 2;+			}+		}+	}++	if ( manifold.pointCount == 0 )+	{+		// single point collision+		b2Vec2 normal = b2Sub( closest2, closest1 );+		if ( b2Dot( normal, normal ) > epsSqr )+		{+			normal = b2Normalize( normal );+		}+		else+		{+			normal = b2LeftPerp( u1 );+		}++		b2Vec2 c1 = b2MulAdd( closest1, radiusA, normal );+		b2Vec2 c2 = b2MulAdd( closest2, -radiusB, normal );++		int i1 = f1 == 0.0f ? 0 : 1;+		int i2 = f2 == 0.0f ? 0 : 1;++		manifold.normal = normal;+		manifold.points[0].point = b2Lerp( c1, c2, 0.5f );+		manifold.points[0].separation = sqrtf( distanceSquared ) - radius;+		manifold.points[0].id = B2_MAKE_ID( i1, i2 );+		manifold.pointCount = 1;+	}++	// Undo the origin shift so points are in frame A+	for ( int i = 0; i < manifold.pointCount; ++i )+	{+		manifold.points[i].point = b2Add( manifold.points[i].point, origin );+	}++	return manifold;+}++b2LocalManifold b2CollideSegmentAndCapsule( const b2Segment* segmentA, const b2Capsule* capsuleB, b2Transform xf )+{+	b2Capsule capsuleA = { segmentA->point1, segmentA->point2, 0.0f };+	return b2CollideCapsules( &capsuleA, capsuleB, xf );+}++b2LocalManifold b2CollidePolygonAndCapsule( const b2Polygon* polygonA, const b2Capsule* capsuleB, b2Transform xf )+{+	b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius );+	return b2CollidePolygons( polygonA, &polyB, xf );+}++// Polygon clipper used to compute contact points when there are potentially two contact points.+static b2LocalManifold b2ClipPolygons( const b2Polygon* polyA, const b2Polygon* polyB, int edgeA, int edgeB, bool flip )+{+	b2LocalManifold manifold = { 0 };++	// reference polygon+	const b2Polygon* poly1;+	int i11, i12;++	// incident polygon+	const b2Polygon* poly2;+	int i21, i22;++	if ( flip )+	{+		poly1 = polyB;+		poly2 = polyA;+		i11 = edgeB;+		i12 = edgeB + 1 < polyB->count ? edgeB + 1 : 0;+		i21 = edgeA;+		i22 = edgeA + 1 < polyA->count ? edgeA + 1 : 0;+	}+	else+	{+		poly1 = polyA;+		poly2 = polyB;+		i11 = edgeA;+		i12 = edgeA + 1 < polyA->count ? edgeA + 1 : 0;+		i21 = edgeB;+		i22 = edgeB + 1 < polyB->count ? edgeB + 1 : 0;+	}++	b2Vec2 normal = poly1->normals[i11];++	// Reference edge vertices+	b2Vec2 v11 = poly1->vertices[i11];+	b2Vec2 v12 = poly1->vertices[i12];++	// Incident edge vertices+	b2Vec2 v21 = poly2->vertices[i21];+	b2Vec2 v22 = poly2->vertices[i22];++	b2Vec2 tangent = b2CrossSV( 1.0f, normal );++	float lower1 = 0.0f;+	float upper1 = b2Dot( b2Sub( v12, v11 ), tangent );++	// Incident edge points opposite of tangent due to CCW winding+	float upper2 = b2Dot( b2Sub( v21, v11 ), tangent );+	float lower2 = b2Dot( b2Sub( v22, v11 ), tangent );++	// Are the segments disjoint?+	if ( upper2 < lower1 || upper1 < lower2 )+	{+		return manifold;+	}++	b2Vec2 vLower;+	if ( lower2 < lower1 && upper2 - lower2 > FLT_EPSILON )+	{+		vLower = b2Lerp( v22, v21, ( lower1 - lower2 ) / ( upper2 - lower2 ) );+	}+	else+	{+		vLower = v22;+	}++	b2Vec2 vUpper;+	if ( upper2 > upper1 && upper2 - lower2 > FLT_EPSILON )+	{+		vUpper = b2Lerp( v22, v21, ( upper1 - lower2 ) / ( upper2 - lower2 ) );+	}+	else+	{+		vUpper = v21;+	}++	// todo vLower can be very close to vUpper, reduce to one point?++	float separationLower = b2Dot( b2Sub( vLower, v11 ), normal );+	float separationUpper = b2Dot( b2Sub( vUpper, v11 ), normal );++	float r1 = poly1->radius;+	float r2 = poly2->radius;++	// Put contact points at midpoint, accounting for radii+	vLower = b2MulAdd( vLower, 0.5f * ( r1 - r2 - separationLower ), normal );+	vUpper = b2MulAdd( vUpper, 0.5f * ( r1 - r2 - separationUpper ), normal );++	float radius = r1 + r2;++	if ( flip == false )+	{+		manifold.normal = normal;+		b2LocalManifoldPoint* cp = manifold.points + 0;++		{+			cp->point = vLower;+			cp->separation = separationLower - radius;+			cp->id = B2_MAKE_ID( i11, i22 );+			manifold.pointCount += 1;+			cp += 1;+		}++		{+			cp->point = vUpper;+			cp->separation = separationUpper - radius;+			cp->id = B2_MAKE_ID( i12, i21 );+			manifold.pointCount += 1;+		}+	}+	else+	{+		manifold.normal = b2Neg( normal );+		b2LocalManifoldPoint* cp = manifold.points + 0;++		{+			cp->point = vUpper;+			cp->separation = separationUpper - radius;+			cp->id = B2_MAKE_ID( i21, i12 );+			manifold.pointCount += 1;+			cp += 1;+		}++		{+			cp->point = vLower;+			cp->separation = separationLower - radius;+			cp->id = B2_MAKE_ID( i22, i11 );+			manifold.pointCount += 1;+		}+	}++	return manifold;+}++// Find the max separation between poly1 and poly2 using edge normals from poly1.+static float b2FindMaxSeparation( int* edgeIndex, const b2Polygon* poly1, const b2Polygon* poly2 )+{+	int count1 = poly1->count;+	int count2 = poly2->count;+	const b2Vec2* n1s = poly1->normals;+	const b2Vec2* v1s = poly1->vertices;+	const b2Vec2* v2s = poly2->vertices;++#if defined( B2_SIMD_SSE2 ) || defined( B2_SIMD_AVX2 )+	if ( count1 <= 4 )+	{+		// [n1x n1y n2x n2y]+		__m128 na = _mm_loadu_ps( &n1s[0].x );+		// [n3x n3y n4x n4y]+		__m128 nb = _mm_loadu_ps( &n1s[2].x );+		// [v1x v1y v2x v2y]+		__m128 va = _mm_loadu_ps( &v1s[0].x );+		// [v3x v3y v4x v4y]+		__m128 vb = _mm_loadu_ps( &v1s[2].x );++		// [n1x n2x n3x n4x]+		__m128 nx = _mm_shuffle_ps( na, nb, _MM_SHUFFLE( 2, 0, 2, 0 ) );+		// [n1y n2y n3y n4y]+		__m128 ny = _mm_shuffle_ps( na, nb, _MM_SHUFFLE( 3, 1, 3, 1 ) );+		// [v1x v2x v3x v4x]+		__m128 px = _mm_shuffle_ps( va, vb, _MM_SHUFFLE( 2, 0, 2, 0 ) );+		// [v1y v2y v3y v4y]+		__m128 py = _mm_shuffle_ps( va, vb, _MM_SHUFFLE( 3, 1, 3, 1 ) );++		__m128 s = _mm_set1_ps( FLT_MAX );+		for ( int j = 0; j < count2; ++j )+		{+			__m128 qx = _mm_set1_ps( v2s[j].x );+			__m128 qy = _mm_set1_ps( v2s[j].y );++			// Four separation values at once, four faces of poly1 for one vertex of poly2.+			// [dot(n1[0], qj - v1[0]) dot(n1[1], qj - v1[1]) dot(n1[2], qj - v1[2]) dot(n1[3]], qj - v1[3])]+			__m128 sj = _mm_add_ps( _mm_mul_ps( nx, _mm_sub_ps( qx, px ) ), _mm_mul_ps( ny, _mm_sub_ps( qy, py ) ) );++			// [min(s1,sj1) min(s2,sj2) min(s3,sj3) min(s4,sj4)]+			s = _mm_min_ps( s, sj );+		}++		// [(0 < count1) (1 < count1) (2 < count1) (3 < count1)]+		__m128 valid = _mm_castsi128_ps( _mm_cmplt_epi32( _mm_setr_epi32( 0, 1, 2, 3 ), _mm_set1_epi32( count1 ) ) );++		// Invalid lanes become -FLT_MAX+		s = _mm_or_ps( _mm_and_ps( valid, s ), _mm_andnot_ps( valid, _mm_set1_ps( -FLT_MAX ) ) );++		// [max(s1,s3) max(s2,s4) max(s3,s1) max(s4,s2)]+		__m128 m = _mm_max_ps( s, _mm_shuffle_ps( s, s, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );++		// Maximum separation splatted+		m = _mm_max_ps( m, _mm_shuffle_ps( m, m, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );++		// Set the bit in the lane holding the maximum. Can be multiple lanes in a tie.+		int bits = _mm_movemask_ps( _mm_cmpeq_ps( s, m ) );++		// Get the lane index of the maximum. Lowest index wins a tie.+		int index = 0;+		while ( ( bits & 1 ) == 0 )+		{+			bits >>= 1;+			index += 1;+		}++		*edgeIndex = index;+		return _mm_cvtss_f32( m );+	}+#endif++	int bestIndex = 0;+	float maxSeparation = -FLT_MAX;+	for ( int i = 0; i < count1; ++i )+	{+		// Get poly1 normal in frame2.+		b2Vec2 n = n1s[i];+		b2Vec2 v1 = v1s[i];++		// Find the deepest point for normal i.+		float si = FLT_MAX;+		for ( int j = 0; j < count2; ++j )+		{+			float sij = b2Dot( n, b2Sub( v2s[j], v1 ) );+			if ( sij < si )+			{+				si = sij;+			}+		}++		if ( si > maxSeparation )+		{+			maxSeparation = si;+			bestIndex = i;+		}+	}++	*edgeIndex = bestIndex;+	return maxSeparation;+}++// Due to speculation, every polygon is rounded+// Algorithm:+//+// compute edge separation using the separating axis test (SAT)+// if (separation > speculation_distance)+//   return+// find reference and incident edge+// if separation >= 0.1f * B2_LINEAR_SLOP+//   compute closest points between reference and incident edge+//   if vertices are closest+//      single vertex-vertex contact+//   else+//      clip edges+//   end+// else+//   clip edges+// end++b2LocalManifold b2CollidePolygons( const b2Polygon* polygonA, const b2Polygon* polygonB, b2Transform xf )+{+	b2Vec2 origin = polygonA->vertices[0];+	float linearSlop = B2_LINEAR_SLOP;+	float speculativeDistance = B2_SPECULATIVE_DISTANCE;++	// Shift to the origin in frame A for round-off, a pure translation in A's frame+	b2Transform xfs = { b2Sub( xf.p, origin ), xf.q };++	b2Polygon localPolyA;+	localPolyA.count = polygonA->count;+	localPolyA.radius = polygonA->radius;+	localPolyA.vertices[0] = b2Vec2_zero;+	localPolyA.normals[0] = polygonA->normals[0];+	for ( int i = 1; i < localPolyA.count; ++i )+	{+		localPolyA.vertices[i] = b2Sub( polygonA->vertices[i], origin );+		localPolyA.normals[i] = polygonA->normals[i];+	}++	// Put polyB in polyA's frame to reduce round-off error+	b2Polygon localPolyB;+	localPolyB.count = polygonB->count;+	localPolyB.radius = polygonB->radius;+	for ( int i = 0; i < localPolyB.count; ++i )+	{+		localPolyB.vertices[i] = b2TransformPoint( xfs, polygonB->vertices[i] );+		localPolyB.normals[i] = b2RotateVector( xfs.q, polygonB->normals[i] );+	}++	float radius = localPolyA.radius + localPolyB.radius;++	int edgeA = 0;+	float separationA = b2FindMaxSeparation( &edgeA, &localPolyA, &localPolyB );+	if ( separationA > speculativeDistance + radius )+	{+		return (b2LocalManifold){ 0 };+	}++	int edgeB = 0;+	float separationB = b2FindMaxSeparation( &edgeB, &localPolyB, &localPolyA );+	if ( separationB > speculativeDistance + radius )+	{+		return (b2LocalManifold){ 0 };+	}++	// Find incident edge+	bool flip;+	if ( separationA >= separationB )+	{+		flip = false;++		b2Vec2 searchDirection = localPolyA.normals[edgeA];++		// Find the incident edge on polyB+		int count = localPolyB.count;+		const b2Vec2* normals = localPolyB.normals;+		edgeB = 0;+		float minDot = FLT_MAX;+		for ( int i = 0; i < count; ++i )+		{+			float dot = b2Dot( searchDirection, normals[i] );+			if ( dot < minDot )+			{+				minDot = dot;+				edgeB = i;+			}+		}+	}+	else+	{+		flip = true;++		b2Vec2 searchDirection = localPolyB.normals[edgeB];++		// Find the incident edge on polyA+		int count = localPolyA.count;+		const b2Vec2* normals = localPolyA.normals;+		edgeA = 0;+		float minDot = FLT_MAX;+		for ( int i = 0; i < count; ++i )+		{+			float dot = b2Dot( searchDirection, normals[i] );+			if ( dot < minDot )+			{+				minDot = dot;+				edgeA = i;+			}+		}+	}++	b2LocalManifold manifold = { 0 };++	// Using slop here to ensure vertex-vertex normal vectors can be safely normalized+	// todo this means edge clipping needs to handle slightly non-overlapping edges.+	if ( separationA > 0.1f * linearSlop || separationB > 0.1f * linearSlop )+	{+#if 1+		// Edges are disjoint. Find closest points between reference edge and incident edge+		// Reference edge on polygon A+		int i11 = edgeA;+		int i12 = edgeA + 1 < localPolyA.count ? edgeA + 1 : 0;+		int i21 = edgeB;+		int i22 = edgeB + 1 < localPolyB.count ? edgeB + 1 : 0;++		b2Vec2 v11 = localPolyA.vertices[i11];+		b2Vec2 v12 = localPolyA.vertices[i12];+		b2Vec2 v21 = localPolyB.vertices[i21];+		b2Vec2 v22 = localPolyB.vertices[i22];++		b2SegmentDistanceResult result = b2ComputeSegmentDistance( v11, v12, v21, v22 );+		B2_ASSERT( result.distanceSquared > 0.0f );+		float distance = sqrtf( result.distanceSquared );+		float separation = distance - radius;++		if ( distance - radius > speculativeDistance )+		{+			// This can happen in the vertex-vertex case+			return manifold;+		}++		// Attempt to clip edges+		manifold = b2ClipPolygons( &localPolyA, &localPolyB, edgeA, edgeB, flip );++		float minSeparation = FLT_MAX;+		for ( int i = 0; i < manifold.pointCount; ++i )+		{+			minSeparation = b2MinFloat( minSeparation, manifold.points[i].separation );+		}++		// Does vertex-vertex have substantially larger separation?+		if ( separation + 0.1f * linearSlop < minSeparation )+		{+			if ( result.fraction1 == 0.0f && result.fraction2 == 0.0f )+			{+				// v11 - v21+				b2Vec2 normal = b2Sub( v21, v11 );+				float invDistance = 1.0f / distance;+				normal.x *= invDistance;+				normal.y *= invDistance;++				b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal );+				b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal );++				manifold.normal = normal;+				manifold.points[0].point = b2Lerp( c1, c2, 0.5f );+				manifold.points[0].separation = distance - radius;+				manifold.points[0].id = B2_MAKE_ID( i11, i21 );+				manifold.pointCount = 1;+			}+			else if ( result.fraction1 == 0.0f && result.fraction2 == 1.0f )+			{+				// v11 - v22+				b2Vec2 normal = b2Sub( v22, v11 );+				float invDistance = 1.0f / distance;+				normal.x *= invDistance;+				normal.y *= invDistance;++				b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal );+				b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal );++				manifold.normal = normal;+				manifold.points[0].point = b2Lerp( c1, c2, 0.5f );+				manifold.points[0].separation = distance - radius;+				manifold.points[0].id = B2_MAKE_ID( i11, i22 );+				manifold.pointCount = 1;+			}+			else if ( result.fraction1 == 1.0f && result.fraction2 == 0.0f )+			{+				// v12 - v21+				b2Vec2 normal = b2Sub( v21, v12 );+				float invDistance = 1.0f / distance;+				normal.x *= invDistance;+				normal.y *= invDistance;++				b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal );+				b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal );++				manifold.normal = normal;+				manifold.points[0].point = b2Lerp( c1, c2, 0.5f );+				manifold.points[0].separation = distance - radius;+				manifold.points[0].id = B2_MAKE_ID( i12, i21 );+				manifold.pointCount = 1;+			}+			else if ( result.fraction1 == 1.0f && result.fraction2 == 1.0f )+			{+				// v12 - v22+				b2Vec2 normal = b2Sub( v22, v12 );+				float invDistance = 1.0f / distance;+				normal.x *= invDistance;+				normal.y *= invDistance;++				b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal );+				b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal );++				manifold.normal = normal;+				manifold.points[0].point = b2Lerp( c1, c2, 0.5f );+				manifold.points[0].separation = distance - radius;+				manifold.points[0].id = B2_MAKE_ID( i12, i22 );+				manifold.pointCount = 1;+			}+		}+#else+		// Polygons are disjoint. Find closest points between reference edge and incident edge+		// Reference edge on polygon A+		int i11 = edgeA;+		int i12 = edgeA + 1 < localPolyA.count ? edgeA + 1 : 0;+		int i21 = edgeB;+		int i22 = edgeB + 1 < localPolyB.count ? edgeB + 1 : 0;++		b2Vec2 v11 = localPolyA.vertices[i11];+		b2Vec2 v12 = localPolyA.vertices[i12];+		b2Vec2 v21 = localPolyB.vertices[i21];+		b2Vec2 v22 = localPolyB.vertices[i22];++		b2SegmentDistanceResult result = b2ComputeSegmentDistance( v11, v12, v21, v22 );++		if ( result.fraction1 == 0.0f && result.fraction2 == 0.0f )+		{+			// v11 - v21+			b2Vec2 normal = b2Sub( v21, v11 );+			B2_ASSERT( result.distanceSquared > 0.0f );+			float distance = sqrtf( result.distanceSquared );+			if ( distance > B2_SPECULATIVE_DISTANCE + radius )+			{+				return manifold;+			}+			float invDistance = 1.0f / distance;+			normal.x *= invDistance;+			normal.y *= invDistance;++			b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal );+			b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal );++			manifold.normal = normal;+			manifold.points[0].point = b2Lerp( c1, c2, 0.5f );+			manifold.points[0].separation = distance - radius;+			manifold.points[0].id = B2_MAKE_ID( i11, i21 );+			manifold.pointCount = 1;+		}+		else if ( result.fraction1 == 0.0f && result.fraction2 == 1.0f )+		{+			// v11 - v22+			b2Vec2 normal = b2Sub( v22, v11 );+			B2_ASSERT( result.distanceSquared > 0.0f );+			float distance = sqrtf( result.distanceSquared );+			if ( distance > B2_SPECULATIVE_DISTANCE + radius )+			{+				return manifold;+			}+			float invDistance = 1.0f / distance;+			normal.x *= invDistance;+			normal.y *= invDistance;++			b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal );+			b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal );++			manifold.normal = normal;+			manifold.points[0].point = b2Lerp( c1, c2, 0.5f );+			manifold.points[0].separation = distance - radius;+			manifold.points[0].id = B2_MAKE_ID( i11, i22 );+			manifold.pointCount = 1;+		}+		else if ( result.fraction1 == 1.0f && result.fraction2 == 0.0f )+		{+			// v12 - v21+			b2Vec2 normal = b2Sub( v21, v12 );+			B2_ASSERT( result.distanceSquared > 0.0f );+			float distance = sqrtf( result.distanceSquared );+			if ( distance > B2_SPECULATIVE_DISTANCE + radius )+			{+				return manifold;+			}+			float invDistance = 1.0f / distance;+			normal.x *= invDistance;+			normal.y *= invDistance;++			b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal );+			b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal );++			manifold.normal = normal;+			manifold.points[0].point = b2Lerp( c1, c2, 0.5f );+			manifold.points[0].separation = distance - radius;+			manifold.points[0].id = B2_MAKE_ID( i12, i21 );+			manifold.pointCount = 1;+		}+		else if ( result.fraction1 == 1.0f && result.fraction2 == 1.0f )+		{+			// v12 - v22+			b2Vec2 normal = b2Sub( v22, v12 );+			B2_ASSERT( result.distanceSquared > 0.0f );+			float distance = sqrtf( result.distanceSquared );+			if ( distance > B2_SPECULATIVE_DISTANCE + radius )+			{+				return manifold;+			}+			float invDistance = 1.0f / distance;+			normal.x *= invDistance;+			normal.y *= invDistance;++			b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal );+			b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal );++			manifold.normal = normal;+			manifold.points[0].point = b2Lerp( c1, c2, 0.5f );+			manifold.points[0].separation = distance - radius;+			manifold.points[0].id = B2_MAKE_ID( i12, i22 );+			manifold.pointCount = 1;+		}+		else+		{+			// Edge region+			manifold = b2ClipPolygons( &localPolyA, &localPolyB, edgeA, edgeB, flip );+		}+#endif+	}+	else+	{+		// Polygons overlap+		manifold = b2ClipPolygons( &localPolyA, &localPolyB, edgeA, edgeB, flip );+	}++	// Undo the origin shift so points are in frame A+	for ( int i = 0; i < manifold.pointCount; ++i )+	{+		manifold.points[i].point = b2Add( manifold.points[i].point, origin );+	}++	return manifold;+}++b2LocalManifold b2CollideSegmentAndCircle( const b2Segment* segmentA, const b2Circle* circleB, b2Transform xf )+{+	b2Capsule capsuleA = { segmentA->point1, segmentA->point2, 0.0f };+	return b2CollideCapsuleAndCircle( &capsuleA, circleB, xf );+}++b2LocalManifold b2CollideSegmentAndPolygon( const b2Segment* segmentA, const b2Polygon* polygonB, b2Transform xf )+{+	b2Polygon polygonA = b2MakeCapsule( segmentA->point1, segmentA->point2, 0.0f );+	return b2CollidePolygons( &polygonA, polygonB, xf );+}++b2LocalManifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, const b2Circle* circleB, b2Transform xf )+{+	b2LocalManifold manifold = { 0 };++	// Compute circle in frame of segment+	b2Vec2 pB = b2TransformPoint( xf, circleB->center );++	b2Vec2 p1 = segmentA->segment.point1;+	b2Vec2 p2 = segmentA->segment.point2;+	b2Vec2 e = b2Sub( p2, p1 );++	// Normal points to the right+	float offset = b2Dot( b2RightPerp( e ), b2Sub( pB, p1 ) );+	if ( offset < 0.0f )+	{+		// collision is one-sided+		return manifold;+	}++	// Barycentric coordinates+	float u = b2Dot( e, b2Sub( p2, pB ) );+	float v = b2Dot( e, b2Sub( pB, p1 ) );++	b2Vec2 pA;++	if ( v <= 0.0f )+	{+		// Behind point1?+		// Is pB in the Voronoi region of the previous edge?+		b2Vec2 prevEdge = b2Sub( p1, segmentA->ghost1 );+		float uPrev = b2Dot( prevEdge, b2Sub( pB, p1 ) );+		if ( uPrev <= 0.0f )+		{+			return manifold;+		}++		pA = p1;+	}+	else if ( u <= 0.0f )+	{+		// Ahead of point2?+		b2Vec2 nextEdge = b2Sub( segmentA->ghost2, p2 );+		float vNext = b2Dot( nextEdge, b2Sub( pB, p2 ) );++		// Is pB in the Voronoi region of the next edge?+		if ( vNext > 0.0f )+		{+			return manifold;+		}++		pA = p2;+	}+	else+	{+		float ee = b2Dot( e, e );+		pA = (b2Vec2){ u * p1.x + v * p2.x, u * p1.y + v * p2.y };+		pA = ee > 0.0f ? b2MulSV( 1.0f / ee, pA ) : p1;+	}++	float distance;+	b2Vec2 normal = b2GetLengthAndNormalize( &distance, b2Sub( pB, pA ) );++	float radius = circleB->radius;+	float separation = distance - radius;+	if ( separation > B2_SPECULATIVE_DISTANCE )+	{+		return manifold;+	}++	b2Vec2 cA = pA;+	b2Vec2 cB = b2MulAdd( pB, -radius, normal );++	manifold.normal = normal;++	b2LocalManifoldPoint* mp = manifold.points + 0;+	mp->point = b2Lerp( cA, cB, 0.5f );+	mp->separation = separation;+	mp->id = 0;+	manifold.pointCount = 1;+	return manifold;+}++b2LocalManifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA, const b2Capsule* capsuleB, b2Transform xf,+												 b2SimplexCache* cache )+{+	b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius );+	return b2CollideChainSegmentAndPolygon( segmentA, &polyB, xf, cache );+}++static b2LocalManifold b2ClipSegments( b2Vec2 a1, b2Vec2 a2, b2Vec2 b1, b2Vec2 b2, b2Vec2 normal, float ra, float rb, uint16_t id1,+									   uint16_t id2 )+{+	b2LocalManifold manifold = { 0 };++	b2Vec2 tangent = b2LeftPerp( normal );++	// Barycentric coordinates of each point relative to a1 along tangent+	float lower1 = 0.0f;+	float upper1 = b2Dot( b2Sub( a2, a1 ), tangent );++	// Incident edge points opposite of tangent due to CCW winding+	float upper2 = b2Dot( b2Sub( b1, a1 ), tangent );+	float lower2 = b2Dot( b2Sub( b2, a1 ), tangent );++	// Do segments overlap?+	if ( upper2 < lower1 || upper1 < lower2 )+	{+		return manifold;+	}++	b2Vec2 vLower;+	if ( lower2 < lower1 && upper2 - lower2 > FLT_EPSILON )+	{+		vLower = b2Lerp( b2, b1, ( lower1 - lower2 ) / ( upper2 - lower2 ) );+	}+	else+	{+		vLower = b2;+	}++	b2Vec2 vUpper;+	if ( upper2 > upper1 && upper2 - lower2 > FLT_EPSILON )+	{+		vUpper = b2Lerp( b2, b1, ( upper1 - lower2 ) / ( upper2 - lower2 ) );+	}+	else+	{+		vUpper = b1;+	}++	// todo vLower can be very close to vUpper, reduce to one point?++	float separationLower = b2Dot( b2Sub( vLower, a1 ), normal );+	float separationUpper = b2Dot( b2Sub( vUpper, a1 ), normal );++	// Put contact points at midpoint, accounting for radii+	vLower = b2MulAdd( vLower, 0.5f * ( ra - rb - separationLower ), normal );+	vUpper = b2MulAdd( vUpper, 0.5f * ( ra - rb - separationUpper ), normal );++	float radius = ra + rb;++	manifold.normal = normal;+	{+		b2LocalManifoldPoint* cp = manifold.points + 0;+		cp->point = vLower;+		cp->separation = separationLower - radius;+		cp->id = id1;+	}++	{+		b2LocalManifoldPoint* cp = manifold.points + 1;+		cp->point = vUpper;+		cp->separation = separationUpper - radius;+		cp->id = id2;+	}++	manifold.pointCount = 2;++	return manifold;+}++enum b2NormalType+{+	// This means the normal points in a direction that is non-smooth relative to a convex vertex and should be skipped+	b2_normalSkip,++	// This means the normal points in a direction that is smooth relative to a convex vertex and should be used for collision+	b2_normalAdmit,++	// This means the normal is in a region of a concave vertex and should be snapped to the segment normal+	b2_normalSnap+};++struct b2ChainSegmentParams+{+	b2Vec2 edge1;+	b2Vec2 normal0;+	b2Vec2 normal2;+	bool convex1;+	bool convex2;+};++// Evaluate Gauss map+// See https://box2d.org/posts/2020/06/ghost-collisions/+static enum b2NormalType b2ClassifyNormal( struct b2ChainSegmentParams params, b2Vec2 normal )+{+	const float sinTol = 0.01f;++	if ( b2Dot( normal, params.edge1 ) <= 0.0f )+	{+		// Normal points towards the segment tail+		if ( params.convex1 )+		{+			if ( b2Cross( normal, params.normal0 ) > sinTol )+			{+				return b2_normalSkip;+			}++			return b2_normalAdmit;+		}+		else+		{+			return b2_normalSnap;+		}+	}+	else+	{+		// Normal points towards segment head+		if ( params.convex2 )+		{+			if ( b2Cross( params.normal2, normal ) > sinTol )+			{+				return b2_normalSkip;+			}++			return b2_normalAdmit;+		}+		else+		{+			return b2_normalSnap;+		}+	}+}++b2LocalManifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA, const b2Polygon* polygonB, b2Transform xf,+												 b2SimplexCache* cache )+{+	b2LocalManifold manifold = { 0 };++	b2Vec2 centroidB = b2TransformPoint( xf, polygonB->centroid );+	float radiusB = polygonB->radius;++	b2Vec2 p1 = segmentA->segment.point1;+	b2Vec2 p2 = segmentA->segment.point2;++	b2Vec2 edge1 = b2Normalize( b2Sub( p2, p1 ) );++	struct b2ChainSegmentParams smoothParams = { 0 };+	smoothParams.edge1 = edge1;++	const float convexTol = 0.01f;+	b2Vec2 edge0 = b2Normalize( b2Sub( p1, segmentA->ghost1 ) );+	smoothParams.normal0 = b2RightPerp( edge0 );+	smoothParams.convex1 = b2Cross( edge0, edge1 ) >= convexTol;++	b2Vec2 edge2 = b2Normalize( b2Sub( segmentA->ghost2, p2 ) );+	smoothParams.normal2 = b2RightPerp( edge2 );+	smoothParams.convex2 = b2Cross( edge1, edge2 ) >= convexTol;++	// Normal points to the right+	b2Vec2 normal1 = b2RightPerp( edge1 );+	bool behind1 = b2Dot( normal1, b2Sub( centroidB, p1 ) ) < 0.0f;+	bool behind0 = true;+	bool behind2 = true;+	if ( smoothParams.convex1 )+	{+		behind0 = b2Dot( smoothParams.normal0, b2Sub( centroidB, p1 ) ) < 0.0f;+	}++	if ( smoothParams.convex2 )+	{+		behind2 = b2Dot( smoothParams.normal2, b2Sub( centroidB, p2 ) ) < 0.0f;+	}++	if ( behind1 && behind0 && behind2 )+	{+		// one-sided collision+		return manifold;+	}++	// Get polygonB in frameA+	int count = polygonB->count;+	b2Vec2 vertices[B2_MAX_POLYGON_VERTICES];+	b2Vec2 normals[B2_MAX_POLYGON_VERTICES];+	for ( int i = 0; i < count; ++i )+	{+		vertices[i] = b2TransformPoint( xf, polygonB->vertices[i] );+		normals[i] = b2RotateVector( xf.q, polygonB->normals[i] );+	}++	// Distance doesn't work correctly with partial polygons+	b2DistanceInput input;+	input.proxyA = b2MakeProxy( &segmentA->segment.point1, 2, 0.0f );+	input.proxyB = b2MakeProxy( vertices, count, 0.0f );+	input.transform = b2Transform_identity;+	input.useRadii = false;++	b2DistanceOutput output = b2ComputeShapeDistance( &input, cache, NULL, 0 );++	if ( output.distance > radiusB + B2_SPECULATIVE_DISTANCE )+	{+		return manifold;+	}++	// Snap concave normals for partial polygon+	b2Vec2 n0 = smoothParams.convex1 ? smoothParams.normal0 : normal1;+	b2Vec2 n2 = smoothParams.convex2 ? smoothParams.normal2 : normal1;++	// Index of incident vertex on polygon+	int incidentIndex = -1;+	int incidentNormal = -1;++	if ( behind1 == false && output.distance > 0.1f * B2_LINEAR_SLOP )+	{+		// The closest features may be two vertices or an edge and a vertex even when there should+		// be face contact++		if ( cache->count == 1 )+		{+			// vertex-vertex collision+			b2Vec2 pA = output.pointA;+			b2Vec2 pB = output.pointB;++			b2Vec2 normal = b2Normalize( b2Sub( pB, pA ) );++			enum b2NormalType type = b2ClassifyNormal( smoothParams, normal );+			if ( type == b2_normalSkip )+			{+				return manifold;+			}++			if ( type == b2_normalAdmit )+			{+				manifold.normal = normal;+				b2LocalManifoldPoint* cp = manifold.points + 0;+				cp->point = pA;+				cp->separation = output.distance - radiusB;+				cp->id = B2_MAKE_ID( cache->indexA[0], cache->indexB[0] );+				manifold.pointCount = 1;+				return manifold;+			}++			// fall through b2_normalSnap+			incidentIndex = cache->indexB[0];+		}+		else+		{+			// vertex-edge collision+			B2_ASSERT( cache->count == 2 );++			int ia1 = cache->indexA[0];+			int ia2 = cache->indexA[1];+			int ib1 = cache->indexB[0];+			int ib2 = cache->indexB[1];++			if ( ia1 == ia2 )+			{+				// 1 point on A, expect 2 points on B+				B2_ASSERT( ib1 != ib2 );++				// Find polygon normal most aligned with vector between closest points.+				// This effectively sorts ib1 and ib2+				b2Vec2 normalB = b2Sub( output.pointA, output.pointB );+				float dot1 = b2Dot( normalB, normals[ib1] );+				float dot2 = b2Dot( normalB, normals[ib2] );+				int ib = dot1 > dot2 ? ib1 : ib2;++				// Use accurate normal+				normalB = normals[ib];++				enum b2NormalType type = b2ClassifyNormal( smoothParams, b2Neg( normalB ) );+				if ( type == b2_normalSkip )+				{+					return manifold;+				}++				if ( type == b2_normalAdmit )+				{+					// Get polygon edge associated with normal+					ib1 = ib;+					ib2 = ib < count - 1 ? ib + 1 : 0;++					b2Vec2 b1 = vertices[ib1];+					b2Vec2 b2 = vertices[ib2];++					// Find incident segment vertex+					dot1 = b2Dot( normalB, b2Sub( p1, b1 ) );+					dot2 = b2Dot( normalB, b2Sub( p2, b1 ) );++					if ( dot1 < dot2 )+					{+						if ( b2Dot( n0, normalB ) < b2Dot( normal1, normalB ) )+						{+							// Neighbor is incident+							return manifold;+						}+					}+					else+					{+						if ( b2Dot( n2, normalB ) < b2Dot( normal1, normalB ) )+						{+							// Neighbor is incident+							return manifold;+						}+					}++					manifold =+						b2ClipSegments( b1, b2, p1, p2, normalB, radiusB, 0.0f, B2_MAKE_ID( ib1, 1 ), B2_MAKE_ID( ib2, 0 ) );++					B2_ASSERT( manifold.pointCount == 0 || manifold.pointCount == 2 );+					if ( manifold.pointCount == 2 )+					{+						manifold.normal = b2Neg( normalB );+					}+					return manifold;+				}++				// fall through b2_normalSnap+				incidentNormal = ib;+			}+			else+			{+				// Get index of incident polygonB vertex+				float dot1 = b2Dot( normal1, b2Sub( vertices[ib1], p1 ) );+				float dot2 = b2Dot( normal1, b2Sub( vertices[ib2], p2 ) );+				incidentIndex = dot1 < dot2 ? ib1 : ib2;+			}+		}+	}+	else+	{+		// SAT edge normal+		float edgeSeparation = FLT_MAX;++		for ( int i = 0; i < count; ++i )+		{+			float s = b2Dot( normal1, b2Sub( vertices[i], p1 ) );+			if ( s < edgeSeparation )+			{+				edgeSeparation = s;+				incidentIndex = i;+			}+		}++		// Check convex neighbor for edge separation+		if ( smoothParams.convex1 )+		{+			float s0 = FLT_MAX;++			for ( int i = 0; i < count; ++i )+			{+				float s = b2Dot( smoothParams.normal0, b2Sub( vertices[i], p1 ) );+				if ( s < s0 )+				{+					s0 = s;+				}+			}++			if ( s0 > edgeSeparation )+			{+				edgeSeparation = s0;++				// Indicate neighbor owns edge separation+				incidentIndex = -1;+			}+		}++		// Check convex neighbor for edge separation+		if ( smoothParams.convex2 )+		{+			float s2 = FLT_MAX;++			for ( int i = 0; i < count; ++i )+			{+				float s = b2Dot( smoothParams.normal2, b2Sub( vertices[i], p2 ) );+				if ( s < s2 )+				{+					s2 = s;+				}+			}++			if ( s2 > edgeSeparation )+			{+				edgeSeparation = s2;++				// Indicate neighbor owns edge separation+				incidentIndex = -1;+			}+		}++		// SAT polygon normals+		float polygonSeparation = -FLT_MAX;+		int referenceIndex = -1;++		for ( int i = 0; i < count; ++i )+		{+			b2Vec2 n = normals[i];++			enum b2NormalType type = b2ClassifyNormal( smoothParams, b2Neg( n ) );+			if ( type != b2_normalAdmit )+			{+				continue;+			}++			// Check the infinite sides of the partial polygon+			// if ((smoothParams.convex1 && b2Cross(n0, n) > 0.0f) || (smoothParams.convex2 && b2Cross(n, n2) > 0.0f))+			//{+			//	continue;+			//}++			b2Vec2 p = vertices[i];+			float s = b2MinFloat( b2Dot( n, b2Sub( p2, p ) ), b2Dot( n, b2Sub( p1, p ) ) );++			if ( s > polygonSeparation )+			{+				polygonSeparation = s;+				referenceIndex = i;+			}+		}++		if ( polygonSeparation > edgeSeparation )+		{+			int ia1 = referenceIndex;+			int ia2 = ia1 < count - 1 ? ia1 + 1 : 0;+			b2Vec2 a1 = vertices[ia1];+			b2Vec2 a2 = vertices[ia2];++			b2Vec2 n = normals[ia1];++			float dot1 = b2Dot( n, b2Sub( p1, a1 ) );+			float dot2 = b2Dot( n, b2Sub( p2, a1 ) );++			if ( dot1 < dot2 )+			{+				if ( b2Dot( n0, n ) < b2Dot( normal1, n ) )+				{+					// Neighbor is incident+					return manifold;+				}+			}+			else+			{+				if ( b2Dot( n2, n ) < b2Dot( normal1, n ) )+				{+					// Neighbor is incident+					return manifold;+				}+			}++			manifold = b2ClipSegments( a1, a2, p1, p2, normals[ia1], radiusB, 0.0f, B2_MAKE_ID( ia1, 1 ), B2_MAKE_ID( ia2, 0 ) );++			B2_ASSERT( manifold.pointCount == 0 || manifold.pointCount == 2 );+			if ( manifold.pointCount == 2 )+			{+				manifold.normal = b2Neg( normals[ia1] );+			}++			return manifold;+		}++		if ( incidentIndex == -1 )+		{+			// neighboring segment is the separating axis+			return manifold;+		}++		// fall through segment normal axis+	}++	B2_ASSERT( incidentNormal != -1 || incidentIndex != -1 );++	// Segment normal++	// Find incident polygon normal: normal adjacent to deepest vertex that is most anti-parallel to segment normal+	b2Vec2 b1, b2;+	int ib1, ib2;++	if ( incidentNormal != -1 )+	{+		ib1 = incidentNormal;+		ib2 = ib1 < count - 1 ? ib1 + 1 : 0;+		b1 = vertices[ib1];+		b2 = vertices[ib2];+	}+	else+	{+		int i2 = incidentIndex;+		int i1 = i2 > 0 ? i2 - 1 : count - 1;+		float d1 = b2Dot( normal1, normals[i1] );+		float d2 = b2Dot( normal1, normals[i2] );+		if ( d1 < d2 )+		{+			ib1 = i1, ib2 = i2;+			b1 = vertices[ib1];+			b2 = vertices[ib2];+		}+		else+		{+			ib1 = i2, ib2 = i2 < count - 1 ? i2 + 1 : 0;+			b1 = vertices[ib1];+			b2 = vertices[ib2];+		}+	}++	manifold = b2ClipSegments( p1, p2, b1, b2, normal1, 0.0f, radiusB, B2_MAKE_ID( 0, ib2 ), B2_MAKE_ID( 1, ib1 ) );++	B2_ASSERT( manifold.pointCount == 0 || manifold.pointCount == 2 );++	return manifold;+}
+ upstream/src/math_functions.c view
@@ -0,0 +1,194 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "box2d/math_functions.h"++#include <float.h>++_Static_assert( sizeof( int32_t ) == sizeof( int ), "Box2D expects int32_t and int to be the same" );++bool b2IsValidFloat( float a )+{+	if ( isnan( a ) )+	{+		return false;+	}++	if ( isinf( a ) )+	{+		return false;+	}++	return true;+}++bool b2IsValidVec2( b2Vec2 v )+{+	if ( isnan( v.x ) || isnan( v.y ) )+	{+		return false;+	}++	if ( isinf( v.x ) || isinf( v.y ) )+	{+		return false;+	}++	return true;+}++bool b2IsValidRot( b2Rot q )+{+	if ( isnan( q.s ) || isnan( q.c ) )+	{+		return false;+	}++	if ( isinf( q.s ) || isinf( q.c ) )+	{+		return false;+	}++	return b2IsNormalizedRot( q );+}++bool b2IsValidTransform(b2Transform t)+{+	if (b2IsValidVec2(t.p) == false)+	{+		return false;+	}++	return b2IsValidRot( t.q );+}++bool b2IsValidPlane( b2Plane a )+{+	return b2IsValidVec2( a.normal ) && b2IsNormalized( a.normal ) && b2IsValidFloat( a.offset );+}++bool b2IsValidPos( b2Pos p )+{+	if ( isnan( p.x ) || isnan( p.y ) )+	{+		return false;+	}++	if ( isinf( p.x ) || isinf( p.y ) )+	{+		return false;+	}++	return true;+}++bool b2IsValidWorldTransform( b2WorldTransform t )+{+	if ( b2IsValidPos( t.p ) == false )+	{+		return false;+	}++	return b2IsValidRot( t.q );+}++// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4+float b2Atan2( 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 = b2AbsFloat( x );+	float ay = b2AbsFloat( y );+	float mx = b2MaxFloat( ay, ax );+	float mn = b2MinFloat( 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+b2CosSin b2ComputeCosSin( float radians )+{+	float x = b2UnwindAngle( radians );+	float pi2 = B2_PI * B2_PI;++	// cosine needs angle in [-pi/2, pi/2]+	float c;+	if ( x < -0.5f * B2_PI )+	{+		float y = x + B2_PI;+		float y2 = y * y;+		c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 );+	}+	else if ( x > 0.5f * B2_PI )+	{+		float y = x - B2_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 + B2_PI;+		s = -16.0f * y * ( B2_PI - y ) / ( 5.0f * pi2 - 4.0f * y * ( B2_PI - y ) );+	}+	else+	{+		s = 16.0f * x * ( B2_PI - x ) / ( 5.0f * pi2 - 4.0f * x * ( B2_PI - x ) );+	}++	float mag = sqrtf( s * s + c * c );+	float invMag = mag > 0.0f ? 1.0f / mag : 0.0f;+	b2CosSin cs = { c * invMag, s * invMag };+	return cs;+}++b2Rot b2ComputeRotationBetweenUnitVectors(b2Vec2 v1, b2Vec2 v2)+{+	B2_ASSERT( b2AbsFloat( 1.0f - b2Length( v1 ) ) < 100.0f * FLT_EPSILON );+	B2_ASSERT( b2AbsFloat( 1.0f - b2Length( v2 ) ) < 100.0f * FLT_EPSILON );++	b2Rot rot;+	rot.c = b2Dot( v1, v2 );+	rot.s = b2Cross( v1, v2 );+	return b2NormalizeRot( rot );+}
+ upstream/src/motor_joint.c view
@@ -0,0 +1,456 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "body.h"+#include "joint.h"+#include "physics_world.h"+#include "recording.h"+#include "solver.h"+#include "solver_set.h"++// needed for dll export+#include "box2d/box2d.h"++void b2MotorJoint_SetLinearVelocity( b2JointId jointId, b2Vec2 velocity )+{+	B2_CHECK_INPUT( b2IsValidVec2( velocity ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetLinearVelocity, jointId, velocity );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.linearVelocity = velocity;+}++b2Vec2 b2MotorJoint_GetLinearVelocity( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.linearVelocity;+}++void b2MotorJoint_SetAngularVelocity( b2JointId jointId, float velocity )+{+	B2_CHECK_INPUT( b2IsValidFloat( velocity ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetAngularVelocity, jointId, velocity );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.angularVelocity = velocity;+}++float b2MotorJoint_GetAngularVelocity( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.angularVelocity;+}++void b2MotorJoint_SetMaxVelocityTorque( b2JointId jointId, float maxTorque )+{+	B2_CHECK_INPUT( b2IsValidFloat( maxTorque ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetMaxVelocityTorque, jointId, maxTorque );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.maxVelocityTorque = maxTorque;+}++float b2MotorJoint_GetMaxVelocityTorque( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.maxVelocityTorque;+}++void b2MotorJoint_SetMaxVelocityForce( b2JointId jointId, float maxForce )+{+	B2_CHECK_INPUT( b2IsValidFloat( maxForce ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetMaxVelocityForce, jointId, maxForce );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.maxVelocityForce = maxForce;+}++float b2MotorJoint_GetMaxVelocityForce( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.maxVelocityForce;+}++void b2MotorJoint_SetLinearHertz( b2JointId jointId, float hertz )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetLinearHertz, jointId, hertz );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.linearHertz = hertz;+}++float b2MotorJoint_GetLinearHertz( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.linearHertz;+}++void b2MotorJoint_SetLinearDampingRatio( b2JointId jointId, float damping )+{+	B2_CHECK_INPUT( b2IsValidFloat( damping ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetLinearDampingRatio, jointId, damping );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.linearDampingRatio = damping;+}++float b2MotorJoint_GetLinearDampingRatio( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.linearDampingRatio;+}++void b2MotorJoint_SetAngularHertz( b2JointId jointId, float hertz )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetAngularHertz, jointId, hertz );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.angularHertz = hertz;+}++float b2MotorJoint_GetAngularHertz( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.angularHertz;+}++void b2MotorJoint_SetAngularDampingRatio( b2JointId jointId, float damping )+{+	B2_CHECK_INPUT( b2IsValidFloat( damping ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetAngularDampingRatio, jointId, damping );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.angularDampingRatio = damping;+}++float b2MotorJoint_GetAngularDampingRatio( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.angularDampingRatio;+}++void b2MotorJoint_SetMaxSpringForce( b2JointId jointId, float maxForce )+{+	B2_CHECK_INPUT( b2IsValidFloat( maxForce ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetMaxSpringForce, jointId, maxForce );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.maxSpringForce = b2MaxFloat( 0.0f, maxForce );+}++float b2MotorJoint_GetMaxSpringForce( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.maxSpringForce;+}++void b2MotorJoint_SetMaxSpringTorque( b2JointId jointId, float maxTorque )+{+	B2_CHECK_INPUT( b2IsValidFloat( maxTorque ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MotorJointSetMaxSpringTorque, jointId, maxTorque );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	joint->motorJoint.maxSpringTorque = b2MaxFloat( 0.0f, maxTorque );+}++float b2MotorJoint_GetMaxSpringTorque( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );+	return joint->motorJoint.maxSpringTorque;+}++b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base )+{+	b2Vec2 force = b2MulSV( world->inv_h, b2Add( base->motorJoint.linearVelocityImpulse, base->motorJoint.linearSpringImpulse ) );+	return force;+}++float b2GetMotorJointTorque( b2World* world, b2JointSim* base )+{+	return world->inv_h * ( 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 b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_motorJoint );++	// chase body id to the solver set where the body lives+	int idA = base->bodyIdA;+	int idB = base->bodyIdB;++	b2World* world = context->world;++	b2Body* bodyA = b2Array_Get( world->bodies, idA );+	b2Body* bodyB = b2Array_Get( world->bodies, idB );++	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );++	b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex );+	b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex );++	int localIndexA = bodyA->localIndex;+	int localIndexB = bodyB->localIndex;++	b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA );+	b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB );++	float mA = bodySimA->invMass;+	float iA = bodySimA->invInertia;+	float mB = bodySimB->invMass;+	float iB = bodySimB->invInertia;++	base->invMassA = mA;+	base->invMassB = mB;+	base->invIA = iA;+	base->invIB = iB;++	b2MotorJoint* joint = &base->motorJoint;+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;++	// Compute joint anchor frames with world space rotation, relative to center of mass+	joint->frameA.q = b2MulRot( bodySimA->transform.q, base->localFrameA.q );+	joint->frameA.p = b2RotateVector( bodySimA->transform.q, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );+	joint->frameB.q = b2MulRot( bodySimB->transform.q, base->localFrameB.q );+	joint->frameB.p = b2RotateVector( bodySimB->transform.q, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );++	// Compute the initial center delta. Incremental position updates are relative to this.+	joint->deltaCenter = b2SubPos( bodySimB->center, bodySimA->center );++	b2Vec2 rA = joint->frameA.p;+	b2Vec2 rB = joint->frameB.p;++	joint->linearSpring = b2MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h );+	joint->angularSpring = b2MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h );++	b2Mat22 kl;+	kl.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;+	kl.cx.y = -rA.y * rA.x * iA - rB.y * rB.x * iB;+	kl.cy.x = kl.cx.y;+	kl.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;+	joint->linearMass = b2GetInverse22( kl );++	float ka = iA + iB;+	joint->angularMass = ka > 0.0f ? 1.0f / ka : 0.0f;++	if ( context->enableWarmStarting == false )+	{+		joint->linearVelocityImpulse = b2Vec2_zero;+		joint->angularVelocityImpulse = 0.0f;+		joint->linearSpringImpulse = b2Vec2_zero;+		joint->angularSpringImpulse = 0.0f;+	}+}++void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_motorJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	b2MotorJoint* joint = &base->motorJoint;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	b2Vec2 linearImpulse = b2Add( joint->linearVelocityImpulse, joint->linearSpringImpulse );+	float angularImpulse = joint->angularVelocityImpulse + joint->angularSpringImpulse;++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, linearImpulse );+		stateA->angularVelocity -= iA * ( b2Cross( rA, linearImpulse ) + angularImpulse );+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, linearImpulse );+		stateB->angularVelocity += iB * ( b2Cross( rB, linearImpulse ) + angularImpulse );+	}+}++void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_motorJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2MotorJoint* joint = &base->motorJoint;+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 vA = stateA->linearVelocity;+	float wA = stateA->angularVelocity;+	b2Vec2 vB = stateB->linearVelocity;+	float wB = stateB->angularVelocity;++	// angular spring+	if ( joint->maxSpringTorque > 0.0f && joint->angularHertz > 0.0f )+	{+		b2Rot qA = b2MulRot( stateA->deltaRotation, joint->frameA.q );+		b2Rot qB = b2MulRot( stateB->deltaRotation, joint->frameB.q );+		b2Rot relQ = b2InvMulRot( qA, qB );++		float c = b2Rot_GetAngle( relQ );+		float bias = joint->angularSpring.biasRate * c;+		float massScale = joint->angularSpring.massScale;+		float impulseScale = joint->angularSpring.impulseScale;++		float cdot = wB - wA;++		float maxImpulse = context->h * joint->maxSpringTorque;+		float oldImpulse = joint->angularSpringImpulse;+		float impulse = -massScale * joint->angularMass * ( cdot + bias ) - impulseScale * oldImpulse;+		joint->angularSpringImpulse = b2ClampFloat( oldImpulse + impulse, -maxImpulse, maxImpulse );+		impulse = joint->angularSpringImpulse - oldImpulse;++		wA -= iA * impulse;+		wB += iB * impulse;+	}++	// angular velocity+	if ( joint->maxVelocityTorque > 0.0f )+	{+		float cdot = wB - wA - joint->angularVelocity;+		float impulse = -joint->angularMass * cdot;++		float maxImpulse = context->h * joint->maxVelocityTorque;+		float oldImpulse = joint->angularVelocityImpulse;+		joint->angularVelocityImpulse = b2ClampFloat( oldImpulse + impulse, -maxImpulse, maxImpulse );+		impulse = joint->angularVelocityImpulse - oldImpulse;++		wA -= iA * impulse;+		wB += iB * impulse;+	}++	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	// linear spring+	if ( joint->maxSpringForce > 0.0f && joint->linearHertz > 0.0f )+	{+		b2Vec2 dcA = stateA->deltaPosition;+		b2Vec2 dcB = stateB->deltaPosition;+		b2Vec2 c = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter );++		b2Vec2 bias = b2MulSV( joint->linearSpring.biasRate, c );+		float massScale = joint->linearSpring.massScale;+		float impulseScale = joint->linearSpring.impulseScale;++		b2Vec2 cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );+		cdot = b2Add( cdot, bias );++		// Updating the effective mass here may be overkill+		b2Mat22 kl;+		kl.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;+		kl.cx.y = -rA.y * rA.x * iA - rB.y * rB.x * iB;+		kl.cy.x = kl.cx.y;+		kl.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;+		joint->linearMass = b2GetInverse22( kl );++		b2Vec2 b = b2MulMV( joint->linearMass, cdot );++		b2Vec2 oldImpulse = joint->linearSpringImpulse;+		b2Vec2 impulse = {+			-massScale * b.x - impulseScale * oldImpulse.x,+			-massScale * b.y - impulseScale * oldImpulse.y,+		};++		float maxImpulse = context->h * joint->maxSpringForce;+		joint->linearSpringImpulse = b2Add( joint->linearSpringImpulse, impulse );++		if ( b2LengthSquared( joint->linearSpringImpulse ) > maxImpulse * maxImpulse )+		{+			joint->linearSpringImpulse = b2Normalize( joint->linearSpringImpulse );+			joint->linearSpringImpulse.x *= maxImpulse;+			joint->linearSpringImpulse.y *= maxImpulse;+		}++		impulse = b2Sub( joint->linearSpringImpulse, oldImpulse );++		vA = b2MulSub( vA, mA, impulse );+		wA -= iA * b2Cross( rA, impulse );+		vB = b2MulAdd( vB, mB, impulse );+		wB += iB * b2Cross( rB, impulse );+	}++	// linear velocity+	if ( joint->maxVelocityForce > 0.0f )+	{+		b2Vec2 cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );+		cdot = b2Sub( cdot, joint->linearVelocity );+		b2Vec2 b = b2MulMV( joint->linearMass, cdot );+		b2Vec2 impulse = { -b.x, -b.y };++		b2Vec2 oldImpulse = joint->linearVelocityImpulse;+		float maxImpulse = context->h * joint->maxVelocityForce;+		joint->linearVelocityImpulse = b2Add( joint->linearVelocityImpulse, impulse );++		if ( b2LengthSquared( joint->linearVelocityImpulse ) > maxImpulse * maxImpulse )+		{+			joint->linearVelocityImpulse = b2Normalize( joint->linearVelocityImpulse );+			joint->linearVelocityImpulse.x *= maxImpulse;+			joint->linearVelocityImpulse.y *= maxImpulse;+		}++		impulse = b2Sub( joint->linearVelocityImpulse, oldImpulse );++		vA = b2MulSub( vA, mA, impulse );+		wA -= iA * b2Cross( rA, impulse );+		vB = b2MulAdd( vB, mB, impulse );+		wB += iB * b2Cross( rB, impulse );+	}++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = vA;+		stateA->angularVelocity = wA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = vB;+		stateB->angularVelocity = wB;+	}+}
+ upstream/src/mover.c view
@@ -0,0 +1,72 @@+// SPDX-FileCopyrightText: 2025 Erin Catto+// SPDX-License-Identifier: MIT++#include "box2d/collision.h"+#include "box2d/constants.h"++b2PlaneSolverResult b2SolvePlanes( b2Vec2 targetDelta, b2CollisionPlane* planes, int count )+{+	for ( int i = 0; i < count; ++i )+	{+		planes[i].push = 0.0f;+	}++	b2Vec2 delta = targetDelta;+	float tolerance = B2_LINEAR_SLOP;++	int iteration;+	for ( iteration = 0; iteration < 20; ++iteration )+	{+		float totalPush = 0.0f;+		for ( int planeIndex = 0; planeIndex < count; ++planeIndex )+		{+			b2CollisionPlane* plane = planes + planeIndex;++			// Add slop to prevent jitter+			float separation = b2PlaneSeparation( plane->plane, delta ) + B2_LINEAR_SLOP;+			// if (separation > 0.0f)+			//{+			//	continue;+			// }++			float push = -separation;++			// Clamp accumulated push+			float accumulatedPush = plane->push;+			plane->push = b2ClampFloat( plane->push + push, 0.0f, plane->pushLimit );+			push = plane->push - accumulatedPush;+			delta = b2MulAdd( delta, push, plane->plane.normal );++			// Track maximum push for convergence+			totalPush += b2AbsFloat( push );+		}++		if ( totalPush < tolerance )+		{+			break;+		}+	}++	return (b2PlaneSolverResult){+		.delta = delta,+		.iterationCount = iteration,+	};+}++b2Vec2 b2ClipVector( b2Vec2 vector, const b2CollisionPlane* planes, int count )+{+	b2Vec2 v = vector;++	for ( int planeIndex = 0; planeIndex < count; ++planeIndex )+	{+		const b2CollisionPlane* plane = planes + planeIndex;+		if ( plane->push == 0.0f || plane->clipVelocity == false )+		{+			continue;+		}++		v = b2MulSub( v, b2MinFloat( 0.0f, b2Dot( v, plane->plane.normal ) ), plane->plane.normal );+	}++	return v;+}
+ upstream/src/mover_joint.c view
@@ -0,0 +1,174 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "body.h"+#include "joint.h"+#include "physics_world.h"+#include "recording.h"+#include "solver.h"+#include "solver_set.h"++// needed for dll export+#include "box2d/box2d.h"++void b2MoverJoint_SetLinearVelocity( b2JointId jointId, b2Vec2 velocity )+{+	B2_CHECK_INPUT( b2IsValidVec2( velocity ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MoverJointSetLinearVelocity, jointId, velocity );++	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_moverJoint );+	joint->moverJoint.linearVelocity = velocity;+}++b2Vec2 b2MoverJoint_GetLinearVelocity( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_moverJoint );+	return joint->moverJoint.linearVelocity;+}++void b2MoverJoint_SetMaxVelocityForce( b2JointId jointId, b2Vec2 maxForce )+{+	B2_CHECK_INPUT( b2IsValidVec2( maxForce ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, MoverJointSetMaxVelocityForce, jointId, maxForce );++	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_moverJoint );+	joint->moverJoint.maxVelocityForce = maxForce;+}++b2Vec2 b2MoverJoint_GetMaxVelocityForce( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_moverJoint );+	return joint->moverJoint.maxVelocityForce;+}++b2Vec2 b2GetMoverJointForce( b2World* world, b2JointSim* base )+{+	b2Vec2 force = b2MulSV( world->inv_h, base->moverJoint.linearVelocityImpulse );+	return force;+}++void b2PrepareMoverJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_moverJoint );++	int idA = base->bodyIdA;+	int idB = base->bodyIdB;++	b2World* world = context->world;++	b2Body* bodyA = b2Array_Get( world->bodies, idA );+	b2Body* bodyB = b2Array_Get( world->bodies, idB );++	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );++	b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex );+	b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex );++	int localIndexA = bodyA->localIndex;+	int localIndexB = bodyB->localIndex;++	b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA );+	b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB );++	float mA = bodySimA->invMass;+	float mB = bodySimB->invMass;++	base->invMassA = mA;+	base->invMassB = mB;+	base->invIA = 0.0f;+	base->invIB = 0.0f;++	b2MoverJoint* joint = &base->moverJoint;+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;++	float k = mA + mB;+	joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f;++	if ( context->enableWarmStarting == false )+	{+		joint->linearVelocityImpulse = b2Vec2_zero;+	}+}++void b2WarmStartMoverJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_moverJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;++	b2MoverJoint* joint = &base->moverJoint;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearVelocityImpulse );+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearVelocityImpulse );+	}+}++void b2SolveMoverJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_moverJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2MoverJoint* joint = &base->moverJoint;+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 vA = stateA->linearVelocity;+	b2Vec2 vB = stateB->linearVelocity;++	// linear velocity+	if ( joint->maxVelocityForce.x > 0.0f || joint->maxVelocityForce.y > 0.0f )+	{+		b2Vec2 cdot = b2Sub( vB, vA );+		cdot = b2Sub( cdot, joint->linearVelocity );+		b2Vec2 b = b2MulSV( joint->linearMass, cdot );+		b2Vec2 impulse = { -b.x, -b.y };++		b2Vec2 oldImpulse = joint->linearVelocityImpulse;+		joint->linearVelocityImpulse = b2Add( joint->linearVelocityImpulse, impulse );+		b2Vec2 maxImpulse = b2MulSV( context->h, joint->maxVelocityForce );++		// clamp x and y separately+		joint->linearVelocityImpulse = b2Clamp( joint->linearVelocityImpulse, b2Neg( maxImpulse ), maxImpulse );++		impulse = b2Sub( joint->linearVelocityImpulse, oldImpulse );+		vA = b2MulSub( vA, mA, impulse );+		vB = b2MulAdd( vB, mB, impulse );+	}+	else+	{+		joint->linearVelocityImpulse = b2Vec2_zero;+	}++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = vA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = vB;+	}+}
+ upstream/src/parallel_for.c view
@@ -0,0 +1,134 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "parallel_for.h"++#include "core.h"+#include "physics_world.h"+#include "platform.h"++#include "box2d/base.h"+#include "box2d/constants.h"++#include <stddef.h>++// Shared state for one b2ParallelFor invocation. Workers race on nextBlock to+// claim work, so a slow chunk can't strand the other threads.+typedef struct b2ParallelForShared+{+	b2AtomicInt nextBlock;+	int blockCount;+	int blockSize;+	int itemCount;+	b2ParallelForCallback* callback;+	void* context;+} b2ParallelForShared;++typedef struct b2ParallelForTask+{+	b2ParallelForShared* shared;+	int workerIndex;+} b2ParallelForTask;++static void b2ParallelForTrampoline( void* taskContext )+{+	b2ParallelForTask* task = taskContext;+	b2ParallelForShared* shared = task->shared;+	int workerIndex = task->workerIndex;+	void* context = shared->context;+	b2ParallelForCallback* callback = shared->callback;++	int blockCount = shared->blockCount;+	int blockSize = shared->blockSize;+	int itemCount = shared->itemCount;++	for ( ;; )+	{+		int blockIndex = b2AtomicFetchAddInt( &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 b2ParallelFor( b2World* world, b2ParallelForCallback* callback, int itemCount, int minRange, void* context )+{+	if ( itemCount <= 0 )+	{+		return;+	}++	B2_ASSERT( minRange > 0 );++	int workerCount = world->workerCount;+	B2_ASSERT( 0 < workerCount && workerCount <= B2_MAX_WORKERS );++	// Target multiple blocks per worker to reduce thread stalls.+	// block size grows once items exceed maxBlockCount * minRange+	// so the block count stays bounded and per-block sync overhead stays low.+	int blocksPerWorker = 8;+	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;+	}+	B2_ASSERT( blockCount >= 1 );+	B2_ASSERT( blockSize * blockCount >= itemCount );++	// No point enqueueing more tasks than blocks.+	int taskCount = workerCount < blockCount ? workerCount : blockCount;++	b2ParallelForShared shared;+	shared.blockCount = blockCount;+	shared.blockSize = blockSize;+	shared.itemCount = itemCount;+	shared.callback = callback;+	shared.context = context;+	b2AtomicStoreInt( &shared.nextBlock, 0 );++	b2ParallelForTask tasks[B2_MAX_WORKERS];+	void* handles[B2_MAX_WORKERS];+	for ( int i = 0; i < taskCount; ++i )+	{+		tasks[i].shared = &shared;+		tasks[i].workerIndex = i;++		if ( world->taskCount < B2_MAX_TASKS )+		{+			handles[i] = world->enqueueTaskFcn( &b2ParallelForTrampoline, tasks + i, world->userTaskContext );+			world->taskCount += 1;+		}+		else+		{+			handles[i] = NULL;+			b2ParallelForTrampoline( 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,22 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++typedef struct b2World b2World;++// Callback invoked by b2ParallelFor to process a range of items. May be called+// multiple times per worker: work is divided into blocks that workers claim+// atomically, so a worker that finishes early picks up the next unclaimed+// block instead of sitting idle. workerIndex is the worker identity and is+// stable across all invocations from the same worker, so it is safe to use as+// an index into per-worker state (e.g. world->taskContexts.data + workerIndex).+typedef void b2ParallelForCallback( 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 8 * workerCount * minRange so block count+// stays bounded.+void b2ParallelFor( b2World* world, b2ParallelForCallback* callback, int itemCount, int minRange, void* context );
+ upstream/src/physics_world.c view
@@ -0,0 +1,3781 @@+// 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 "physics_world.h"++#include "aabb.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 "dynamic_tree.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 "simd.h"+#include "solver.h"+#include "solver_set.h"++#include "box2d/box2d.h"+#include "box2d/constants.h"++#include <float.h>+#include <stdio.h>+#include <string.h>++_Static_assert( B2_MAX_WORLDS > 0, "must be 1 or more" );+_Static_assert( B2_MAX_WORLDS < UINT16_MAX, "B2_MAX_WORLDS limit exceeded" );+static b2World b2_worlds[B2_MAX_WORLDS];++static b2World* b2GetUnlockedWorldFromId( b2WorldId id )+{+	B2_ASSERT( 1 <= id.index1 && id.index1 <= B2_MAX_WORLDS );+	b2World* world = b2_worlds + ( id.index1 - 1 );+	B2_ASSERT( id.index1 == world->worldId + 1 );+	B2_ASSERT( id.generation == world->generation );++	// A world accessed from an id should not be locked+	if ( world->locked )+	{+		B2_ASSERT( false );+		return NULL;+	}+	return world;+}++b2World* b2GetWorldFromId( b2WorldId id )+{+	B2_ASSERT( 1 <= id.index1 && id.index1 <= B2_MAX_WORLDS );+	b2World* world = b2_worlds + ( id.index1 - 1 );+	B2_ASSERT( id.index1 == world->worldId + 1 );+	B2_ASSERT( id.generation == world->generation );+	return world;+}++b2World* b2GetWorld( int index )+{+	B2_ASSERT( 0 <= index && index < B2_MAX_WORLDS );+	b2World* world = b2_worlds + index;+	B2_ASSERT( world->worldId == index );+	return world;+}++b2World* b2GetWorldLocked( int index )+{+	B2_ASSERT( 0 <= index && index < B2_MAX_WORLDS );+	b2World* world = b2_worlds + index;+	B2_ASSERT( world->worldId == index );+	if ( world->locked )+	{+		B2_ASSERT( false );+		return NULL;+	}++	return world;+}++static void* b2DefaultAddTaskFcn( b2TaskCallbackFcn* task, void* taskContext, void* userContext )+{+	B2_UNUSED( userContext );+	task( taskContext );+	return NULL;+}++static void b2DefaultFinishTaskFcn( void* userTask, void* userContext )+{+	B2_UNUSED( userTask, userContext );+}++static void b2CreateWorkerContexts( b2World* world )+{+	b2Array_Create( world->taskContexts );+	b2Array_ResizeAndSetZero( world->taskContexts, world->workerCount );++	b2Array_Create( world->sensorTaskContexts );+	b2Array_ResizeAndSetZero( world->sensorTaskContexts, world->workerCount );++	for ( int i = 0; i < world->workerCount; ++i )+	{+		b2Array_CreateN( world->taskContexts.data[i].sensorHits, 8 );+		b2Array_Create( world->taskContexts.data[i].pairKeys );+		world->taskContexts.data[i].contactStateBitSet = b2CreateBitSet( 1024 );+		world->taskContexts.data[i].hitEventBitSet = b2CreateBitSet( 1024 );+		world->taskContexts.data[i].hasHitEvents = false;+		world->taskContexts.data[i].jointStateBitSet = b2CreateBitSet( 1024 );+		world->taskContexts.data[i].enlargedSimBitSet = b2CreateBitSet( 256 );+		world->taskContexts.data[i].awakeIslandBitSet = b2CreateBitSet( 256 );+		world->taskContexts.data[i].splitIslandId = B2_NULL_INDEX;++		world->sensorTaskContexts.data[i].eventBits = b2CreateBitSet( 128 );+	}+}++static void b2DestroyWorkerContexts( b2World* world )+{+	for ( int i = 0; i < world->workerCount; ++i )+	{+		b2Array_Destroy( world->taskContexts.data[i].sensorHits );+		b2Array_Destroy( world->taskContexts.data[i].pairKeys );+		b2DestroyBitSet( &world->taskContexts.data[i].contactStateBitSet );+		b2DestroyBitSet( &world->taskContexts.data[i].hitEventBitSet );+		b2DestroyBitSet( &world->taskContexts.data[i].jointStateBitSet );+		b2DestroyBitSet( &world->taskContexts.data[i].enlargedSimBitSet );+		b2DestroyBitSet( &world->taskContexts.data[i].awakeIslandBitSet );++		b2DestroyBitSet( &world->sensorTaskContexts.data[i].eventBits );+	}++	b2Array_Destroy( world->taskContexts );+	b2Array_Destroy( world->sensorTaskContexts );+}++b2WorldId b2CreateWorld( const b2WorldDef* def )+{+	_Static_assert( B2_MAX_WORLDS < UINT16_MAX, "B2_MAX_WORLDS limit exceeded" );+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->gravity ), (b2WorldId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->restitutionThreshold ), (b2WorldId){ 0 } );+	B2_CHECK_INPUT_RETURN( def->restitutionIterations >= 0, (b2WorldId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->hitEventThreshold ), (b2WorldId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->contactHertz ), (b2WorldId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->contactDampingRatio ), (b2WorldId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->contactSpeed ), (b2WorldId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->maximumLinearSpeed ) && def->maximumLinearSpeed > 0.0f, (b2WorldId){ 0 } );++	if ( b2IsDenormalFlushEnabled() )+	{+		b2Log( "Denormal flushing is enabled, determinism not supported" );+	}++	int worldId = B2_NULL_INDEX;+	for ( int i = 0; i < B2_MAX_WORLDS; ++i )+	{+		if ( b2_worlds[i].inUse == false )+		{+			worldId = i;+			break;+		}+	}++	if ( worldId == B2_NULL_INDEX )+	{+		return (b2WorldId){ 0 };+	}++	b2World* world = b2_worlds + worldId;+	uint16_t generation = world->generation;++	*world = (b2World){ 0 };++	world->worldId = (uint16_t)worldId;+	world->generation = generation;+	world->inUse = true;++	world->stack = b2CreateStack( b2MaxInt( 2048, def->capacity.arenaByteCount ) );+	b2CreateBroadPhase( &world->broadPhase, &def->capacity );+	b2CreateGraph( &world->constraintGraph, &def->capacity );++	// pools+	world->bodyIdPool = b2CreateIdPool();++	int bodyCapacity = b2MaxInt( 16, def->capacity.staticBodyCount + def->capacity.dynamicBodyCount );+	b2Array_CreateN( world->bodies, bodyCapacity );+	b2Array_CreateN( world->solverSets, 8 );++	// add empty static, active, and disabled body sets+	world->solverSetIdPool = b2CreateIdPool();+	b2SolverSet set = { 0 };++	// static set+	set.setIndex = b2AllocId( &world->solverSetIdPool );+	b2Array_Push( world->solverSets, set );+	b2Array_Reserve( world->solverSets.data[b2_staticSet].bodySims, b2MaxInt( 16, def->capacity.staticBodyCount ) );+	B2_ASSERT( world->solverSets.data[b2_staticSet].setIndex == b2_staticSet );++	// disabled set+	set.setIndex = b2AllocId( &world->solverSetIdPool );+	b2Array_Push( world->solverSets, set );+	B2_ASSERT( world->solverSets.data[b2_disabledSet].setIndex == b2_disabledSet );++	// awake set+	set.setIndex = b2AllocId( &world->solverSetIdPool );+	b2Array_Push( world->solverSets, set );+	b2Array_Reserve( world->solverSets.data[b2_awakeSet].bodySims, b2MaxInt( 16, def->capacity.dynamicBodyCount ) );+	b2Array_Reserve( world->solverSets.data[b2_awakeSet].bodyStates, b2MaxInt( 16, def->capacity.dynamicBodyCount ) );+	b2Array_Reserve( world->solverSets.data[b2_awakeSet].contactSims, b2MaxInt( 16, def->capacity.contactCount ) );+	B2_ASSERT( world->solverSets.data[b2_awakeSet].setIndex == b2_awakeSet );++	world->shapeIdPool = b2CreateIdPool();++	int shapeCapacity = b2MaxInt( 16, def->capacity.staticShapeCount + def->capacity.dynamicShapeCount );+	b2Array_CreateN( world->shapes, shapeCapacity );+	b2Array_CreateN( world->fatAABBs, shapeCapacity );++	world->chainIdPool = b2CreateIdPool();+	b2Array_CreateN( world->chainShapes, 4 );++	world->contactIdPool = b2CreateIdPool();+	b2Array_CreateN( world->contacts, b2MaxInt( 16, def->capacity.contactCount ) );++	world->jointIdPool = b2CreateIdPool();+	b2Array_CreateN( world->joints, 16 );++	world->islandIdPool = b2CreateIdPool();+	b2Array_CreateN( world->islands, b2MaxInt( 16, def->capacity.dynamicBodyCount ) );++	b2Array_CreateN( world->sensors, 4 );++	b2Array_CreateN( world->bodyMoveEvents, 4 );+	b2Array_CreateN( world->sensorBeginEvents, 4 );+	b2Array_CreateN( world->sensorEndEvents[0], 4 );+	b2Array_CreateN( world->sensorEndEvents[1], 4 );+	b2Array_CreateN( world->contactBeginEvents, 4 );+	b2Array_CreateN( world->contactEndEvents[0], 4 );+	b2Array_CreateN( world->contactEndEvents[1], 4 );+	b2Array_CreateN( world->contactHitEvents, 4 );+	b2Array_CreateN( world->jointEvents, 4 );+	world->endEventArrayIndex = 0;++	world->stepIndex = 0;+	world->splitIslandId = B2_NULL_INDEX;+	world->activeTaskCount = 0;+	world->taskCount = 0;+	world->gravity = def->gravity;+	world->hitEventThreshold = b2ClampFloat( def->hitEventThreshold, 0.0f, FLT_MAX );+	world->restitutionThreshold = b2ClampFloat( def->restitutionThreshold, 0.0f, FLT_MAX );+	// Clamp this to avoid overflowing the solver sync flags.+	world->restitutionIterations = b2ClampInt( def->restitutionIterations, 0, B2_MAX_RESTITUTION_ITERATIONS );+	world->enableRestitutionPropagation = def->enableRestitutionPropagation;+	world->maxLinearSpeed = def->maximumLinearSpeed;+	world->contactSpeed = b2ClampFloat( def->contactSpeed, 0.0f, FLT_MAX );+	world->contactHertz = b2ClampFloat( def->contactHertz, 0.0f, FLT_MAX );+	world->contactDampingRatio = b2ClampFloat( def->contactDampingRatio, 0.0f, FLT_MAX );+	world->contactRecycleDistance = B2_CONTACT_RECYCLE_DISTANCE;++	if ( def->frictionCallback != NULL )+	{+		world->frictionCallback = def->frictionCallback;+	}++	if ( def->restitutionCallback != NULL )+	{+		world->restitutionCallback = def->restitutionCallback;+	}++	world->enableSleep = def->enableSleep;+	world->locked = false;+	world->enableWarmStarting = true;+	world->enableContinuous = def->enableContinuous;+	world->simdWidth = b2GetSIMDWidth();+	world->userTreeTask = NULL;+	world->userData = def->userData;++	if ( def->workerCount > 0 && def->enqueueTask != NULL && def->finishTask != NULL )+	{+		// External task system+		world->workerCount = b2MinInt( def->workerCount, B2_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 = b2MinInt( def->workerCount, B2_MAX_WORKERS );+		world->scheduler = b2CreateScheduler( world->workerCount );+		world->enqueueTaskFcn = b2SchedulerEnqueueTask;+		world->finishTaskFcn = b2SchedulerFinishTask;+		world->userTaskContext = world->scheduler;+	}+	else+	{+		// Serial fallback+		world->workerCount = 1;+		world->enqueueTaskFcn = b2DefaultAddTaskFcn;+		world->finishTaskFcn = b2DefaultFinishTaskFcn;+		world->userTaskContext = NULL;+		world->scheduler = NULL;+	}++	b2CreateWorkerContexts( world );++	world->debugBodySet = b2CreateBitSet( 256 );+	world->debugJointSet = b2CreateBitSet( 256 );+	world->debugContactSet = b2CreateBitSet( 256 );+	world->debugIslandSet = b2CreateBitSet( 256 );++	// Recording is started by the host with b2World_StartRecording, never from the world def+	world->recording = NULL;++	// add one to worldId so that 0 represents a null b2WorldId+	return (b2WorldId){+		.index1 = (uint16_t)( worldId + 1 ),+		.generation = world->generation,+	};+}++void b2DestroyWorld( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );++	// Detach any recording before teardown. The user owns and frees the recording buffer.+	b2StopRecordingInternal( world );++	if ( world->scheduler != NULL )+	{+		b2DestroyScheduler( world->scheduler );+		world->scheduler = NULL;+	}++	b2DestroyBitSet( &world->debugBodySet );+	b2DestroyBitSet( &world->debugJointSet );+	b2DestroyBitSet( &world->debugContactSet );+	b2DestroyBitSet( &world->debugIslandSet );++	b2DestroyWorkerContexts( world );++	b2Array_Destroy( world->bodyMoveEvents );+	b2Array_Destroy( world->sensorBeginEvents );+	b2Array_Destroy( world->sensorEndEvents[0] );+	b2Array_Destroy( world->sensorEndEvents[1] );+	b2Array_Destroy( world->contactBeginEvents );+	b2Array_Destroy( world->contactEndEvents[0] );+	b2Array_Destroy( world->contactEndEvents[1] );+	b2Array_Destroy( world->contactHitEvents );+	b2Array_Destroy( world->jointEvents );++	int chainCapacity = world->chainShapes.count;+	for ( int i = 0; i < chainCapacity; ++i )+	{+		b2ChainShape* chain = world->chainShapes.data + i;+		if ( chain->id != B2_NULL_INDEX )+		{+			b2FreeChainData( chain );+		}+		else+		{+			B2_ASSERT( chain->shapeIndices == NULL );+		}+	}++	int sensorCount = world->sensors.count;+	for ( int i = 0; i < sensorCount; ++i )+	{+		b2Array_Destroy( world->sensors.data[i].hits );+		b2Array_Destroy( world->sensors.data[i].overlaps1 );+		b2Array_Destroy( world->sensors.data[i].overlaps2 );+	}++	b2Array_Destroy( world->sensors );++	b2Array_Destroy( world->bodies );+	b2Array_Destroy( world->shapes );+	b2Array_Destroy( world->fatAABBs );+	b2Array_Destroy( world->chainShapes );+	b2Array_Destroy( world->contacts );+	b2Array_Destroy( world->joints );++	for ( int i = 0; i < world->islands.count; ++i )+	{+		b2Array_Destroy( world->islands.data[i].bodies );+		b2Array_Destroy( world->islands.data[i].contacts );+		b2Array_Destroy( world->islands.data[i].joints );+	}+	b2Array_Destroy( world->islands );++	// Destroy solver sets+	int setCapacity = world->solverSets.count;+	for ( int i = 0; i < setCapacity; ++i )+	{+		b2SolverSet* set = world->solverSets.data + i;+		if ( set->setIndex != B2_NULL_INDEX )+		{+			b2DestroySolverSet( world, i );+		}+	}++	b2Array_Destroy( world->solverSets );++	b2DestroyGraph( &world->constraintGraph );+	b2DestroyBroadPhase( &world->broadPhase );++	b2DestroyIdPool( &world->bodyIdPool );+	b2DestroyIdPool( &world->shapeIdPool );+	b2DestroyIdPool( &world->chainIdPool );+	b2DestroyIdPool( &world->contactIdPool );+	b2DestroyIdPool( &world->jointIdPool );+	b2DestroyIdPool( &world->islandIdPool );+	b2DestroyIdPool( &world->solverSetIdPool );++	b2DestroyStack( &world->stack );++	// Wipe world but preserve generation+	uint16_t generation = world->generation;+	*world = (b2World){ 0 };+	world->worldId = 0;+	world->generation = generation + 1;+}++static inline float b2RelativeCos( b2Rot a, b2Rot b )+{+	return a.c * b.c + a.s * b.s;+}++static inline b2BodySim* b2ResolveContactBodySim( b2World* world, b2BodySim* awakeSims, b2BodySim* staticSims, int encodedIndex,+												  int bodyId )+{+	if ( encodedIndex >= 0 )+	{+		return awakeSims + encodedIndex;+	}++	if ( b2IsStaticSimIndex( encodedIndex ) )+	{+		return staticSims - encodedIndex - 2;+	}++	return b2GetBodySim( world, b2Array_Get( world->bodies, bodyId ) );+}++static void b2CollideTask( int startIndex, int endIndex, int workerIndex, void* context )+{+	b2TracyCZoneNC( collide_task, "Collide", b2_colorDodgerBlue, true );++	b2StepContext* stepContext = context;+	b2World* world = stepContext->world;+	b2TaskContext* taskContext = world->taskContexts.data + workerIndex;+	const b2ContactCollideSpan* spans = stepContext->collideSpans;+	b2Shape* shapes = world->shapes.data;+	const b2AABB* fatAABBs = world->fatAABBs.data;+	b2SolverSet* solverSets = world->solverSets.data;+	b2BodySim* awakeSims = solverSets[b2_awakeSet].bodySims.data;+	b2BodySim* staticSims = solverSets[b2_staticSet].bodySims.data;++	B2_ASSERT( startIndex < endIndex );++	int spanIndex = 0;+	while ( spans[spanIndex + 1].start <= startIndex )+	{+		spanIndex += 1;+	}++	int spanStart = spans[spanIndex].start;+	int spanEnd = spans[spanIndex + 1].start;+	b2ContactSim* spanBase = spans[spanIndex].contacts;++	float recycleDistance = world->contactRecycleDistance;+	float speculativeDistance = B2_SPECULATIVE_DISTANCE;+	float recycleDistanceNonTouching = b2MinFloat( recycleDistance, speculativeDistance );++	for ( int contactIndex = startIndex; contactIndex < endIndex; ++contactIndex )+	{+		if ( contactIndex == spanEnd )+		{+			spanIndex += 1;+			spanStart = spans[spanIndex].start;+			spanEnd = spans[spanIndex + 1].start;+			spanBase = spans[spanIndex].contacts;+		}++		b2ContactSim* contactSim = spanBase + ( contactIndex - spanStart );++		int contactId = contactSim->contactId;+		int shapeIdA = contactSim->shapeIdA;+		int shapeIdB = contactSim->shapeIdB;++		// Do proxies still overlap?+		bool overlap = b2OverlapV( fatAABBs + shapeIdA, fatAABBs + shapeIdB );+		if ( overlap == false )+		{+			contactSim->simFlags |= b2_simDisjoint;+			contactSim->simFlags &= ~b2_simTouchingFlag;+			b2SetBit( &taskContext->contactStateBitSet, contactId );+		}+		else+		{+			b2Shape* shapeA = shapes + shapeIdA;+			b2Shape* shapeB = shapes + shapeIdB;++			bool wasTouching = ( contactSim->simFlags & b2_simTouchingFlag );++			// Update contact respecting shape/body order (A,B)+			int encodedA = contactSim->encodedBodySimA;+			int encodedB = contactSim->encodedBodySimB;+			b2BodySim* bodySimA = b2ResolveContactBodySim( world, awakeSims, staticSims, encodedA, shapeA->bodyId );+			b2BodySim* bodySimB = b2ResolveContactBodySim( world, awakeSims, staticSims, encodedB, shapeB->bodyId );+			b2WorldTransform transformA = bodySimA->transform;+			b2WorldTransform transformB = bodySimB->transform;++			// These may not be skipped by relative transform check below+			contactSim->invMassA = bodySimA->invMass;+			contactSim->invIA = bodySimA->invInertia;+			contactSim->invMassB = bodySimB->invMass;+			contactSim->invIB = bodySimB->invInertia;++			// Contact recycling optimization. Please cite this code 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 ( recycleDistance > 0.0f && ( contactSim->simFlags & b2_simRelativeTransformValid ) &&+				 ( contactSim->simFlags & b2_contactRecycleFlag ) )+			{+				b2Rot cachedQA = contactSim->cachedRotationA;+				b2Rot cachedQB = contactSim->cachedRotationB;+				b2Transform xfc = contactSim->cachedRelativePose;+				b2Transform xf = b2InvMulWorldTransforms( transformA, transformB );++				float cosA = b2RelativeCos( transformA.q, cachedQA );+				float cosB = b2RelativeCos( transformB.q, cachedQB );+				float minCos = b2MinFloat( cosA, cosB );++				float maxExtentA = b2IsStaticSimIndex( encodedA ) ? 0.0f : bodySimA->maxExtent;+				float maxExtentB = b2IsStaticSimIndex( encodedB ) ? 0.0f : bodySimB->maxExtent;+				float maxExtent = b2MaxFloat( maxExtentA, maxExtentB );+				float distance = b2Distance( xf.p, xfc.p );+				b2Rot qr = b2InvMulRot( xf.q, xfc.q );++				// This metric is used for fast bodies and sleeping. It comes from conservative advancement.+				// Note that qr.s == sin(theta) ~= theta for small angles.+				// Need a tighter tolerance for non-touching shapes so that contacts are not missed.+				float tolerance = wasTouching ? recycleDistance : recycleDistanceNonTouching;++				if ( minCos > B2_CONTACT_RECYCLE_COS_ANGLE && distance + maxExtent * b2AbsFloat( qr.s ) < tolerance )+				{+					b2Rot dqA = b2MulRot( transformA.q, b2InvertRot( cachedQA ) );+					b2Rot dqB = b2MulRot( transformB.q, b2InvertRot( cachedQB ) );+					b2Vec2 normal = contactSim->manifold.normal;++					// Minimize round-off+					b2Vec2 dc = b2SubPos( bodySimB->center, bodySimA->center );++					for ( int i = 0; i < contactSim->manifold.pointCount; ++i )+					{+						// Keep anchors but update separation, same as sub-stepping. This eliminates jitter.+						b2ManifoldPoint* mp = contactSim->manifold.points + i;+						b2Vec2 rA = b2RotateVector( dqA, mp->anchorA );+						b2Vec2 rB = b2RotateVector( dqB, mp->anchorB );+						b2Vec2 dp = b2Add( dc, b2Sub( rB, rA ) );+						+						mp->separation = mp->baseSeparation + b2Dot( dp, normal );+						mp->normalVelocity = 0.0f;+						mp->persisted = true;+					}++					taskContext->recycledContactCount += 1;++					// Contact is recycled. This also skips updating other aspects of the contact+					// such as material parameters.+					continue;+				}+			}++			// Caching for contact recycling.+			contactSim->cachedRotationA = transformA.q;+			contactSim->cachedRotationB = transformB.q;+			contactSim->cachedRelativePose = b2InvMulWorldTransforms( transformA, transformB );+			contactSim->simFlags |= b2_simRelativeTransformValid;++			b2Vec2 centerOffsetA = b2RotateVector( transformA.q, bodySimA->localCenter );+			b2Vec2 centerOffsetB = b2RotateVector( transformB.q, bodySimB->localCenter );++			// This updates solid contacts+			bool touching =+				b2UpdateContact( world, contactSim, shapeA, transformA, centerOffsetA, shapeB, transformB, centerOffsetB );++			// State changes that affect island connectivity. Also affects contact events.+			if ( touching == true && wasTouching == false )+			{+				contactSim->simFlags |= b2_simStartedTouching;+				b2SetBit( &taskContext->contactStateBitSet, contactId );+			}+			else if ( touching == false && wasTouching == true )+			{+				contactSim->simFlags |= b2_simStoppedTouching;+				b2SetBit( &taskContext->contactStateBitSet, contactId );+			}++			if ( touching )+			{+				for ( int i = 0; i < contactSim->manifold.pointCount; ++i )+				{+					// Cache separation+					b2ManifoldPoint* mp = contactSim->manifold.points + i;+					mp->baseSeparation = mp->separation;+				}+			}++			// To make this work, the time of impact code needs to adjust the target+			// distance based on the number of TOI events for a body.+			// if (touching && bodySimB->isFast)+			//{+			//	b2Manifold* manifold = &contactSim->manifold;+			//	int pointCount = manifold->pointCount;+			//	for (int i = 0; i < pointCount; ++i)+			//	{+			//		// trick the solver into pushing the fast shapes apart+			//		manifold->points[i].separation -= 0.25f * B2_SPECULATIVE_DISTANCE;+			//	}+			//}+		}+	}++	b2TracyCZoneEnd( collide_task );+}++static void b2AddNonTouchingContact( b2World* world, b2Contact* contact, b2ContactSim* contactSim )+{+	B2_ASSERT( contact->setIndex == b2_awakeSet );+	b2SolverSet* set = b2Array_Get( world->solverSets, b2_awakeSet );+	contact->colorIndex = B2_NULL_INDEX;+	contact->localIndex = set->contactSims.count;++	b2ContactSim* newContactSim = b2Array_Emplace( set->contactSims );+	memcpy( newContactSim, contactSim, sizeof( b2ContactSim ) );+}++static void b2RemoveNonTouchingContact( b2World* world, int setIndex, int localIndex )+{+	b2SolverSet* set = b2Array_Get( world->solverSets, setIndex );+	int movedIndex = b2Array_RemoveSwap( set->contactSims, localIndex );+	if ( movedIndex != B2_NULL_INDEX )+	{+		b2ContactSim* movedContactSim = set->contactSims.data + localIndex;+		b2Contact* movedContact = b2Array_Get( world->contacts, movedContactSim->contactId );+		B2_ASSERT( movedContact->setIndex == setIndex );+		B2_ASSERT( movedContact->localIndex == movedIndex );+		B2_ASSERT( movedContact->colorIndex == B2_NULL_INDEX );+		movedContact->localIndex = localIndex;+	}+}++// Narrow-phase collision+static void b2Collide( b2StepContext* context )+{+	b2World* world = context->world;++	B2_ASSERT( world->workerCount > 0 );++	b2TracyCZoneNC( collide, "Narrow Phase", b2_colorDodgerBlue, true );++	// gather contacts into a single array for easier parallel-for+	int contactCount = 0;+	b2GraphColor* graphColors = world->constraintGraph.colors;+	for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i )+	{+		contactCount += graphColors[i].contactSims.count;+	}++	int nonTouchingCount = world->solverSets.data[b2_awakeSet].contactSims.count;+	contactCount += nonTouchingCount;++	if ( contactCount == 0 )+	{+		b2TracyCZoneEnd( collide );+		return;+	}++	b2ContactCollideSpan collideSpans[B2_GRAPH_COLOR_COUNT + 2];+	int spanCount = 0;+	int contactIndex = 0;+	for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i )+	{+		b2GraphColor* color = graphColors + i;+		int count = color->contactSims.count;+		if ( count > 0 )+		{+			collideSpans[spanCount].start = contactIndex;+			collideSpans[spanCount].contacts = color->contactSims.data;+			spanCount += 1;+			contactIndex += count;+		}+	}++	if ( nonTouchingCount > 0 )+	{+		collideSpans[spanCount].start = contactIndex;+		collideSpans[spanCount].contacts = world->solverSets.data[b2_awakeSet].contactSims.data;+		spanCount += 1;+		contactIndex += nonTouchingCount;+	}++	B2_ASSERT( contactIndex == contactCount );+	B2_ASSERT( spanCount <= B2_GRAPH_COLOR_COUNT + 1 );++	collideSpans[spanCount].start = contactCount;+	collideSpans[spanCount].contacts = NULL;++	context->collideSpans = collideSpans;++	// Contact bit set on ids because contact pointers are unstable as they move between touching and not touching.+	int contactIdCapacity = b2GetIdCapacity( &world->contactIdPool );+	for ( int i = 0; i < world->workerCount; ++i )+	{+		b2SetBitCountAndClear( &world->taskContexts.data[i].contactStateBitSet, contactIdCapacity );+		world->taskContexts.data[i].recycledContactCount = 0;+	}++	// Task should take at least 40us on a 4GHz CPU (10K cycles)+	int minRange = 64;+	b2ParallelFor( world, &b2CollideTask, contactCount, minRange, context );++	context->collideSpans = NULL;++	// Serially update contact state+	b2TracyCZoneNC( contact_state, "Contact State", b2_colorLightSlateGray, true );++	// Bitwise OR all contact bits+	b2BitSet* bitSet = &world->taskContexts.data[0].contactStateBitSet;+	for ( int i = 1; i < world->workerCount; ++i )+	{+		b2InPlaceUnion( bitSet, &world->taskContexts.data[i].contactStateBitSet );+	}++	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );++	int endEventArrayIndex = world->endEventArrayIndex;++	const b2Shape* 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 = b2CTZ64( bits );+			int contactId = (int)( 64 * k + ctz );++			b2Contact* contact = b2Array_Get( world->contacts, contactId );+			B2_ASSERT( contact->setIndex == b2_awakeSet );++			int colorIndex = contact->colorIndex;+			int localIndex = contact->localIndex;++			b2ContactSim* contactSim = NULL;+			if ( colorIndex != B2_NULL_INDEX )+			{+				// contact lives in constraint graph+				B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );+				b2GraphColor* color = graphColors + colorIndex;+				contactSim = b2Array_Get( color->contactSims, localIndex );+			}+			else+			{+				contactSim = b2Array_Get( awakeSet->contactSims, localIndex );+			}++			uint32_t flags = contact->flags;+			uint32_t simFlags = contactSim->simFlags;++			if ( simFlags & b2_simDisjoint )+			{+				// Bounding boxes no longer overlap+				b2DestroyContact( world, contact );+				contact = NULL;+				contactSim = NULL;+			}+			else if ( simFlags & b2_simStartedTouching )+			{+				B2_ASSERT( contact->islandId == B2_NULL_INDEX );++				if ( flags & b2_contactEnableContactEvents )+				{+					const b2Shape* shapeA = shapes + contact->shapeIdA;+					const b2Shape* shapeB = shapes + contact->shapeIdB;+					b2ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation };+					b2ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation };+					b2ContactId contactFullId = {+						.index1 = contactId + 1,+						.world0 = worldId,+						.padding = 0,+						.generation = contact->generation,+					};++					b2ContactBeginTouchEvent event = { shapeIdA, shapeIdB, contactFullId };+					b2Array_Push( world->contactBeginEvents, event );+				}++				B2_ASSERT( contactSim->manifold.pointCount > 0 );+				B2_ASSERT( contact->setIndex == b2_awakeSet );++				// Link first because this wakes colliding bodies and ensures the body sims+				// are in the correct place.+				contact->flags |= b2_contactTouchingFlag;+				b2LinkContact( world, contact );++				// Make sure these didn't change+				B2_ASSERT( contact->colorIndex == B2_NULL_INDEX );+				B2_ASSERT( contact->localIndex == localIndex );++				// Contact sim pointer may have become orphaned due to awake set growth,+				// so I just need to refresh it.+				contactSim = b2Array_Get( awakeSet->contactSims, localIndex );++				contactSim->simFlags &= ~b2_simStartedTouching;++				// Add first for memcpy+				b2AddContactToGraph( world, contactSim, contact );++				// This destroys the contact sim+				b2RemoveNonTouchingContact( world, b2_awakeSet, localIndex );++				contactSim = NULL;+			}+			else if ( simFlags & b2_simStoppedTouching )+			{+				contactSim->simFlags &= ~b2_simStoppedTouching;+				contact->flags &= ~b2_contactTouchingFlag;++				if ( contact->flags & b2_contactEnableContactEvents )+				{+					const b2Shape* shapeA = shapes + contact->shapeIdA;+					const b2Shape* shapeB = shapes + contact->shapeIdB;+					b2ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation };+					b2ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation };+					b2ContactId contactFullId = {+						.index1 = contactId + 1,+						.world0 = worldId,+						.padding = 0,+						.generation = contact->generation,+					};++					b2ContactEndTouchEvent event = { shapeIdA, shapeIdB, contactFullId };+					b2Array_Push( world->contactEndEvents[endEventArrayIndex], event );+				}++				B2_ASSERT( contactSim->manifold.pointCount == 0 );++				b2UnlinkContact( world, contact );+				int bodyIdA = contact->edges[0].bodyId;+				int bodyIdB = contact->edges[1].bodyId;++				// Add first for memcpy+				b2AddNonTouchingContact( world, contact, contactSim );+				b2RemoveContactFromGraph( world, bodyIdA, bodyIdB, colorIndex, localIndex );+				contact = NULL;+				contactSim = NULL;+			}++			// Clear the smallest set bit+			bits = bits & ( bits - 1 );+		}+	}++	b2ValidateSolverSets( world );+	b2ValidateContacts( world );++	b2TracyCZoneEnd( contact_state );+	b2TracyCZoneEnd( collide );+}++void b2World_Step( b2WorldId worldId, float timeStep, int subStepCount )+{+	B2_CHECK_INPUT( b2IsValidFloat( timeStep ) );+	B2_ASSERT( 0 < subStepCount );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		b2TracyCFrame;+		return;+	}++	// Record step inputs before simulation runs+	B2_REC( world, Step, worldId, timeStep, subStepCount );++	// Prepare to capture events+	// Ensure user does not access stale data if there is an early return+	b2Array_Clear( world->bodyMoveEvents );+	b2Array_Clear( world->sensorBeginEvents );+	b2Array_Clear( world->contactBeginEvents );+	b2Array_Clear( world->contactHitEvents );+	b2Array_Clear( world->jointEvents );++	world->profile = (b2Profile){ 0 };++	b2TracyCZoneNC( world_step, "Step", b2_colorBox2DGreen, true );++	world->locked = true;+	world->activeTaskCount = 0;+	world->taskCount = 0;++	if ( world->scheduler != NULL )+	{+		b2ResetScheduler( world->scheduler );+	}++	uint64_t stepTicks = b2GetTicks();++	{+		b2Capacity* c = &world->maxCapacity;+		c->staticShapeCount = b2MaxInt( c->staticShapeCount, world->broadPhase.trees[b2_staticBody].proxyCount );+		c->dynamicShapeCount = b2MaxInt( c->dynamicShapeCount, world->broadPhase.trees[b2_dynamicBody].proxyCount );++		int staticBodyCount = world->solverSets.data[b2_staticSet].bodySims.count;+		c->staticBodyCount = b2MaxInt( c->staticBodyCount, staticBodyCount );++		// this includes kinematic bodies+		int totalBodyCount = b2GetIdCount( &world->bodyIdPool );+		c->dynamicBodyCount = b2MaxInt( c->dynamicBodyCount, totalBodyCount - staticBodyCount );++		int totalContactCount = b2GetIdCount( &world->contactIdPool );+		c->contactCount = b2MaxInt( c->contactCount, totalContactCount );+	}++	// Update collision pairs and create contacts+	{+		uint64_t pairTicks = b2GetTicks();+		b2UpdateBroadPhasePairs( world );+		world->profile.pairs = b2GetMilliseconds( pairTicks );+	}++	b2StepContext context = { 0 };+	context.world = world;+	context.dt = timeStep;+	context.subStepCount = b2MaxInt( 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 = b2MinFloat( world->contactHertz, 0.125f * context.inv_h );+	context.contactSoftness = b2MakeSoft( contactHertz, world->contactDampingRatio, context.h );+	context.staticSoftness = b2MakeSoft( 2.0f * contactHertz, world->contactDampingRatio, context.h );++	context.restitutionThreshold = world->restitutionThreshold;+	context.maxLinearVelocity = world->maxLinearSpeed;+	context.enableWarmStarting = world->enableWarmStarting;++	// Narrow phase : update contacts+	{+		uint64_t collideTicks = b2GetTicks();+		b2Collide( &context );+		world->profile.collide = b2GetMilliseconds( collideTicks );+	}++	// Integrate velocities, solve velocity constraints, and integrate positions.+	if ( timeStep > 0.0f )+	{+		uint64_t solveTicks = b2GetTicks();+		b2Solve( world, &context );+		world->profile.solve = b2GetMilliseconds( solveTicks );+	}++	// Finish the tree task in case b2Solve didn't finish it+	if ( world->userTreeTask )+	{+		world->finishTaskFcn( world->userTreeTask, world->userTaskContext );+		world->userTreeTask = NULL;+		world->activeTaskCount -= 1;+		b2ValidateNoEnlarged( &world->broadPhase );+	}++	// Update sensors+	{+		uint64_t sensorTicks = b2GetTicks();+		b2OverlapSensors( world );+		world->profile.sensors = b2GetMilliseconds( sensorTicks );+	}++	world->profile.step = b2GetMilliseconds( stepTicks );++	B2_ASSERT( b2GetStackAllocation( &world->stack ) == 0 );++	world->maxCapacity.arenaByteCount = b2MaxInt( world->maxCapacity.arenaByteCount, b2GetMaxStackAllocation( &world->stack ) );++	// Ensure stack is large enough+	b2GrowStack( &world->stack );++	// Make sure all tasks that were started were also finished+	B2_ASSERT( world->activeTaskCount == 0 );++	b2TracyCZoneEnd( world_step );++	// Swap end event array buffers+	world->endEventArrayIndex = 1 - world->endEventArrayIndex;+	b2Array_Clear( world->sensorEndEvents[world->endEventArrayIndex] );+	b2Array_Clear( world->contactEndEvents[world->endEventArrayIndex] );++	if ( world->recording != NULL )+	{+		// Write the per-step StateHash while the world is still locked. Queries early return while+		// locked, so this keeps the shared recording buffer single-writer without a lock. StateHash+		// proves the simulation reproduced exactly on replay.+		uint64_t hash = b2HashWorldState( world );+		b2RecArgs_StateHash stateHash = { worldId, hash };+		b2RecWrite_StateHash( world->recording, &stateHash );++		// Grow the recorded bounds so a replay can frame the whole motion, not just frame 0+		b2AABB bounds;+		if ( b2ComputeWorldBounds( world, &bounds ) )+		{+			b2RecAccumulateBounds( world->recording, bounds );+		}+	}++	world->locked = false;++	b2TracyCFrame;+}++static void b2DrawShape( b2DebugDraw* draw, b2Shape* shape, b2WorldTransform transform, b2HexColor color, bool drawChainNormals )+{+	switch ( shape->type )+	{+		case b2_capsuleShape:+		{+			b2Capsule* capsule = &shape->capsule;+			b2Pos p1 = b2TransformWorldPoint( transform, capsule->center1 );+			b2Pos p2 = b2TransformWorldPoint( transform, capsule->center2 );+			draw->DrawSolidCapsuleFcn( p1, p2, capsule->radius, color, draw->context );+		}+		break;++		case b2_circleShape:+		{+			b2Circle* circle = &shape->circle;+			draw->DrawSolidCircleFcn( transform, circle->center, circle->radius, color, draw->context );+		}+		break;++		case b2_polygonShape:+		{+			b2Polygon* poly = &shape->polygon;+			draw->DrawSolidPolygonFcn( transform, poly->vertices, poly->count, poly->radius, color, draw->context );+		}+		break;++		case b2_segmentShape:+		{+			b2Segment* segment = &shape->segment;+			b2Pos p1 = b2TransformWorldPoint( transform, segment->point1 );+			b2Pos p2 = b2TransformWorldPoint( transform, segment->point2 );+			draw->DrawLineFcn( p1, p2, color, draw->context );+		}+		break;++		case b2_chainSegmentShape:+		{+			b2Segment* segment = &shape->chainSegment.segment;+			b2Pos p1 = b2TransformWorldPoint( transform, segment->point1 );+			b2Pos p2 = b2TransformWorldPoint( transform, segment->point2 );+			draw->DrawLineFcn( p1, p2, color, draw->context );+			draw->DrawPointFcn( p2, 4.0f, color, draw->context );++			if ( drawChainNormals )+			{+				b2Pos c = b2LerpPos( p1, p2, 0.5f );+				b2Vec2 e = b2Normalize( b2SubPos( p2, p1 ) );+				b2Vec2 n = b2RightPerp( e );+				float L = 0.2f * b2GetLengthUnitsPerMeter();+				draw->DrawLineFcn( c, b2OffsetPos( c, b2MulSV( L, n ) ), b2_colorPaleGreen, draw->context );+			}+		}+		break;++		default:+			break;+	}+}++struct DrawContext+{+	b2World* world;+	b2DebugDraw* draw;+};++static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;++	struct DrawContext* drawContext = context;+	b2World* world = drawContext->world;+	b2DebugDraw* draw = drawContext->draw;++	b2Shape* shape = b2Array_Get( world->shapes, shapeId );+	B2_ASSERT( shape->id == shapeId );+	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	b2BodySim* bodySim = b2GetBodySim( world, body );++	b2SetBit( &world->debugBodySet, shape->bodyId );++	if ( draw->drawShapes )+	{+		b2HexColor color;++		if ( shape->material.customColor != 0 )+		{+			color = shape->material.customColor;+		}+		else if ( body->type == b2_dynamicBody && body->mass == 0.0f )+		{+			// Bad body+			color = b2_colorRed;+		}+		else if ( body->setIndex == b2_disabledSet )+		{+			color = b2_colorSlateGray;+		}+		else if ( shape->sensorIndex != B2_NULL_INDEX )+		{+			color = b2_colorWheat;+		}+		else if ( bodySim->flags & b2_hadTimeOfImpact )+		{+			color = b2_colorLime;+		}+		else if ( ( bodySim->flags & b2_isBullet ) && body->setIndex == b2_awakeSet )+		{+			color = b2_colorTurquoise;+		}+		else if ( body->flags & b2_isSpeedCapped )+		{+			color = b2_colorYellow;+		}+		else if ( bodySim->flags & b2_isFast )+		{+			color = b2_colorSalmon;+		}+		else if ( body->type == b2_staticBody )+		{+			color = b2_colorPaleGreen;+		}+		else if ( body->type == b2_kinematicBody )+		{+			color = b2_colorRoyalBlue;+		}+		else if ( body->setIndex == b2_awakeSet )+		{+			color = b2_colorPink;+		}+		else+		{+			color = b2_colorGray;+		}++		b2DrawShape( draw, shape, bodySim->transform, color, draw->drawChainNormals );+	}++	if ( draw->drawBounds )+	{+		draw->DrawBoundsFcn( world->fatAABBs.data[shapeId], b2_colorGold, draw->context );+	}++	return true;+}++// todo this has varying order for moving shapes, causing flicker when overlapping shapes are moving+// solution: display order by shape id modulus 3, keep 3 buckets in GLSolid* and flush in 3 passes.+void b2World_Draw( b2WorldId worldId, b2DebugDraw* draw )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_ASSERT( b2IsValidAABB( draw->drawingBounds ) );++	const float k_axisScale = 0.3f;+	b2HexColor speculativeColor = b2_colorGainsboro;+	b2HexColor addColor = b2_colorGreen;+	b2HexColor persistColor = b2_colorBlue;+	b2HexColor normalColor = b2_colorDimGray;+	b2HexColor impulseColor = b2_colorMagenta;+	b2HexColor frictionColor = b2_colorYellow;++	int bodyCapacity = b2GetIdCapacity( &world->bodyIdPool );+	b2SetBitCountAndClear( &world->debugBodySet, bodyCapacity );++	int jointCapacity = b2GetIdCapacity( &world->jointIdPool );+	b2SetBitCountAndClear( &world->debugJointSet, jointCapacity );++	int contactCapacity = b2GetIdCapacity( &world->contactIdPool );+	b2SetBitCountAndClear( &world->debugContactSet, contactCapacity );++	int islandCapacity = b2GetIdCapacity( &world->islandIdPool );+	b2SetBitCountAndClear( &world->debugIslandSet, islandCapacity );++	struct DrawContext drawContext = { world, draw };++	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2DynamicTree_OverlapAABBAll( world->broadPhase.trees + i, draw->drawingBounds, DrawQueryCallback, &drawContext );+	}++	uint32_t wordCount = world->debugBodySet.blockCount;+	uint64_t* bits = world->debugBodySet.bits;+	for ( uint32_t k = 0; k < wordCount; ++k )+	{+		uint64_t word = bits[k];+		while ( word != 0 )+		{+			uint32_t ctz = b2CTZ64( word );+			uint32_t bodyId = 64 * k + ctz;++			b2Body* body = b2Array_Get( world->bodies, (int)bodyId );++			if ( draw->drawBodyNames && body->name[0] != 0 )+			{+				b2Vec2 offset = { 0.1f, 0.1f };+				b2BodySim* bodySim = b2GetBodySim( world, body );++				b2WorldTransform transform = { bodySim->center, bodySim->transform.q };+				b2Pos p = b2TransformWorldPoint( transform, offset );+				draw->DrawStringFcn( p, body->name, b2_colorWhiteSmoke, draw->context );+			}++			if ( draw->drawMass && body->type == b2_dynamicBody )+			{+				b2Vec2 offset = { 0.1f, 0.1f };+				b2BodySim* bodySim = b2GetBodySim( world, body );++				b2WorldTransform transform = { bodySim->center, bodySim->transform.q };+				draw->DrawLineFcn( bodySim->center0, bodySim->center, b2_colorWhiteSmoke, draw->context );+				draw->DrawTransformFcn( transform, draw->context );++				b2Pos p = b2TransformWorldPoint( transform, offset );+				char buffer[32];+				snprintf( buffer, 32, "  %.2f", body->mass );+				draw->DrawStringFcn( p, buffer, b2_colorWhite, draw->context );+			}++			if ( draw->drawJoints )+			{+				int jointKey = body->headJointKey;+				while ( jointKey != B2_NULL_INDEX )+				{+					int jointId = jointKey >> 1;+					int edgeIndex = jointKey & 1;+					b2Joint* joint = b2Array_Get( world->joints, jointId );++					// avoid double draw+					if ( b2GetBit( &world->debugJointSet, jointId ) == false )+					{+						b2DrawJoint( draw, world, joint );+						b2SetBit( &world->debugJointSet, jointId );+					}++					jointKey = joint->edges[edgeIndex].nextKey;+				}+			}++			const float linearSlop = B2_LINEAR_SLOP;+			if ( draw->drawContacts && body->type == b2_dynamicBody )+			{+				int contactKey = body->headContactKey;+				while ( contactKey != B2_NULL_INDEX )+				{+					int contactId = contactKey >> 1;+					int edgeIndex = contactKey & 1;+					b2Contact* contact = b2Array_Get( world->contacts, contactId );+					contactKey = contact->edges[edgeIndex].nextKey;++					// avoid double draw+					if ( b2GetBit( &world->debugContactSet, contactId ) == false )+					{+						b2ContactSim* contactSim = b2GetContactSim( world, contact );+						b2Body* bodyA = b2Array_Get( world->bodies, contact->edges[0].bodyId );+						b2BodySim* bodySimA = b2GetBodySim( world, bodyA );+						b2Body* bodyB = b2Array_Get( world->bodies, contact->edges[1].bodyId );+						b2BodySim* bodySimB = b2GetBodySim( world, bodyB );+						int pointCount = contactSim->manifold.pointCount;+						b2Vec2 normal = contactSim->manifold.normal;+						char buffer[32];++						for ( int j = 0; j < pointCount; ++j )+						{+							b2ManifoldPoint* mp = contactSim->manifold.points + j;++							b2Pos p;+							if ( draw->drawAnchorA )+							{+								p = b2OffsetPos( bodySimA->center, mp->anchorA );+							}+							else+							{+								p = b2OffsetPos( bodySimB->center, mp->anchorB );+							}++							if ( draw->drawGraphColors && contact->colorIndex != B2_NULL_INDEX )+							{+								// graph color+								float pointSize = contact->colorIndex == B2_OVERFLOW_INDEX ? 7.5f : 5.0f;+								draw->DrawPointFcn( p, pointSize, b2GetGraphColor( contact->colorIndex ), draw->context );+								// m_context->draw.DrawString(point->position, "%d", point->color);+							}+							else if ( mp->separation > linearSlop )+							{+								// Speculative+								draw->DrawPointFcn( p, 5.0f, speculativeColor, draw->context );+							}+							else if ( mp->persisted == false )+							{+								// Add+								draw->DrawPointFcn( p, 10.0f, addColor, draw->context );+							}+							else if ( mp->persisted == true )+							{+								// Persist+								draw->DrawPointFcn( p, 5.0f, persistColor, draw->context );+							}++							if ( draw->drawContactNormals )+							{+								b2Pos p1 = p;+								b2Pos p2 = b2OffsetPos( p1, b2MulSV( k_axisScale, normal ) );+								draw->DrawLineFcn( p1, p2, normalColor, draw->context );++								snprintf( buffer, B2_ARRAY_COUNT( buffer ), " %.2f", mp->separation );+								draw->DrawStringFcn( p1, buffer, b2_colorWhite, draw->context );+							}+							else if ( draw->drawContactForces )+							{+								// todo validate+								// multiply by one-half due to relax iteration+								float force = 0.5f * mp->totalNormalImpulse * world->inv_dt;+								b2Pos p1 = p;+								b2Pos p2 = b2OffsetPos( p1, b2MulSV( draw->forceScale * force, normal ) );+								draw->DrawLineFcn( p1, p2, impulseColor, draw->context );+								snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.1f", force );+								draw->DrawStringFcn( p1, buffer, b2_colorWhite, draw->context );+							}++							if ( draw->drawContactFeatures )+							{+								snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%d", mp->id );+								draw->DrawStringFcn( p, buffer, b2_colorOrange, draw->context );+							}++							if ( draw->drawFrictionForces )+							{+								float force = 0.5f * mp->tangentImpulse * world->inv_h;+								b2Vec2 tangent = b2RightPerp( normal );+								b2Pos p1 = p;+								b2Pos p2 = b2OffsetPos( p1, b2MulSV( draw->forceScale * force, tangent ) );+								draw->DrawLineFcn( p1, p2, frictionColor, draw->context );+								snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.1f", force );+								draw->DrawStringFcn( p1, buffer, b2_colorWhite, draw->context );+							}+						}++						b2SetBit( &world->debugContactSet, contactId );+					}++					contactKey = contact->edges[edgeIndex].nextKey;+				}+			}++			if ( draw->drawIslands )+			{+				int islandId = body->islandId;+				if ( islandId != B2_NULL_INDEX && b2GetBit( &world->debugIslandSet, islandId ) == false )+				{+					b2Island* island = world->islands.data + islandId;+					if ( island->setIndex == B2_NULL_INDEX )+					{+						continue;+					}++					int shapeCount = 0;+					b2AABB aabb = {+						.lowerBound = { FLT_MAX, FLT_MAX },+						.upperBound = { -FLT_MAX, -FLT_MAX },+					};++					for ( int bodyIndex = 0; bodyIndex < island->bodies.count; ++bodyIndex )+					{+						int islandBodyId = island->bodies.data[bodyIndex];+						b2Body* islandBody = b2Array_Get( world->bodies, islandBodyId );+						int shapeId = islandBody->headShapeId;+						while ( shapeId != B2_NULL_INDEX )+						{+							b2Shape* shape = b2Array_Get( world->shapes, shapeId );+							aabb = b2AABB_Union( aabb, world->fatAABBs.data[shapeId] );+							shapeCount += 1;+							shapeId = shape->nextShapeId;+						}+					}++					if ( shapeCount > 0 )+					{+						draw->DrawBoundsFcn( aabb, b2_colorOrangeRed, draw->context );+					}++					b2SetBit( &world->debugIslandSet, islandId );+				}+			}++			// Clear the smallest set bit+			word = word & ( word - 1 );+		}+	}+}++bool b2ComputeWorldBounds( b2World* world, b2AABB* bounds )+{+	b2AABB worldBounds = { 0 };+	bool haveBounds = false;++	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2DynamicTree* tree = world->broadPhase.trees + i;+		if ( b2DynamicTree_GetProxyCount( tree ) == 0 )+		{+			continue;+		}++		b2AABB treeBounds = b2DynamicTree_GetRootBounds( tree );+		worldBounds = haveBounds ? b2AABB_Union( worldBounds, treeBounds ) : treeBounds;+		haveBounds = true;+	}++	*bounds = worldBounds;+	return haveBounds;+}++b2AABB b2World_GetBounds( b2WorldId worldId )+{+	b2World* world = b2GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return (b2AABB){ 0 };+	}++	b2AABB bounds;+	b2ComputeWorldBounds( world, &bounds );+	return bounds;+}++b2BodyEvents b2World_GetBodyEvents( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return (b2BodyEvents){ 0 };+	}++	int count = world->bodyMoveEvents.count;+	b2BodyEvents events = { world->bodyMoveEvents.data, count };+	return events;+}++b2SensorEvents b2World_GetSensorEvents( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return (b2SensorEvents){ 0 };+	}++	// Careful to use previous buffer+	int endEventArrayIndex = 1 - world->endEventArrayIndex;++	int beginCount = world->sensorBeginEvents.count;+	int endCount = world->sensorEndEvents[endEventArrayIndex].count;++	b2SensorEvents events = {+		.beginEvents = world->sensorBeginEvents.data,+		.endEvents = world->sensorEndEvents[endEventArrayIndex].data,+		.beginCount = beginCount,+		.endCount = endCount,+	};+	return events;+}++b2ContactEvents b2World_GetContactEvents( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return (b2ContactEvents){ 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;++	b2ContactEvents events = {+		.beginEvents = world->contactBeginEvents.data,+		.endEvents = world->contactEndEvents[endEventArrayIndex].data,+		.hitEvents = world->contactHitEvents.data,+		.beginCount = beginCount,+		.endCount = endCount,+		.hitCount = hitCount,+	};++	return events;+}++b2JointEvents b2World_GetJointEvents( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return (b2JointEvents){ 0 };+	}++	int count = world->jointEvents.count;+	b2JointEvents events = { world->jointEvents.data, count };+	return events;+}++bool b2World_IsValid( b2WorldId id )+{+	if ( id.index1 < 1 || B2_MAX_WORLDS < id.index1 )+	{+		return false;+	}++	b2World* world = b2_worlds + ( id.index1 - 1 );++	if ( world->worldId != id.index1 - 1 )+	{+		// world is not allocated+		return false;+	}++	return id.generation == world->generation;+}++bool b2Body_IsValid( b2BodyId id )+{+	if ( B2_MAX_WORLDS <= id.world0 )+	{+		// invalid world+		return false;+	}++	b2World* world = b2_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;+	}++	b2Body* body = world->bodies.data + ( id.index1 - 1 );+	if ( body->setIndex == B2_NULL_INDEX )+	{+		// this was freed+		return false;+	}++	B2_ASSERT( body->localIndex != B2_NULL_INDEX );++	if ( body->generation != id.generation )+	{+		// this id is orphaned+		return false;+	}++	return true;+}++bool b2Shape_IsValid( b2ShapeId id )+{+	if ( B2_MAX_WORLDS <= id.world0 )+	{+		return false;+	}++	b2World* world = b2_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;+	}++	b2Shape* shape = world->shapes.data + shapeId;+	if ( shape->id == B2_NULL_INDEX )+	{+		// shape is free+		return false;+	}++	B2_ASSERT( shape->id == shapeId );++	return id.generation == shape->generation;+}++bool b2Chain_IsValid( b2ChainId id )+{+	if ( B2_MAX_WORLDS <= id.world0 )+	{+		return false;+	}++	b2World* world = b2_worlds + id.world0;+	if ( world->worldId != id.world0 )+	{+		// world is free+		return false;+	}++	int chainId = id.index1 - 1;+	if ( chainId < 0 || world->chainShapes.count <= chainId )+	{+		return false;+	}++	b2ChainShape* chain = world->chainShapes.data + chainId;+	if ( chain->id == B2_NULL_INDEX )+	{+		// chain is free+		return false;+	}++	B2_ASSERT( chain->id == chainId );++	return id.generation == chain->generation;+}++bool b2Joint_IsValid( b2JointId id )+{+	if ( B2_MAX_WORLDS <= id.world0 )+	{+		return false;+	}++	b2World* world = b2_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;+	}++	b2Joint* joint = world->joints.data + jointId;+	if ( joint->jointId == B2_NULL_INDEX )+	{+		// joint is free+		return false;+	}++	B2_ASSERT( joint->jointId == jointId );++	return id.generation == joint->generation;+}++bool b2Contact_IsValid( b2ContactId id )+{+	if ( B2_MAX_WORLDS <= id.world0 )+	{+		return false;+	}++	b2World* world = b2_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;+	}++	b2Contact* contact = world->contacts.data + contactId;+	if ( contact->contactId == B2_NULL_INDEX )+	{+		// contact is free+		return false;+	}++	B2_ASSERT( contact->contactId == contactId );++	return id.generation == contact->generation;+}++void b2World_EnableSleeping( b2WorldId worldId, bool flag )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldEnableSleeping, worldId, flag );++	if ( flag == world->enableSleep )+	{+		return;+	}++	world->enableSleep = flag;++	if ( flag == false )+	{+		int setCount = world->solverSets.count;+		for ( int i = b2_firstSleepingSet; i < setCount; ++i )+		{+			b2SolverSet* set = b2Array_Get( world->solverSets, i );+			if ( set->bodySims.count > 0 )+			{+				b2WakeSolverSet( world, i );+			}+		}+	}+}++bool b2World_IsSleepingEnabled( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->enableSleep;+}++void b2World_EnableWarmStarting( b2WorldId worldId, bool flag )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldEnableWarmStarting, worldId, flag );++	world->enableWarmStarting = flag;+}++bool b2World_IsWarmStartingEnabled( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->enableWarmStarting;+}++void b2World_EnableSSE2Fallback( b2WorldId worldId, bool flag )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	world->simdWidth = flag ? 4 : b2GetSIMDWidth();+}++int b2World_GetAwakeBodyCount( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	return awakeSet->bodySims.count;+}++void b2World_EnableContinuous( b2WorldId worldId, bool flag )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldEnableContinuous, worldId, flag );++	world->enableContinuous = flag;+}++bool b2World_IsContinuousEnabled( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->enableContinuous;+}++void b2World_SetRestitutionThreshold( b2WorldId worldId, float value )+{+	B2_CHECK_INPUT( b2IsValidFloat( value ) );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldSetRestitutionThreshold, worldId, value );++	world->restitutionThreshold = b2ClampFloat( value, 0.0f, FLT_MAX );+}++float b2World_GetRestitutionThreshold( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->restitutionThreshold;+}++void b2World_SetRestitutionIterations( b2WorldId worldId, int iterations )+{+	B2_CHECK_INPUT( iterations >= 0 );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldSetRestitutionIterations, worldId, iterations );++	world->restitutionIterations = b2ClampInt( iterations, 0, B2_MAX_RESTITUTION_ITERATIONS );+}++int b2World_GetRestitutionIterations( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->restitutionIterations;+}++void b2World_EnableRestitutionPropagation( b2WorldId worldId, bool flag )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldEnableRestitutionPropagation, worldId, flag );++	world->enableRestitutionPropagation = flag;+}++bool b2World_IsRestitutionPropagationEnabled( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->enableRestitutionPropagation;+}++void b2World_SetHitEventThreshold( b2WorldId worldId, float value )+{+	B2_CHECK_INPUT( b2IsValidFloat( value ) );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldSetHitEventThreshold, worldId, value );++	world->hitEventThreshold = b2ClampFloat( value, 0.0f, FLT_MAX );+}++float b2World_GetHitEventThreshold( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->hitEventThreshold;+}++void b2World_SetContactTuning( b2WorldId worldId, float hertz, float dampingRatio, float pushSpeed )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );+	B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) );+	B2_CHECK_INPUT( b2IsValidFloat( pushSpeed ) );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldSetContactTuning, worldId, hertz, dampingRatio, pushSpeed );++	world->contactHertz = b2ClampFloat( hertz, 0.0f, FLT_MAX );+	world->contactDampingRatio = b2ClampFloat( dampingRatio, 0.0f, FLT_MAX );+	world->contactSpeed = b2ClampFloat( pushSpeed, 0.0f, FLT_MAX );+}++void b2World_GetContactTuning( b2WorldId worldId, float* hertz, float* dampingRatio, float* pushSpeed )+{+	b2World* world = b2GetWorldFromId( worldId );+	*hertz = world->contactHertz;+	*dampingRatio = world->contactDampingRatio;+	*pushSpeed = world->contactSpeed;+}++void b2World_SetContactRecycleDistance( b2WorldId worldId, float recycleDistance )+{+	B2_CHECK_INPUT( b2IsValidFloat( recycleDistance ) );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldSetContactRecycleDistance, worldId, recycleDistance );++	world->contactRecycleDistance = b2ClampFloat( recycleDistance, 0.0f, FLT_MAX );+}++float b2World_GetContactRecycleDistance( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->contactRecycleDistance;+}++void b2World_SetMaxLinearSpeed( b2WorldId worldId, float maximumLinearSpeed )+{+	B2_CHECK_INPUT( b2IsValidFloat( maximumLinearSpeed ) && maximumLinearSpeed > 0.0f );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldSetMaxLinearSpeed, worldId, maximumLinearSpeed );++	world->maxLinearSpeed = maximumLinearSpeed;+}++float b2World_GetMaxLinearSpeed( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->maxLinearSpeed;+}++b2Profile b2World_GetProfile( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->profile;+}++b2Counters b2World_GetCounters( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	b2Counters s = { 0 };+	s.bodyCount = b2GetIdCount( &world->bodyIdPool );+	s.shapeCount = b2GetIdCount( &world->shapeIdPool );+	s.contactCount = b2GetIdCount( &world->contactIdPool );+	s.jointCount = b2GetIdCount( &world->jointIdPool );+	s.islandCount = b2GetIdCount( &world->islandIdPool );++	b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody;+	s.staticTreeHeight = b2DynamicTree_GetHeight( staticTree );++	b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody;+	b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody;+	s.treeHeight = b2MaxInt( b2DynamicTree_GetHeight( dynamicTree ), b2DynamicTree_GetHeight( kinematicTree ) );++	s.stackUsed = b2GetMaxStackAllocation( &world->stack );+	s.byteCount = b2GetByteCount();+	s.taskCount = world->taskCount;++	s.recycledContactCount = 0;+	for ( int i = 0; i < world->workerCount; ++i )+	{+		s.recycledContactCount += world->taskContexts.data[i].recycledContactCount;+	}++	s.awakeContactCount = 0;+	for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i )+	{+		b2GraphColor* color = world->constraintGraph.colors + i;+		s.colorCounts[i] = color->contactSims.count + color->jointSims.count;+		s.awakeContactCount += color->contactSims.count;+	}+	s.awakeContactCount += world->solverSets.data[b2_awakeSet].contactSims.count;++	return s;+}++b2Capacity b2World_GetMaxCapacity( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->maxCapacity;+}++void b2World_SetUserData( b2WorldId worldId, void* userData )+{+	b2World* world = b2GetWorldFromId( worldId );+	world->userData = userData;+}++void* b2World_GetUserData( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->userData;+}++void b2World_SetFrictionCallback( b2WorldId worldId, b2FrictionCallbackFcn* callback )+{+	b2World* world = b2GetWorldFromId( worldId );+	if ( world->locked )+	{+		return;+	}++	world->frictionCallback = callback;+}++void b2World_SetRestitutionCallback( b2WorldId worldId, b2RestitutionCallbackFcn* callback )+{+	b2World* world = b2GetWorldFromId( worldId );+	if ( world->locked )+	{+		return;+	}++	world->restitutionCallback = callback;+}++void b2World_SetWorkerCount( b2WorldId worldId, int count )+{+	b2World* world = b2GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return;+	}++	if ( count == world->workerCount )+	{+		return;+	}++	b2DestroyWorkerContexts( world );+	world->workerCount = b2ClampInt( count, 1, B2_MAX_WORKERS );+	b2CreateWorkerContexts( world );+}++int b2World_GetWorkerCount( b2WorldId worldId )+{+	b2World* world = b2GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return 0;+	}++	return world->workerCount;+}++void b2World_StartRecording( b2WorldId worldId, b2Recording* recording )+{+	// Must be a step boundary, so refuse a locked world+	b2World* world = b2GetUnlockedWorldFromId( worldId );++	if ( world == NULL || recording == NULL || world->recording != NULL )+	{+		return;+	}++	if ( world->preSolveFcn != NULL || world->preContinuousFcn != NULL || world->customFilterFcn != NULL )+	{+		printf( "b2World_StartRecording: preSolve and custom filtering are not supported when recording\n" );+		return;+	}++	b2StartRecordingIntoBuffer( world, recording );+}++void b2World_StopRecording( b2WorldId worldId )+{+	b2World* world = b2GetUnlockedWorldFromId( worldId );+	if ( world == NULL )+	{+		return;+	}++	b2StopRecordingInternal( world );+}++void b2World_DumpMemoryStats( b2WorldId worldId )+{+	FILE* file = fopen( "box2d_memory.txt", "w" );+	if ( file == NULL )+	{+		return;+	}++	b2World* world = b2GetWorldFromId( worldId );++	int total = 0;++	// id pools+	int bodyIdBytes = b2GetIdBytes( &world->bodyIdPool );+	int solverSetIdBytes = b2GetIdBytes( &world->solverSetIdPool );+	int jointIdBytes = b2GetIdBytes( &world->jointIdPool );+	int contactIdBytes = b2GetIdBytes( &world->contactIdPool );+	int islandIdBytes = b2GetIdBytes( &world->islandIdPool );+	int shapeIdBytes = b2GetIdBytes( &world->shapeIdPool );+	int chainIdBytes = b2GetIdBytes( &world->chainIdPool );+	total += bodyIdBytes + solverSetIdBytes + jointIdBytes + contactIdBytes + islandIdBytes + shapeIdBytes + chainIdBytes;++	fprintf( file, "id pools\n" );+	fprintf( file, "body ids: %d\n", bodyIdBytes );+	fprintf( file, "solver set ids: %d\n", solverSetIdBytes );+	fprintf( file, "joint ids: %d\n", jointIdBytes );+	fprintf( file, "contact ids: %d\n", contactIdBytes );+	fprintf( file, "island ids: %d\n", islandIdBytes );+	fprintf( file, "shape ids: %d\n", shapeIdBytes );+	fprintf( file, "chain ids: %d\n", chainIdBytes );+	fprintf( file, "\n" );++	// Islands own per-island body/contact/joint link arrays+	int islandLinkBytes = 0;+	for ( int i = 0; i < world->islands.count; ++i )+	{+		b2Island* island = world->islands.data + i;+		islandLinkBytes += b2Array_ByteCount( island->bodies );+		islandLinkBytes += b2Array_ByteCount( island->contacts );+		islandLinkBytes += b2Array_ByteCount( island->joints );+	}++	// world arrays+	int bodyArrayBytes = b2Array_ByteCount( world->bodies );+	int solverSetArrayBytes = b2Array_ByteCount( world->solverSets );+	int jointArrayBytes = b2Array_ByteCount( world->joints );+	int contactArrayBytes = b2Array_ByteCount( world->contacts );+	int islandArrayBytes = b2Array_ByteCount( world->islands );+	int shapeArrayBytes = b2Array_ByteCount( world->shapes ) + b2Array_ByteCount( world->fatAABBs );+	int chainArrayBytes = b2Array_ByteCount( world->chainShapes );+	int sensorArrayBytes = b2Array_ByteCount( world->sensors );+	total += bodyArrayBytes + solverSetArrayBytes + jointArrayBytes + contactArrayBytes + islandArrayBytes + islandLinkBytes ++			 shapeArrayBytes + chainArrayBytes + sensorArrayBytes;++	fprintf( file, "world arrays\n" );+	fprintf( file, "bodies: %d\n", bodyArrayBytes );+	fprintf( file, "solver sets: %d\n", solverSetArrayBytes );+	fprintf( file, "joints: %d\n", jointArrayBytes );+	fprintf( file, "contacts: %d\n", contactArrayBytes );+	fprintf( file, "islands: %d\n", islandArrayBytes );+	fprintf( file, "island links: %d\n", islandLinkBytes );+	fprintf( file, "shapes: %d\n", shapeArrayBytes );+	fprintf( file, "chains: %d\n", chainArrayBytes );+	fprintf( file, "sensors: %d\n", sensorArrayBytes );+	fprintf( file, "\n" );++	// Chain shapes own index and surface material arrays+	int chainDataBytes = 0;+	for ( int i = 0; i < world->chainShapes.count; ++i )+	{+		b2ChainShape* chain = world->chainShapes.data + i;+		if ( chain->id == B2_NULL_INDEX )+		{+			continue;+		}++		chainDataBytes += chain->segmentCount * (int)sizeof( int );+	}++	// Sensors own overlap tracking arrays. The sensor array is dense.+	int sensorOverlapBytes = 0;+	for ( int i = 0; i < world->sensors.count; ++i )+	{+		b2Sensor* sensor = world->sensors.data + i;+		sensorOverlapBytes += b2Array_ByteCount( sensor->hits );+		sensorOverlapBytes += b2Array_ByteCount( sensor->overlaps1 );+		sensorOverlapBytes += b2Array_ByteCount( sensor->overlaps2 );+	}+	total += chainDataBytes + sensorOverlapBytes;++	fprintf( file, "owned arrays\n" );+	fprintf( file, "chain data: %d\n", chainDataBytes );+	fprintf( file, "sensor overlaps: %d\n", sensorOverlapBytes );+	fprintf( file, "\n" );++	// broad-phase+	int staticTreeBytes = (int)b2DynamicTree_GetByteCount( world->broadPhase.trees + b2_staticBody );+	int kinematicTreeBytes = (int)b2DynamicTree_GetByteCount( world->broadPhase.trees + b2_kinematicBody );+	int dynamicTreeBytes = (int)b2DynamicTree_GetByteCount( world->broadPhase.trees + b2_dynamicBody );+	b2HashSet* pairSet = &world->broadPhase.pairSet;+	int pairSetBytes = b2GetHashSetBytes( pairSet );+	total += staticTreeBytes + kinematicTreeBytes + dynamicTreeBytes + pairSetBytes;++	fprintf( file, "broad-phase\n" );+	fprintf( file, "static tree: %d\n", staticTreeBytes );+	fprintf( file, "kinematic tree: %d\n", kinematicTreeBytes );+	fprintf( file, "dynamic tree: %d\n", dynamicTreeBytes );+	fprintf( file, "pairSet: %d (%u, %u)\n", pairSetBytes, pairSet->count, pairSet->capacity );+	fprintf( file, "\n" );++	// solver sets+	int bodySimCapacity = 0;+	int bodyStateCapacity = 0;+	int jointSimCapacity = 0;+	int contactSimCapacity = 0;+	int islandSimCapacity = 0;+	int solverSetCapacity = world->solverSets.count;+	for ( int i = 0; i < solverSetCapacity; ++i )+	{+		b2SolverSet* set = world->solverSets.data + i;+		if ( set->setIndex == B2_NULL_INDEX )+		{+			continue;+		}++		bodySimCapacity += set->bodySims.capacity;+		bodyStateCapacity += set->bodyStates.capacity;+		jointSimCapacity += set->jointSims.capacity;+		contactSimCapacity += set->contactSims.capacity;+		islandSimCapacity += set->islandSims.capacity;+	}++	int setBodySimBytes = bodySimCapacity * (int)sizeof( b2BodySim );+	int setBodyStateBytes = bodyStateCapacity * (int)sizeof( b2BodyState );+	int setJointSimBytes = jointSimCapacity * (int)sizeof( b2JointSim );+	int setContactSimBytes = contactSimCapacity * (int)sizeof( b2ContactSim );+	int setIslandSimBytes = islandSimCapacity * (int)sizeof( b2IslandSim );+	total += setBodySimBytes + setBodyStateBytes + setJointSimBytes + setContactSimBytes + setIslandSimBytes;++	fprintf( file, "solver sets\n" );+	fprintf( file, "body sim: %d\n", setBodySimBytes );+	fprintf( file, "body state: %d\n", setBodyStateBytes );+	fprintf( file, "joint sim: %d\n", setJointSimBytes );+	fprintf( file, "contact sim: %d\n", setContactSimBytes );+	fprintf( file, "island sim: %d\n", setIslandSimBytes );+	fprintf( file, "\n" );++	// constraint graph+	int bodyBitSetBytes = 0;+	contactSimCapacity = 0;+	jointSimCapacity = 0;+	for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i )+	{+		b2GraphColor* c = world->constraintGraph.colors + i;+		bodyBitSetBytes += b2GetBitSetBytes( &c->bodySet );+		contactSimCapacity += c->contactSims.capacity;+		jointSimCapacity += c->jointSims.capacity;+	}++	int graphJointSimBytes = jointSimCapacity * (int)sizeof( b2JointSim );+	int graphContactSimBytes = contactSimCapacity * (int)sizeof( b2ContactSim );+	total += bodyBitSetBytes + graphJointSimBytes + graphContactSimBytes;++	fprintf( file, "constraint graph\n" );+	fprintf( file, "body bit sets: %d\n", bodyBitSetBytes );+	fprintf( file, "joint sim: %d\n", graphJointSimBytes );+	fprintf( file, "contact sim: %d\n", graphContactSimBytes );+	fprintf( file, "\n" );++	// Per worker task storage and its bit sets+	int taskContextBytes = b2Array_ByteCount( world->taskContexts );+	for ( int i = 0; i < world->taskContexts.count; ++i )+	{+		b2TaskContext* taskContext = world->taskContexts.data + i;+		taskContextBytes += b2Array_ByteCount( taskContext->sensorHits );+		taskContextBytes += b2Array_ByteCount( taskContext->pairKeys );+		taskContextBytes += b2GetBitSetBytes( &taskContext->contactStateBitSet );+		taskContextBytes += b2GetBitSetBytes( &taskContext->hitEventBitSet );+		taskContextBytes += b2GetBitSetBytes( &taskContext->jointStateBitSet );+		taskContextBytes += b2GetBitSetBytes( &taskContext->enlargedSimBitSet );+		taskContextBytes += b2GetBitSetBytes( &taskContext->awakeIslandBitSet );+	}++	int sensorTaskContextBytes = b2Array_ByteCount( world->sensorTaskContexts );+	for ( int i = 0; i < world->sensorTaskContexts.count; ++i )+	{+		b2SensorTaskContext* taskContext = world->sensorTaskContexts.data + i;+		sensorTaskContextBytes += b2GetBitSetBytes( &taskContext->eventBits );+	}+	total += taskContextBytes + sensorTaskContextBytes;++	fprintf( file, "task contexts\n" );+	fprintf( file, "worker: %d\n", taskContextBytes );+	fprintf( file, "sensor: %d\n", sensorTaskContextBytes );+	fprintf( file, "\n" );++	// Double buffered event arrays+	int eventBytes = 0;+	eventBytes += b2Array_ByteCount( world->bodyMoveEvents );+	eventBytes += b2Array_ByteCount( world->sensorBeginEvents );+	eventBytes += b2Array_ByteCount( world->contactBeginEvents );+	eventBytes += b2Array_ByteCount( world->sensorEndEvents[0] );+	eventBytes += b2Array_ByteCount( world->sensorEndEvents[1] );+	eventBytes += b2Array_ByteCount( world->contactEndEvents[0] );+	eventBytes += b2Array_ByteCount( world->contactEndEvents[1] );+	eventBytes += b2Array_ByteCount( world->contactHitEvents );+	eventBytes += b2Array_ByteCount( world->jointEvents );+	total += eventBytes;++	fprintf( file, "events: %d\n\n", eventBytes );++	// Debug draw bit sets+	int debugBytes = 0;+	debugBytes += b2GetBitSetBytes( &world->debugBodySet );+	debugBytes += b2GetBitSetBytes( &world->debugJointSet );+	debugBytes += b2GetBitSetBytes( &world->debugContactSet );+	debugBytes += b2GetBitSetBytes( &world->debugIslandSet );+	total += debugBytes;++	fprintf( file, "debug draw: %d\n\n", debugBytes );++	// stack allocator+	total += world->stack.capacity;+	fprintf( file, "stack allocator: %d\n\n", world->stack.capacity );++	fprintf( file, "total: %d\n", total );++	fclose( file );+}++typedef struct WorldQueryContext+{+	b2World* world;+	b2OverlapResultCallbackFcn* fcn;+	b2QueryFilter filter;+	void* userContext;+} WorldQueryContext;++static bool TreeQueryCallback( int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;++	WorldQueryContext* worldContext = context;+	b2World* world = worldContext->world;++	b2Shape* shape = b2Array_Get( world->shapes, shapeId );++	if ( b2ShouldQueryCollide( shape->filter, worldContext->filter ) == false )+	{+		return true;+	}++	b2ShapeId id = { shapeId + 1, world->worldId, shape->generation };+	bool result = worldContext->fcn( id, worldContext->userContext );+	return result;+}++b2TreeStats b2World_OverlapAABB( b2WorldId worldId, b2Pos origin, b2AABB aabb, b2QueryFilter filter, b2OverlapResultCallbackFcn* fcn,+								 void* context )+{+	b2TreeStats treeStats = { 0 };++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return treeStats;+	}++	B2_ASSERT( b2IsValidPos( origin ) );+	B2_ASSERT( b2IsValidAABB( aabb ) );++	b2RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b2RecQueryBegin( &recWriter, context );+		recWriter.userFcn.overlapFcn = fcn;+		b2RecW_WORLDID( &recWriter.buf, worldId );+		b2RecW_POSITION( &recWriter.buf, origin );+		b2RecW_AABB( &recWriter.buf, aabb );+		b2RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b2RecReserveU32( &recWriter.buf );+		fcn = b2RecOverlapTrampoline;+		context = &recWriter;+	}++	// Lift to a world float box with outward rounding so the conservative tree test never misses+	b2AABB worldBox = b2OffsetAABB( aabb, origin );++	WorldQueryContext worldContext = { world, fcn, filter, context };++	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2TreeStats treeResult =+			b2DynamicTree_OverlapAABB( world->broadPhase.trees + i, worldBox, filter.maskBits, TreeQueryCallback, &worldContext );++		treeStats.nodeVisits += treeResult.nodeVisits;+		treeStats.leafVisits += treeResult.leafVisits;+	}++	if ( world->recording != NULL )+	{+		b2RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b2RecW_TREESTATS( &recWriter.buf, treeStats );+		b2RecQueryCommit( world->recording, 0xE0, &recWriter );+	}++	return treeStats;+}++typedef struct WorldOverlapContext+{+	b2World* world;+	b2OverlapResultCallbackFcn* fcn;+	b2QueryFilter filter;+	const b2ShapeProxy* proxy;+	b2Pos origin;+	void* userContext;+} WorldOverlapContext;++static bool TreeOverlapCallback( int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;++	WorldOverlapContext* worldContext = context;+	b2World* world = worldContext->world;++	b2Shape* shape = b2Array_Get( world->shapes, shapeId );++	if ( b2ShouldQueryCollide( shape->filter, worldContext->filter ) == false )+	{+		return true;+	}++	// Re-center on the query origin so the distance test stays in float precision far from the world origin+	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	b2Transform transform = b2ToRelativeTransform( b2GetBodyTransformQuick( world, body ), worldContext->origin );++	b2DistanceInput input;+	input.proxyA = *worldContext->proxy;+	input.proxyB = b2MakeShapeDistanceProxy( shape );+	input.transform = transform;+	input.useRadii = true;++	b2SimplexCache cache = { 0 };+	b2DistanceOutput output = b2ComputeShapeDistance( &input, &cache, NULL, 0 );++	float tolerance = 0.1f * B2_LINEAR_SLOP;+	if ( output.distance > tolerance )+	{+		return true;+	}++	b2ShapeId id = { shape->id + 1, world->worldId, shape->generation };+	bool result = worldContext->fcn( id, worldContext->userContext );+	return result;+}++b2TreeStats b2World_OverlapShape( b2WorldId worldId, b2Pos origin, const b2ShapeProxy* proxy, b2QueryFilter filter,+								  b2OverlapResultCallbackFcn* fcn, void* context )+{+	b2TreeStats treeStats = { 0 };++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return treeStats;+	}++	B2_ASSERT( b2IsValidPos( origin ) );++	b2RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b2RecQueryBegin( &recWriter, context );+		recWriter.userFcn.overlapFcn = fcn;+		b2RecW_WORLDID( &recWriter.buf, worldId );+		b2RecW_POSITION( &recWriter.buf, origin );+		b2RecW_SHAPEPROXY( &recWriter.buf, *proxy );+		b2RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b2RecReserveU32( &recWriter.buf );+		fcn = b2RecOverlapTrampoline;+		context = &recWriter;+	}++	// Relative box lifted to world float with outward rounding, conservative for the tree+	b2AABB aabb = b2OffsetAABB( b2MakeAABB( proxy->points, proxy->count, proxy->radius ), origin );+	WorldOverlapContext worldContext = {+		world, fcn, filter, proxy, origin, context,+	};++	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2TreeStats treeResult =+			b2DynamicTree_OverlapAABB( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext );++		treeStats.nodeVisits += treeResult.nodeVisits;+		treeStats.leafVisits += treeResult.leafVisits;+	}++	if ( world->recording != NULL )+	{+		b2RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b2RecW_TREESTATS( &recWriter.buf, treeStats );+		b2RecQueryCommit( world->recording, 0xE1, &recWriter );+	}++	return treeStats;+}++typedef struct WorldRayCastContext+{+	b2World* world;+	b2CastResultCallbackFcn* fcn;+	b2QueryFilter filter;+	float fraction;+	b2Pos origin;+	void* userContext;+} WorldRayCastContext;++static float RayCastCallback( const b2RayCastInput* input, int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;++	WorldRayCastContext* worldContext = context;+	b2World* world = worldContext->world;++	b2Shape* shape = b2Array_Get( world->shapes, shapeId );++	if ( b2ShouldQueryCollide( shape->filter, worldContext->filter ) == false )+	{+		return input->maxFraction;+	}++	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	b2WorldTransform bodyTransform = b2GetBodyTransformQuick( world, body );+	b2Transform transform = b2ToRelativeTransform( bodyTransform, worldContext->origin );++	b2RayCastInput localInput = *input;+	localInput.origin = b2Vec2_zero;+	b2CastOutput output = b2RayCastShape( &localInput, shape, transform );++	if ( output.hit )+	{+		b2ShapeId id = { shapeId + 1, world->worldId, shape->generation };+		b2Pos point = b2OffsetPos( worldContext->origin, output.point );+		float fraction = worldContext->fcn( id, point, output.normal, output.fraction, worldContext->userContext );++		// The user may return -1 to skip this shape+		if ( 0.0f <= fraction && fraction <= 1.0f )+		{+			worldContext->fraction = fraction;+		}++		return fraction;+	}++	return input->maxFraction;+}++b2TreeStats b2World_CastRay( b2WorldId worldId, b2Pos origin, b2Vec2 translation, b2QueryFilter filter, b2CastResultCallbackFcn* fcn,+							 void* context )+{+	b2TreeStats treeStats = { 0 };++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return treeStats;+	}++	B2_ASSERT( b2IsValidPos( origin ) );+	B2_ASSERT( b2IsValidVec2( translation ) );++	b2RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b2RecQueryBegin( &recWriter, context );+		recWriter.userFcn.castFcn = fcn;+		b2RecW_WORLDID( &recWriter.buf, worldId );+		b2RecW_POSITION( &recWriter.buf, origin );+		b2RecW_VEC2( &recWriter.buf, translation );+		b2RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b2RecReserveU32( &recWriter.buf );+		fcn = b2RecCastTrampoline;+		context = &recWriter;+	}++	// Tree traversal sees the origin truncated to float, displacing the ray by up to one+	// coordinate ULP, a graze sized miss tolerance at extreme range. Per-shape casts+	// re-difference against the full precision origin carried on the context.+	b2RayCastInput input = { b2ToVec2( origin ), translation, 1.0f };++	WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, origin, context };++	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2TreeStats treeResult =+			b2DynamicTree_CastRay( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext );+		treeStats.nodeVisits += treeResult.nodeVisits;+		treeStats.leafVisits += treeResult.leafVisits;++		if ( worldContext.fraction == 0.0f )+		{+			break;+		}++		input.maxFraction = worldContext.fraction;+	}++	if ( world->recording != NULL )+	{+		b2RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b2RecW_TREESTATS( &recWriter.buf, treeStats );+		b2RecQueryCommit( world->recording, 0xE2, &recWriter );+	}++	return treeStats;+}++// This callback finds the closest hit. This is the most common callback used in games.+static float b2RayCastClosestFcn( b2ShapeId shapeId, b2Pos point, b2Vec2 normal, float fraction, void* context )+{+	// Ignore initial overlap+	if ( fraction == 0.0f )+	{+		return -1.0f;+	}++	b2RayResult* rayResult = (b2RayResult*)context;+	rayResult->shapeId = shapeId;+	rayResult->point = point;+	rayResult->normal = normal;+	rayResult->fraction = fraction;+	rayResult->hit = true;+	return fraction;+}++b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Pos origin, b2Vec2 translation, b2QueryFilter filter )+{+	b2RayResult result = { 0 };++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return result;+	}++	B2_ASSERT( b2IsValidPos( origin ) );+	B2_ASSERT( b2IsValidVec2( translation ) );++	b2RayCastInput input = { b2ToVec2( origin ), translation, 1.0f };+	WorldRayCastContext worldContext = { world, b2RayCastClosestFcn, filter, 1.0f, origin, &result };++	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2TreeStats treeResult =+			b2DynamicTree_CastRay( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext );+		result.nodeVisits += treeResult.nodeVisits;+		result.leafVisits += treeResult.leafVisits;++		if ( worldContext.fraction == 0.0f )+		{+			break;+		}++		input.maxFraction = worldContext.fraction;+	}++	if ( world->recording != NULL )+	{+		b2RecBuffer recBuf = { 0 };+		b2RecW_WORLDID( &recBuf, worldId );+		b2RecW_POSITION( &recBuf, origin );+		b2RecW_VEC2( &recBuf, translation );+		b2RecW_QUERYFILTER( &recBuf, filter );+		b2RecW_RAYRESULT( &recBuf, result );+		b2RecCommitRecord( world->recording, 0xE5, recBuf.data, recBuf.size );+		b2RecBufFree( &recBuf );+	}++	return result;+}++typedef struct WorldShapeCastContext+{+	b2World* world;+	b2CastResultCallbackFcn* fcn;+	b2QueryFilter filter;+	float fraction;+	b2Pos origin;+	// origin relative input+	b2ShapeCastInput input;+	void* userContext;+} WorldShapeCastContext;++static float ShapeCastCallback( const b2BoxCastInput* input, int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;++	WorldShapeCastContext* worldContext = context;+	b2World* world = worldContext->world;++	b2Shape* shape = b2Array_Get( world->shapes, shapeId );++	if ( b2ShouldQueryCollide( shape->filter, worldContext->filter ) == false )+	{+		return input->maxFraction;+	}++	// Rebuild from the origin relative input, taking only the advancing fraction from the tree.+	// The tree input is world float and would lose the cast far from the origin.+	b2ShapeCastInput localInput = worldContext->input;+	localInput.maxFraction = input->maxFraction;++	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );+	b2Transform localTransform = b2ToRelativeTransform( transform, worldContext->origin );++	b2CastOutput output = b2ShapeCastShape( &localInput, shape, localTransform );++	if ( output.hit )+	{+		b2ShapeId id = { shapeId + 1, world->worldId, shape->generation };++		b2Pos point = b2OffsetPos( worldContext->origin, output.point );+		float fraction = worldContext->fcn( id, point, output.normal, output.fraction, worldContext->userContext );++		// The user may return -1 to skip this shape+		if ( 0.0f <= fraction && fraction <= 1.0f )+		{+			worldContext->fraction = fraction;+		}++		return fraction;+	}++	return input->maxFraction;+}++b2TreeStats b2World_CastShape( b2WorldId worldId, b2Pos origin, const b2ShapeProxy* proxy, b2Vec2 translation,+							   b2QueryFilter filter, b2CastResultCallbackFcn* fcn, void* context )+{+	b2TreeStats treeStats = { 0 };++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return treeStats;+	}++	B2_ASSERT( b2IsValidPos( origin ) );+	B2_ASSERT( b2IsValidVec2( translation ) );++	b2RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b2RecQueryBegin( &recWriter, context );+		recWriter.userFcn.castFcn = fcn;+		b2RecW_WORLDID( &recWriter.buf, worldId );+		b2RecW_POSITION( &recWriter.buf, origin );+		b2RecW_SHAPEPROXY( &recWriter.buf, *proxy );+		b2RecW_VEC2( &recWriter.buf, translation );+		b2RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b2RecReserveU32( &recWriter.buf );+		fcn = b2RecCastTrampoline;+		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.userContext = context;++	// Create a conservative world space box from the proxy and origin.+	b2AABB localBox = b2MakeAABB( proxy->points, proxy->count, proxy->radius );+	b2AABB box = b2OffsetAABB( localBox, origin );+	b2BoxCastInput treeInput = { box, translation, 1.0f };++	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2TreeStats treeResult =+			b2DynamicTree_CastBox( world->broadPhase.trees + i, &treeInput, filter.maskBits, ShapeCastCallback, &worldContext );+		treeStats.nodeVisits += treeResult.nodeVisits;+		treeStats.leafVisits += treeResult.leafVisits;++		if ( worldContext.fraction == 0.0f )+		{+			break;+		}++		treeInput.maxFraction = worldContext.fraction;+	}++	if ( world->recording != NULL )+	{+		b2RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		b2RecW_TREESTATS( &recWriter.buf, treeStats );+		b2RecQueryCommit( world->recording, 0xE3, &recWriter );+	}++	return treeStats;+}++typedef struct WorldMoverCastContext+{+	b2World* world;+	b2QueryFilter filter;+	float fraction;+	b2Pos origin;+	// origin relative input+	b2ShapeCastInput input;+} WorldMoverCastContext;++static float MoverCastCallback( const b2BoxCastInput* input, int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;+	WorldMoverCastContext* worldContext = context;+	b2World* world = worldContext->world;++	b2Shape* shape = b2Array_Get( world->shapes, shapeId );++	if ( b2ShouldQueryCollide( shape->filter, worldContext->filter ) == false )+	{+		return worldContext->fraction;+	}++	// Rebuild from the origin relative input, taking only the advancing fraction from the tree+	b2ShapeCastInput localInput = worldContext->input;+	localInput.maxFraction = input->maxFraction;++	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	b2WorldTransform bodyTransform = b2GetBodyTransformQuick( world, body );+	b2Transform transform = b2ToRelativeTransform( bodyTransform, worldContext->origin );++	b2CastOutput output = b2ShapeCastShape( &localInput, shape, transform );+	if ( output.fraction == 0.0f )+	{+		// Ignore overlapping shapes+		return worldContext->fraction;+	}++	worldContext->fraction = output.fraction;+	return output.fraction;+}++float b2World_CastMover( b2WorldId worldId, b2Pos origin, const b2Capsule* mover, b2Vec2 translation, b2QueryFilter filter )+{+	B2_ASSERT( b2IsValidPos( origin ) );+	B2_ASSERT( b2IsValidVec2( translation ) );+	B2_ASSERT( mover->radius > 2.0f * B2_LINEAR_SLOP );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return 1.0f;+	}++	WorldMoverCastContext worldContext = { 0 };+	worldContext.world = world;+	worldContext.filter = filter;+	worldContext.fraction = 1.0f;+	worldContext.origin = origin;+	worldContext.input.proxy.points[0] = mover->center1;+	worldContext.input.proxy.points[1] = mover->center2;+	worldContext.input.proxy.count = 2;+	worldContext.input.proxy.radius = mover->radius;+	worldContext.input.translation = translation;+	worldContext.input.maxFraction = 1.0f;+	worldContext.input.canEncroach = true;++	// Bound the capsule in origin relative space then lift to a conservative world float box+	b2Vec2 centers[2] = { mover->center1, mover->center2 };+	b2AABB box = b2OffsetAABB( b2MakeAABB( centers, 2, mover->radius ), origin );+	b2BoxCastInput treeInput = { box, translation, 1.0f };++	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2DynamicTree_CastBox( world->broadPhase.trees + i, &treeInput, filter.maskBits, MoverCastCallback, &worldContext );++		if ( worldContext.fraction == 0.0f )+		{+			break;+		}++		treeInput.maxFraction = worldContext.fraction;+	}++	if ( world->recording != NULL )+	{+		b2RecBuffer recBuf = { 0 };+		b2RecW_WORLDID( &recBuf, worldId );+		b2RecW_POSITION( &recBuf, origin );+		b2RecW_CAPSULE( &recBuf, *mover );+		b2RecW_VEC2( &recBuf, translation );+		b2RecW_QUERYFILTER( &recBuf, filter );+		b2RecW_F32( &recBuf, worldContext.fraction );+		b2RecCommitRecord( world->recording, 0xE6, recBuf.data, recBuf.size );+		b2RecBufFree( &recBuf );+	}++	return worldContext.fraction;+}++typedef struct WorldMoverContext+{+	b2World* world;+	b2PlaneResultCallbackFcn* fcn;+	b2QueryFilter filter;+	b2Capsule mover;+	b2Pos origin;+	void* userContext;+} WorldMoverContext;++static bool TreeCollideCallback( int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;+	WorldMoverContext* worldContext = (WorldMoverContext*)context;+	b2World* world = worldContext->world;++	b2Shape* shape = b2Array_Get( world->shapes, shapeId );++	if ( b2ShouldQueryCollide( shape->filter, worldContext->filter ) == false )+	{+		return true;+	}++	// Re-center on the query origin, the mover and the resulting planes are origin relative+	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	b2Transform transform = b2ToRelativeTransform( b2GetBodyTransformQuick( world, body ), worldContext->origin );++	b2PlaneResult result = b2CollideMover( &worldContext->mover, shape, transform );++	// todo handle deep overlap+	if ( result.hit && b2IsNormalized( result.plane.normal ) )+	{+		b2ShapeId id = { shape->id + 1, world->worldId, shape->generation };+		return worldContext->fcn( id, &result, 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 b2World_CollideMover( b2WorldId worldId, b2Pos origin, const b2Capsule* mover, b2QueryFilter filter, b2PlaneResultCallbackFcn* fcn,+						   void* context )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_ASSERT( b2IsValidPos( origin ) );++	b2RecQueryWriter recWriter = { 0 };+	if ( world->recording != NULL )+	{+		b2RecQueryBegin( &recWriter, context );+		recWriter.userFcn.planeFcn = fcn;+		b2RecW_WORLDID( &recWriter.buf, worldId );+		b2RecW_POSITION( &recWriter.buf, origin );+		b2RecW_CAPSULE( &recWriter.buf, *mover );+		b2RecW_QUERYFILTER( &recWriter.buf, filter );+		recWriter.countOffset = b2RecReserveU32( &recWriter.buf );+		fcn = b2RecPlaneTrampoline;+		context = &recWriter;+	}++	b2Vec2 r = { mover->radius, mover->radius };++	// Relative box lifted to world float with outward rounding, conservative for the tree+	b2AABB relBox;+	relBox.lowerBound = b2Sub( b2Min( mover->center1, mover->center2 ), r );+	relBox.upperBound = b2Add( b2Max( mover->center1, mover->center2 ), r );+	b2AABB aabb = b2OffsetAABB( relBox, origin );++	WorldMoverContext worldContext = {+		world, fcn, filter, *mover, origin, context,+	};++	for ( int i = 0; i < b2_bodyTypeCount; ++i )+	{+		b2DynamicTree_OverlapAABB( world->broadPhase.trees + i, aabb, filter.maskBits, TreeCollideCallback, &worldContext );+	}++	if ( world->recording != NULL )+	{+		b2RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount );+		// CollideMover returns void; no TREESTATS tail+		b2RecQueryCommit( world->recording, 0xE4, &recWriter );+	}+}++void b2World_SetCustomFilterCallback( b2WorldId worldId, b2CustomFilterCallbackFcn* fcn, void* context )+{+	b2World* world = b2GetWorldFromId( worldId );+	if ( fcn != NULL && world->recording != NULL )+	{+		printf( "b2World_SetCustomFilterCallback: customFilter not supported while recording\n" );+		B2_ASSERT( false && "customFilter callbacks are not supported while recording" );+		return;+	}++	world->customFilterFcn = fcn;+	world->customFilterContext = context;+}++void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveCallbackFcn* preSolveFcn, b2PreContinuousCallbackFcn* preContinuousFcn,+								  void* context )+{+	b2World* world = b2GetWorldFromId( worldId );+	if ( ( preSolveFcn != NULL || preContinuousFcn != NULL ) && world->recording != NULL )+	{+		printf( "b2World_SetPreSolveCallback: preSolve not supported while recording\n" );+		B2_ASSERT( false && "preSolve callbacks are not supported while recording" );+		return;+	}++	world->preSolveFcn = preSolveFcn;+	world->preContinuousFcn = preContinuousFcn;+	world->preSolveContext = context;+}++void b2World_SetGravity( b2WorldId worldId, b2Vec2 gravity )+{+	B2_CHECK_INPUT( b2IsValidVec2( gravity ) );++	b2World* world = b2GetWorldFromId( worldId );+	B2_REC( world, WorldSetGravity, worldId, gravity );+	world->gravity = gravity;+}++b2Vec2 b2World_GetGravity( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return world->gravity;+}++struct ExplosionContext+{+	b2World* world;+	b2Pos position;+	float radius;+	float falloff;+	float impulsePerLength;+};++static bool ExplosionCallback( int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;+	struct ExplosionContext* explosionContext = context;+	b2World* world = explosionContext->world;++	b2Shape* shape = b2Array_Get( world->shapes, shapeId );++	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	B2_ASSERT( body->type == b2_dynamicBody );++	b2WorldTransform xf = b2GetBodyTransformQuick( 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.+	b2Vec2 localPosition = b2InvTransformWorldPoint( xf, explosionContext->position );++	b2DistanceInput input;+	input.proxyA = b2MakeShapeDistanceProxy( shape );+	input.proxyB = b2MakeProxy( &localPosition, 1, 0.0f );+	input.transform = b2Transform_identity;+	input.useRadii = true;++	b2SimplexCache cache = { 0 };+	b2DistanceOutput output = b2ComputeShapeDistance( &input, &cache, NULL, 0 );++	float radius = explosionContext->radius;+	float falloff = explosionContext->falloff;+	if ( output.distance > radius + falloff )+	{+		return true;+	}++	b2WakeBody( world, body );++	if ( body->setIndex != b2_awakeSet )+	{+		return true;+	}++	b2Vec2 closestPoint = output.pointA;+	if ( output.distance == 0.0f )+	{+		closestPoint = b2GetShapeCentroid( shape );+	}++	b2Vec2 direction = b2Sub( closestPoint, localPosition );+	if ( b2LengthSquared( direction ) > 100.0f * FLT_EPSILON * FLT_EPSILON )+	{+		direction = b2Normalize( direction );+	}+	else+	{+		direction = (b2Vec2){ 1.0f, 0.0f };+	}++	b2Vec2 localLine = b2LeftPerp( direction );+	float perimeter = b2GetShapeProjectedPerimeter( shape, localLine );+	float scale = 1.0f;+	if ( output.distance > radius && falloff > 0.0f )+	{+		scale = b2ClampFloat( ( radius + falloff - output.distance ) / falloff, 0.0f, 1.0f );+	}++	float magnitude = explosionContext->impulsePerLength * perimeter * scale;+	b2Vec2 impulse = b2MulSV( magnitude, b2RotateVector( xf.q, direction ) );++	int localIndex = body->localIndex;+	b2SolverSet* set = b2Array_Get( world->solverSets, b2_awakeSet );+	b2BodyState* state = b2Array_Get( set->bodyStates, localIndex );+	b2BodySim* bodySim = b2Array_Get( set->bodySims, localIndex );+	state->linearVelocity = b2MulAdd( state->linearVelocity, bodySim->invMass, impulse );++	// Lever arm from the center of mass to the closest point, rotated to world+	b2Vec2 r = b2RotateVector( xf.q, b2Sub( closestPoint, bodySim->localCenter ) );+	state->angularVelocity += bodySim->invInertia * b2Cross( r, impulse );++	return true;+}++void b2World_Explode( b2WorldId worldId, const b2ExplosionDef* explosionDef )+{+	uint64_t maskBits = explosionDef->maskBits;+	b2Pos position = explosionDef->position;+	float radius = explosionDef->radius;+	float falloff = explosionDef->falloff;+	float impulsePerLength = explosionDef->impulsePerLength;++	B2_CHECK_INPUT( b2IsValidPos( position ) );+	B2_CHECK_INPUT( b2IsValidFloat( radius ) && radius >= 0.0f );+	B2_CHECK_INPUT( b2IsValidFloat( falloff ) && falloff >= 0.0f );+	B2_CHECK_INPUT( b2IsValidFloat( impulsePerLength ) );++	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldExplode, worldId, *explosionDef );++	struct ExplosionContext explosionContext = { world, position, radius, falloff, impulsePerLength };++	// The broad-phase tree is float, so translate a local query box out to world with outward rounding+	float extent = radius + falloff;+	b2AABB localBox = { { -extent, -extent }, { extent, extent } };+	b2AABB aabb = b2OffsetAABB( localBox, position );++	b2DynamicTree_OverlapAABB( world->broadPhase.trees + b2_dynamicBody, aabb, maskBits, ExplosionCallback, &explosionContext );+}++// This is for internal testing. Not optimized.+void b2World_RebuildStaticTree( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, WorldRebuildStaticTree, worldId );++	b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody;++	// Moved proxies must retain the flag in the rebuild.+	int movedCount = 0;+	int* movedProxies = NULL;+	int proxyCount = b2DynamicTree_GetProxyCount( staticTree );+	if ( b2HasTreeMoved( staticTree ) )+	{+		movedProxies = b2Alloc( proxyCount * sizeof( int ) );+		movedCount = b2DynamicTree_GatherMovedProxies( staticTree, movedProxies );+	}++	b2DynamicTree_Rebuild( staticTree, true );++	for ( int i = 0; i < movedCount; ++i )+	{+		b2DynamicTree_MarkProxyMovedSerial( staticTree, movedProxies[i] );+	}++	if ( movedProxies != NULL )+	{+		b2Free( movedProxies, proxyCount * sizeof( int ) );+	}+}++#if B2_ENABLE_VALIDATION+// This validates island graph connectivity for each body+void b2ValidateConnectivity( b2World* world )+{+	b2Body* bodies = world->bodies.data;+	int bodyCapacity = world->bodies.count;++	for ( int bodyIndex = 0; bodyIndex < bodyCapacity; ++bodyIndex )+	{+		b2Body* body = bodies + bodyIndex;+		if ( body->id == B2_NULL_INDEX )+		{+			b2ValidateFreeId( &world->bodyIdPool, bodyIndex );+			continue;+		}++		b2ValidateUsedId( &world->bodyIdPool, bodyIndex );++		B2_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 != B2_NULL_INDEX )+		{+			int contactId = contactKey >> 1;+			int edgeIndex = contactKey & 1;++			b2Contact* contact = b2Array_Get( world->contacts, contactId );++			bool touching = ( contact->flags & b2_contactTouchingFlag ) != 0;+			if ( touching )+			{+				if ( bodySetIndex != b2_staticSet )+				{+					int contactIslandId = contact->islandId;+					B2_ASSERT( contactIslandId == bodyIslandId );+				}+			}+			else+			{+				B2_ASSERT( contact->islandId == B2_NULL_INDEX );+			}++			contactKey = contact->edges[edgeIndex].nextKey;+		}++		int jointKey = body->headJointKey;+		while ( jointKey != B2_NULL_INDEX )+		{+			int jointId = jointKey >> 1;+			int edgeIndex = jointKey & 1;++			b2Joint* joint = b2Array_Get( world->joints, jointId );++			int otherEdgeIndex = edgeIndex ^ 1;++			b2Body* otherBody = b2Array_Get( world->bodies, joint->edges[otherEdgeIndex].bodyId );++			if ( bodySetIndex == b2_disabledSet || otherBody->setIndex == b2_disabledSet )+			{+				B2_ASSERT( joint->islandId == B2_NULL_INDEX );+			}+			else if ( bodySetIndex == b2_staticSet )+			{+				// Intentional nesting+				if ( otherBody->setIndex == b2_staticSet )+				{+					B2_ASSERT( joint->islandId == B2_NULL_INDEX );+				}+			}+			else if ( body->type != b2_dynamicBody && otherBody->type != b2_dynamicBody )+			{+				B2_ASSERT( joint->islandId == B2_NULL_INDEX );+			}+			else+			{+				int jointIslandId = joint->islandId;+				B2_ASSERT( jointIslandId == bodyIslandId );+			}++			jointKey = joint->edges[edgeIndex].nextKey;+		}+	}+}++// Validates solver sets, but not island connectivity+void b2ValidateSolverSets( b2World* world )+{+	B2_ASSERT( b2GetIdCapacity( &world->bodyIdPool ) == world->bodies.count );+	B2_ASSERT( b2GetIdCapacity( &world->contactIdPool ) == world->contacts.count );+	B2_ASSERT( b2GetIdCapacity( &world->jointIdPool ) == world->joints.count );+	B2_ASSERT( b2GetIdCapacity( &world->islandIdPool ) == world->islands.count );+	B2_ASSERT( b2GetIdCapacity( &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 )+	{+		b2SolverSet* set = world->solverSets.data + setIndex;+		if ( set->setIndex != B2_NULL_INDEX )+		{+			activeSetCount += 1;++			if ( setIndex == b2_staticSet )+			{+				B2_ASSERT( set->contactSims.count == 0 );+				B2_ASSERT( set->islandSims.count == 0 );+				B2_ASSERT( set->bodyStates.count == 0 );+			}+			else if ( setIndex == b2_disabledSet )+			{+				B2_ASSERT( set->islandSims.count == 0 );+				B2_ASSERT( set->bodyStates.count == 0 );+			}+			else if ( setIndex == b2_awakeSet )+			{+				B2_ASSERT( set->bodySims.count == set->bodyStates.count );+				B2_ASSERT( set->jointSims.count == 0 );+			}+			else+			{+				B2_ASSERT( set->bodyStates.count == 0 );+			}++			// Validate bodies+			{+				b2Body* bodies = world->bodies.data;+				B2_ASSERT( set->bodySims.count >= 0 );+				totalBodyCount += set->bodySims.count;+				for ( int i = 0; i < set->bodySims.count; ++i )+				{+					b2BodySim* bodySim = set->bodySims.data + i;++					int bodyId = bodySim->bodyId;+					B2_ASSERT( 0 <= bodyId && bodyId < world->bodies.count );+					b2Body* body = bodies + bodyId;+					B2_ASSERT( body->setIndex == setIndex );+					B2_ASSERT( body->localIndex == i );++					uint32_t syncedFlags = body->flags & ~( b2_isFast | b2_bodyTransientFlags );+					B2_ASSERT( ( bodySim->flags & syncedFlags ) == syncedFlags );++					b2BodyState* bodyState = b2GetBodyState( world, body );+					if ( bodyState != NULL )+					{+						B2_ASSERT( ( bodyState->flags & syncedFlags ) == syncedFlags );+					}++					if ( body->type == b2_dynamicBody )+					{+						B2_ASSERT( body->flags & b2_dynamicFlag );+					}++					if ( setIndex == b2_disabledSet )+					{+						B2_ASSERT( body->headContactKey == B2_NULL_INDEX );+					}++					// Validate body shapes+					int prevShapeId = B2_NULL_INDEX;+					int shapeId = body->headShapeId;+					while ( shapeId != B2_NULL_INDEX )+					{+						b2Shape* shape = b2Array_Get( world->shapes, shapeId );+						B2_ASSERT( shape->id == shapeId );+						B2_ASSERT( shape->prevShapeId == prevShapeId );++						if ( setIndex == b2_disabledSet )+						{+							B2_ASSERT( shape->proxyKey == B2_NULL_INDEX );+						}+						else if ( setIndex == b2_staticSet )+						{+							B2_ASSERT( B2_PROXY_TYPE( shape->proxyKey ) == b2_staticBody );+						}+						else+						{+							b2BodyType proxyType = B2_PROXY_TYPE( shape->proxyKey );+							B2_ASSERT( proxyType == b2_kinematicBody || proxyType == b2_dynamicBody );+						}++						prevShapeId = shapeId;+						shapeId = shape->nextShapeId;+					}++					// Validate body contacts+					int contactKey = body->headContactKey;+					while ( contactKey != B2_NULL_INDEX )+					{+						int contactId = contactKey >> 1;+						int edgeIndex = contactKey & 1;++						b2Contact* contact = b2Array_Get( world->contacts, contactId );+						B2_ASSERT( contact->setIndex != b2_staticSet );+						B2_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 != B2_NULL_INDEX )+					{+						int jointId = jointKey >> 1;+						int edgeIndex = jointKey & 1;++						b2Joint* joint = b2Array_Get( world->joints, jointId );++						int otherEdgeIndex = edgeIndex ^ 1;++						b2Body* otherBody = b2Array_Get( world->bodies, joint->edges[otherEdgeIndex].bodyId );++						if ( setIndex == b2_disabledSet || otherBody->setIndex == b2_disabledSet )+						{+							B2_ASSERT( joint->setIndex == b2_disabledSet );+						}+						else if ( setIndex == b2_staticSet && otherBody->setIndex == b2_staticSet )+						{+							B2_ASSERT( joint->setIndex == b2_staticSet );+						}+						else if ( body->type != b2_dynamicBody && otherBody->type != b2_dynamicBody )+						{+							B2_ASSERT( joint->setIndex == b2_staticSet );+						}+						else if ( setIndex == b2_awakeSet )+						{+							B2_ASSERT( joint->setIndex == b2_awakeSet );+						}+						else if ( setIndex >= b2_firstSleepingSet )+						{+							B2_ASSERT( joint->setIndex == setIndex );+						}++						b2JointSim* jointSim = b2GetJointSim( world, joint );+						B2_ASSERT( jointSim->jointId == jointId );+						B2_ASSERT( jointSim->bodyIdA == joint->edges[0].bodyId );+						B2_ASSERT( jointSim->bodyIdB == joint->edges[1].bodyId );++						jointKey = joint->edges[edgeIndex].nextKey;+					}+				}+			}++			// Validate contacts+			{+				B2_ASSERT( set->contactSims.count >= 0 );+				totalContactCount += set->contactSims.count;+				for ( int i = 0; i < set->contactSims.count; ++i )+				{+					b2ContactSim* contactSim = set->contactSims.data + i;+					b2Contact* contact = b2Array_Get( world->contacts, contactSim->contactId );+					if ( setIndex == b2_awakeSet )+					{+						// contact should be non-touching if awake+						// or it could be this contact hasn't been transferred yet+						B2_ASSERT( contactSim->manifold.pointCount == 0 ||+								   ( contactSim->simFlags & b2_simStartedTouching ) != 0 );+					}+					B2_ASSERT( contact->setIndex == setIndex );+					B2_ASSERT( contact->colorIndex == B2_NULL_INDEX );+					B2_ASSERT( contact->localIndex == i );++					b2Body* edgeBodyA = b2Array_Get( world->bodies, contact->edges[0].bodyId );+					b2Body* edgeBodyB = b2Array_Get( world->bodies, contact->edges[1].bodyId );+					B2_ASSERT( contactSim->encodedBodySimA == b2EncodeBodySimIndex( edgeBodyA ) );+					B2_ASSERT( contactSim->encodedBodySimB == b2EncodeBodySimIndex( edgeBodyB ) );+				}+			}++			// Validate joints+			{+				B2_ASSERT( set->jointSims.count >= 0 );+				totalJointCount += set->jointSims.count;+				for ( int i = 0; i < set->jointSims.count; ++i )+				{+					b2JointSim* jointSim = set->jointSims.data + i;+					b2Joint* joint = b2Array_Get( world->joints, jointSim->jointId );+					B2_ASSERT( joint->setIndex == setIndex );+					B2_ASSERT( joint->colorIndex == B2_NULL_INDEX );+					B2_ASSERT( joint->localIndex == i );+				}+			}++			// Validate islands+			{+				B2_ASSERT( set->islandSims.count >= 0 );+				totalIslandCount += set->islandSims.count;+				for ( int i = 0; i < set->islandSims.count; ++i )+				{+					b2IslandSim* islandSim = set->islandSims.data + i;+					b2Island* island = b2Array_Get( world->islands, islandSim->islandId );+					B2_ASSERT( island->setIndex == setIndex );+					B2_ASSERT( island->localIndex == i );+				}+			}+		}+		else+		{+			B2_ASSERT( set->bodySims.count == 0 );+			B2_ASSERT( set->contactSims.count == 0 );+			B2_ASSERT( set->jointSims.count == 0 );+			B2_ASSERT( set->islandSims.count == 0 );+			B2_ASSERT( set->bodyStates.count == 0 );+		}+	}++	int setIdCount = b2GetIdCount( &world->solverSetIdPool );+	B2_ASSERT( activeSetCount == setIdCount );++	int bodyIdCount = b2GetIdCount( &world->bodyIdPool );+	B2_ASSERT( totalBodyCount == bodyIdCount );++	int islandIdCount = b2GetIdCount( &world->islandIdPool );+	B2_ASSERT( totalIslandCount == islandIdCount );++	// Validate constraint graph+	for ( int colorIndex = 0; colorIndex < B2_GRAPH_COLOR_COUNT; ++colorIndex )+	{+		b2GraphColor* color = world->constraintGraph.colors + colorIndex;+		int bitCount = 0;++		B2_ASSERT( color->contactSims.count >= 0 );++		totalContactCount += color->contactSims.count;+		for ( int i = 0; i < color->contactSims.count; ++i )+		{+			b2ContactSim* contactSim = color->contactSims.data + i;+			b2Contact* contact = b2Array_Get( world->contacts, contactSim->contactId );+			// contact should be touching in the constraint graph or awaiting transfer to non-touching+			B2_ASSERT( contactSim->manifold.pointCount > 0 ||+					   ( contactSim->simFlags & ( b2_simStoppedTouching | b2_simDisjoint ) ) != 0 );+			B2_ASSERT( contact->setIndex == b2_awakeSet );+			B2_ASSERT( contact->colorIndex == colorIndex );+			B2_ASSERT( contact->localIndex == i );++			int bodyIdA = contact->edges[0].bodyId;+			int bodyIdB = contact->edges[1].bodyId;++			b2Body* edgeBodyA = b2Array_Get( world->bodies, bodyIdA );+			b2Body* edgeBodyB = b2Array_Get( world->bodies, bodyIdB );+			B2_ASSERT( contactSim->encodedBodySimA == b2EncodeBodySimIndex( edgeBodyA ) );+			B2_ASSERT( contactSim->encodedBodySimB == b2EncodeBodySimIndex( edgeBodyB ) );++			if ( colorIndex < B2_OVERFLOW_INDEX )+			{+				B2_ASSERT( b2GetBit( &color->bodySet, bodyIdA ) == ( edgeBodyA->type == b2_dynamicBody ) );+				B2_ASSERT( b2GetBit( &color->bodySet, bodyIdB ) == ( edgeBodyB->type == b2_dynamicBody ) );++				bitCount += edgeBodyA->type == b2_dynamicBody ? 1 : 0;+				bitCount += edgeBodyB->type == b2_dynamicBody ? 1 : 0;+			}+		}++		B2_ASSERT( color->jointSims.count >= 0 );+		totalJointCount += color->jointSims.count;+		for ( int i = 0; i < color->jointSims.count; ++i )+		{+			b2JointSim* jointSim = color->jointSims.data + i;+			b2Joint* joint = b2Array_Get( world->joints, jointSim->jointId );+			B2_ASSERT( joint->setIndex == b2_awakeSet );+			B2_ASSERT( joint->colorIndex == colorIndex );+			B2_ASSERT( joint->localIndex == i );++			int bodyIdA = joint->edges[0].bodyId;+			int bodyIdB = joint->edges[1].bodyId;++			if ( colorIndex < B2_OVERFLOW_INDEX )+			{+				b2Body* bodyA = b2Array_Get( world->bodies, bodyIdA );+				b2Body* bodyB = b2Array_Get( world->bodies, bodyIdB );+				B2_ASSERT( b2GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b2_dynamicBody ) );+				B2_ASSERT( b2GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b2_dynamicBody ) );++				bitCount += bodyA->type == b2_dynamicBody ? 1 : 0;+				bitCount += bodyB->type == b2_dynamicBody ? 1 : 0;+			}+		}++		// Validate the bit population for this graph color+		B2_ASSERT( bitCount == b2CountSetBits( &color->bodySet ) );+	}++	int contactIdCount = b2GetIdCount( &world->contactIdPool );+	B2_ASSERT( totalContactCount == contactIdCount );+	B2_ASSERT( totalContactCount == (int)world->broadPhase.pairSet.count );++	int jointIdCount = b2GetIdCount( &world->jointIdPool );+	B2_ASSERT( totalJointCount == jointIdCount );++// Validate shapes+// This is very slow on compounds+#if 0+	int shapeCapacity = world->shapes.count;+	for (int shapeIndex = 0; shapeIndex < shapeCapacity; shapeIndex += 1)+	{+		b2Shape* shape = world->shapes.data + shapeIndex;+		if (shape->id != shapeIndex)+		{+			continue;+		}++		b2Body* body = b2Array_Get(world->bodies, shape->bodyId);++		b2SolverSet* set = b2Array_Get(world->solverSets, body->setIndex);+		b2BodySim* bodySim = b2Array_Get(set->bodySims, body->localIndex);+		B2_ASSERT(bodySim->bodyId == shape->bodyId);++		bool found = false;+		int shapeCount = 0;+		int index = body->headShapeId;+		while (index != B2_NULL_INDEX)+		{+			b2Shape* s = b2Array_Get(world->shapes, index);+			if (index == shapeIndex)+			{+				found = true;+			}++			index = s->nextShapeId;+			shapeCount += 1;+		}++		B2_ASSERT(found);+		B2_ASSERT(shapeCount == body->shapeCount);+	}+#endif+}++// Validate contact touching status.+void b2ValidateContacts( b2World* world )+{+	int contactCount = world->contacts.count;+	B2_ASSERT( contactCount == b2GetIdCapacity( &world->contactIdPool ) );+	int allocatedContactCount = 0;++	for ( int contactIndex = 0; contactIndex < contactCount; ++contactIndex )+	{+		b2Contact* contact = b2Array_Get( world->contacts, contactIndex );+		if ( contact->contactId == B2_NULL_INDEX )+		{+			continue;+		}++		B2_ASSERT( contact->contactId == contactIndex );++		allocatedContactCount += 1;++		bool touching = ( contact->flags & b2_contactTouchingFlag ) != 0;++		int setId = contact->setIndex;++		if ( setId == b2_awakeSet )+		{+			if ( touching )+			{+				B2_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B2_GRAPH_COLOR_COUNT );+			}+			else+			{+				B2_ASSERT( contact->colorIndex == B2_NULL_INDEX );+			}+		}+		else if ( setId >= b2_firstSleepingSet )+		{+			// Only touching contacts allowed in a sleeping set+			B2_ASSERT( touching == true );+		}+		else+		{+			// Sleeping and non-touching contacts belong in the disabled set+			B2_ASSERT( touching == false && setId == b2_disabledSet );+		}++		b2ContactSim* contactSim = b2GetContactSim( world, contact );+		B2_ASSERT( contactSim->contactId == contactIndex );+		B2_ASSERT( contactSim->bodyIdA == contact->edges[0].bodyId );+		B2_ASSERT( contactSim->bodyIdB == contact->edges[1].bodyId );++		bool simTouching = ( contactSim->simFlags & b2_simTouchingFlag ) != 0;+		B2_ASSERT( touching == simTouching );++		B2_ASSERT( 0 <= contactSim->manifold.pointCount && contactSim->manifold.pointCount <= 2 );+	}++	int contactIdCount = b2GetIdCount( &world->contactIdPool );+	B2_ASSERT( allocatedContactCount == contactIdCount );+}++#else++void b2ValidateConnectivity( b2World* world )+{+	B2_UNUSED( world );+}++void b2ValidateSolverSets( b2World* world )+{+	B2_UNUSED( world );+}++void b2ValidateContacts( b2World* world )+{+	B2_UNUSED( world );+}++#endif
+ upstream/src/physics_world.h view
@@ -0,0 +1,235 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++typedef struct b2Recording b2Recording;++#include "arena_allocator.h"+#include "bitset.h"+#include "broad_phase.h"+#include "constraint_graph.h"+#include "container.h"+#include "id_pool.h"+#include "sensor.h"+#include "shape.h"+#include "solver_set.h"++#include "box2d/types.h"++b2DeclareArray( b2BodyMoveEvent );+b2DeclareArray( b2ContactBeginTouchEvent );+b2DeclareArray( b2ContactEndTouchEvent );+b2DeclareArray( b2ContactHitEvent );+b2DeclareArray( b2JointEvent );+b2DeclareArray( b2SensorBeginTouchEvent );+b2DeclareArray( b2SensorEndTouchEvent );+b2DeclareArray( b2TaskContext );++// Per thread task storage+typedef struct b2TaskContext+{+	// Collect per thread sensor continuous hit events.+	b2Array( b2SensorHit ) sensorHits;++	// Broad-phase pairs.+	b2Array( uint64_t ) pairKeys;++	// These bits align with the contact id capacity and signal a change in contact status+	b2BitSet contactStateBitSet;++	// These bits align with the contact id capacity and signal a hit event.+	b2BitSet hitEventBitSet;++	// Fast-path flag: true when this worker set at least one bit in hitEventBitSet this step.+	bool hasHitEvents;++	// These bits align with the joint id capacity and signal a change in contact status+	b2BitSet jointStateBitSet;++	// 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.+	b2BitSet enlargedSimBitSet;++	// Used to put islands to sleep+	b2BitSet awakeIslandBitSet;++	// Per worker split island candidate+	float splitSleepTime;+	int splitIslandId;++	// Number of contacts recycled this step (collide pass).+	int recycledContactCount;++} b2TaskContext;++// The world struct manages all physics entities, dynamic simulation,  and asynchronous queries.+// The world also contains efficient memory management facilities.+typedef struct b2World+{+	b2Stack stack;+	b2BroadPhase broadPhase;+	b2ConstraintGraph constraintGraph;++	// 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 b2Body.+	b2IdPool 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.+	b2Array( b2Body ) bodies;++	// Provides free list for solver sets.+	b2IdPool 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.+	b2Array( b2SolverSet ) solverSets;++	// Used to create stable ids for joints+	b2IdPool jointIdPool;++	// This is a sparse array that maps joint ids to the joint data stored in the constraint graph+	// or in the solver sets.+	b2Array( b2Joint ) joints;++	// Used to create stable ids for contacts+	b2IdPool contactIdPool;++	// This is a sparse array that maps contact ids to the contact data stored in the constraint graph+	// or in the solver sets.+	b2Array( b2Contact ) contacts;++	// Used to create stable ids for islands+	b2IdPool islandIdPool;++	// Persistent islands+	b2Array( b2Island ) islands;++	b2IdPool shapeIdPool;+	b2IdPool chainIdPool;++	// These are sparse arrays that point into the pools above+	b2Array( b2Shape ) shapes;++	// Shape enlarged bounds (hot data split).+	b2Array( b2AABB ) fatAABBs;++	b2Array( b2ChainShape ) chainShapes;++	// This is a dense array of sensor data.+	b2Array( b2Sensor ) sensors;++	// Per thread storage+	b2Array( b2TaskContext ) taskContexts;+	b2Array( b2SensorTaskContext ) sensorTaskContexts;++	b2Array( b2BodyMoveEvent ) bodyMoveEvents;+	b2Array( b2SensorBeginTouchEvent ) sensorBeginEvents;+	b2Array( b2ContactBeginTouchEvent ) contactBeginEvents;++	// End events are double buffered so that the user doesn't need to flush events+	b2Array( b2SensorEndTouchEvent ) sensorEndEvents[2];+	b2Array( b2ContactEndTouchEvent ) contactEndEvents[2];+	int endEventArrayIndex;++	b2Array( b2ContactHitEvent ) contactHitEvents;+	b2Array( b2JointEvent ) jointEvents;++	// todo consider deferred waking and impulses to make it possible+	// to apply forces and impulses from multiple threads+	// impulses must be deferred because sleeping bodies have no velocity state+	// Problems:+	// - multiple forces applied to the same body from multiple threads++	// Used to track debug draw+	b2BitSet debugBodySet;+	b2BitSet debugJointSet;+	b2BitSet debugContactSet;+	b2BitSet debugIslandSet;++	// 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;++	b2Vec2 gravity;+	float hitEventThreshold;+	float restitutionThreshold;+	int restitutionIterations;+	bool enableRestitutionPropagation;+	float maxLinearSpeed;+	float contactSpeed;+	float contactHertz;+	float contactDampingRatio;+	float contactRecycleDistance;++	b2FrictionCallbackFcn* frictionCallback;+	b2RestitutionCallbackFcn* restitutionCallback;++	uint16_t generation;++	b2Profile profile;++	b2Capacity maxCapacity;++	b2PreSolveCallbackFcn* preSolveFcn;+	b2PreContinuousCallbackFcn* preContinuousFcn;+	void* preSolveContext;++	b2CustomFilterCallbackFcn* customFilterFcn;+	void* customFilterContext;++	int workerCount;+	int simdWidth;+	b2EnqueueTaskCallbackFcn* enqueueTaskFcn;+	b2FinishTaskCallbackFcn* finishTaskFcn;+	void* userTaskContext;+	void* userTreeTask;++	struct b2Scheduler* scheduler;++	void* userData;++	b2Recording* recording; // NULL unless b2World_StartRecording is active, owned by the host++	// Remember type step used for reporting forces and torques+	// inverse sub-step+	float inv_h;++	// inverse full-step+	float inv_dt;++	int activeTaskCount;+	int taskCount;++	uint16_t worldId;++	bool enableSleep;+	bool locked;+	bool enableWarmStarting;+	bool enableContinuous;+	bool inUse;+} b2World;++b2World* b2GetWorldFromId( b2WorldId id );+b2World* b2GetWorld( int index );+b2World* b2GetWorldLocked( int index );++// Union of the broad-phase root bounds across all body types. Returns false when no tree holds a+// proxy, so callers don't fold an empty world's origin into a running bounds.+bool b2ComputeWorldBounds( b2World* world, b2AABB* bounds );++void b2ValidateConnectivity( b2World* world );+void b2ValidateSolverSets( b2World* world );+void b2ValidateContacts( b2World* world );
+ upstream/src/platform.h view
@@ -0,0 +1,350 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "core.h"++#include <stdbool.h>+#include <stdint.h>++#if defined( _MSC_VER ) && ( defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC ) )+#include <intrin.h>+#elif defined( _MSC_VER )+#include <intrin0.h>+#endif++#if defined( _M_X64 ) || defined( __x86_64__ ) || defined( _M_IX86 ) || defined( __i386__ )+#include <xmmintrin.h>+#endif++#if defined( _MSC_VER )+#if defined( _M_X64 ) || defined( __x86_64__ ) || defined( _M_IX86 ) || defined( __i386__ )+#define b2Prefetch( addr ) _mm_prefetch( (const char*)( addr ), _MM_HINT_T0 )+#else+#define b2Prefetch( addr ) __prefetch( (const void*)( addr ) )+#endif+#elif defined( __GNUC__ ) || defined( __clang__ )+#define b2Prefetch( addr ) __builtin_prefetch( (const void*)( addr ), 0, 3 )+#else+#define b2Prefetch( addr ) ( (void)( addr ) )+#endif++#if ( defined( __GNUC__ ) || defined( __clang__ ) ) && ( defined( __i386__ ) || defined( __x86_64__ ) )+static inline void b2Pause( void )+{+	__asm__ __volatile__( "pause\n" );+}+#elif ( defined( __arm__ ) && defined( __ARM_ARCH ) && __ARM_ARCH >= 7 ) || defined( __aarch64__ )+static inline void b2Pause( void )+{+	__asm__ __volatile__( "yield" ::: "memory" );+}+#elif defined( _MSC_VER ) && ( defined( _M_IX86 ) || defined( _M_X64 ) )+static inline void b2Pause( void )+{+	_mm_pause();+}+#elif defined( _MSC_VER ) && ( defined( _M_ARM ) || defined( _M_ARM64 ) )+static inline void b2Pause( void )+{+	__yield();+}+#else+static inline void b2Pause( void )+{+}+#endif+++static inline void b2AtomicStoreInt( b2AtomicInt* 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 b2AtomicLoadInt( b2AtomicInt* 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 b2AtomicFetchAddInt( b2AtomicInt* 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 uint16_t b2AtomicFetchOrU16( uint16_t* a, uint16_t mask )+{+#if defined( _MSC_VER )+	return (uint16_t)_InterlockedOr16( (short*)a, (short)mask );+#elif defined( __GNUC__ ) || defined( __clang__ )+	return __atomic_fetch_or( a, mask, __ATOMIC_SEQ_CST );+#else+#error "Unsupported platform"+#endif+}++static inline bool b2AtomicCompareExchangeInt( b2AtomicInt* 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 b2AtomicStoreU32( b2AtomicU32* 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 uint16_t b2AtomicLoadU16( const uint16_t* a )+{+#if defined( _MSC_VER ) && !defined( __clang__ )+	return (uint16_t)__iso_volatile_load16( (const volatile __int16*)a );+#elif defined( __GNUC__ ) || defined( __clang__ )+	return __atomic_load_n( a, __ATOMIC_RELAXED );+#else+#error "Unsupported platform"+#endif+}++static inline uint32_t b2AtomicLoadU32( b2AtomicU32* 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+}++static inline uint32_t b2AtomicLoadU32Raw( uint32_t* a )+{+#if defined( _MSC_VER ) && !defined( __clang__ )+	return (uint32_t)__iso_volatile_load32( (volatile __int32*)a );+#elif defined( __GNUC__ ) || defined( __clang__ )+	return __atomic_load_n( a, __ATOMIC_RELAXED );+#else+#error "Unsupported platform"+#endif+}++static inline uint32_t b2AtomicFetchOrU32( uint32_t* a, uint32_t mask )+{+#if defined( _MSC_VER )+	return (uint32_t)_InterlockedOr( (long*)a, (long)mask );+#elif defined( __GNUC__ ) || defined( __clang__ )+	return __atomic_fetch_or( a, mask, __ATOMIC_SEQ_CST );+#else+#error "Unsupported platform"+#endif+}++static inline int64_t b2AtomicFetchAddI64( b2AtomicI64* a, int64_t increment )+{+#if defined( _MSC_VER )+	return (int64_t)_InterlockedExchangeAdd64( (__int64*)&a->value, (__int64)increment );+#elif defined( __GNUC__ ) || defined( __clang__ )+	return __atomic_fetch_add( &a->value, increment, __ATOMIC_SEQ_CST );+#else+#error "Unsupported platform"+#endif+}++static inline int64_t b2AtomicLoadI64( b2AtomicI64* a )+{+#if defined( _MSC_VER ) && !defined( __clang__ ) && !defined( _M_ARM )+	int64_t value = __iso_volatile_load64( (volatile __int64*)&a->value );+#if defined( _M_ARM64 ) || defined( _M_ARM64EC )+	__dmb( 0xB );+#else+	_ReadWriteBarrier();+#endif+	return value;+#elif defined( _MSC_VER ) && !defined( __clang__ )+	// 32-bit ARM has no plain atomic 64-bit load+	return _InterlockedOr64( (__int64*)&a->value, 0 );+#elif defined( __GNUC__ ) || defined( __clang__ )+	return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST );+#else+#error "Unsupported platform"+#endif+}++// Denormal flushing is a per thread mode the host can turn on, usually by linking a module+// built with fast math. This breaks determinism.+static inline bool b2IsDenormalFlushEnabled( void )+{+#if defined( _M_X64 ) || defined( __x86_64__ ) || defined( _M_IX86 ) || defined( __i386__ )+	// FTZ is bit 15, DAZ is bit 6+	uint32_t csr = _mm_getcsr();+	return ( csr & 0x8040 ) != 0;+#elif defined( _M_ARM64 ) || defined( __aarch64__ )+	// FZ is bit 24 and flushes denormal inputs and outputs together+	uint64_t fpcr;+#if defined( _MSC_VER )+	fpcr = _ReadStatusReg( ARM64_FPCR );+#else+	__asm__ __volatile__( "mrs %0, fpcr" : "=r"( fpcr ) );+#endif+	return ( fpcr & ( 1ull << 24 ) ) != 0;+#else+	// wasm has no flush mode+	return false;+#endif+}++#if defined( _MSC_VER ) && !defined( __clang__ )++#include <intrin0.h>++// https://en.wikipedia.org/wiki/Find_first_set++static inline uint32_t b2CTZ32( 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 b2CLZ32( 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 b2CTZ64( 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 b2PopCount64( uint64_t block )+{+	return (int)__popcnt64( block );+}++#else++static inline uint32_t b2CTZ32( uint32_t block )+{+	return __builtin_ctz( block );+}++static inline uint32_t b2CLZ32( uint32_t value )+{+	return __builtin_clz( value );+}++static inline uint32_t b2CTZ64( uint64_t block )+{+	return __builtin_ctzll( block );+}++static inline int b2PopCount64( uint64_t block )+{+	return __builtin_popcountll( block );+}+#endif++static inline bool b2IsPowerOf2( int x )+{+	return ( x & ( x - 1 ) ) == 0;+}++static inline int b2BoundingPowerOf2( int x )+{+	if ( x <= 1 )+	{+		return 1;+	}++	return 32 - (int)b2CLZ32( (uint32_t)x - 1 );+}++static inline int b2RoundUpPowerOf2( int x )+{+	if ( x <= 1 )+	{+		return 1;+	}++	return 1 << ( 32 - (int)b2CLZ32( (uint32_t)x - 1 ) );+}
+ upstream/src/pogo_joint.c view
@@ -0,0 +1,286 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "body.h"+#include "joint.h"+#include "physics_world.h"+#include "recording.h"+#include "solver.h"+#include "solver_set.h"++// needed for dll export+#include "box2d/box2d.h"++static const b2Vec2 b2_pogoAxis = { 0.0f, 1.0f };++void b2PogoJoint_SetRestLength( b2JointId jointId, float length )+{+	B2_CHECK_INPUT( b2IsValidFloat( length ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PogoJointSetRestLength, jointId, length );+	+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint );+	joint->pogoJoint.restLength = length;+}++float b2PogoJoint_GetRestLength( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint );+	return joint->pogoJoint.restLength;+}++float b2PogoJoint_GetSpringHertz( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint );+	return joint->pogoJoint.hertz;+}++void b2PogoJoint_SetSpringHertz( b2JointId jointId, float hertz )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );++	 b2World* world = b2GetWorld( jointId.world0 );+	 B2_REC( world, PogoJointSetSpringHertz, jointId, hertz );++	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint );+	joint->pogoJoint.hertz = hertz;+}++void b2PogoJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )+{+	B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) );++	 b2World* world = b2GetWorld( jointId.world0 );+	 B2_REC( world, PogoJointSetSpringDampingRatio, jointId, dampingRatio );++	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint );+	joint->pogoJoint.dampingRatio = dampingRatio;+}++float b2PogoJoint_GetSpringDampingRatio( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint );+	return joint->pogoJoint.dampingRatio;+}++float b2PogoJoint_GetCurrentLength( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_pogoJoint );++	// Relative to body A so the difference stays in float precision far from the origin+	b2WorldTransform wxfA = b2GetBodyTransform( world, jointSim->bodyIdA );+	b2Transform transformA = b2ToRelativeTransform( wxfA, wxfA.p );+	b2Transform transformB = b2ToRelativeTransform( b2GetBodyTransform( world, jointSim->bodyIdB ), wxfA.p );++	b2Vec2 axis = b2RotateVector( jointSim->localFrameB.q, b2_pogoAxis );+	axis = b2RotateVector( transformB.q, axis );+	b2Vec2 pA = b2TransformPoint( transformA, jointSim->localFrameA.p );+	b2Vec2 pB = b2TransformPoint( transformB, jointSim->localFrameB.p );+	b2Vec2 d = b2Sub( pB, pA );+	float length = b2Dot( d, axis );+	return length;+}++float b2PogoJoint_GetImpulse( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint );+	return joint->pogoJoint.impulse;+}++float b2PogoJoint_GetCurrentSpeed( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_pogoJoint );+	return joint->pogoJoint.velocity;+}++b2Vec2 b2GetPogoJointForce( b2World* world, b2JointSim* base )+{+	b2Vec2 force = b2MulSV( world->inv_h * base->pogoJoint.impulse, base->pogoJoint.normal );+	return force;+}++void b2PreparePogoJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_pogoJoint );++	b2World* world = context->world;++	// chase body id to the solver set where the body lives+	int idA = base->bodyIdA;+	int idB = base->bodyIdB;++	b2Body* bodyA = b2Array_Get( world->bodies, idA );+	b2Body* bodyB = b2Array_Get( world->bodies, idB );++	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );++	b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex );+	b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex );++	int localIndexA = bodyA->localIndex;+	int localIndexB = bodyB->localIndex;++	b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA );+	b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB );++	float mA = bodySimA->invMass;+	float iA = bodySimA->invInertia;+	float mB = bodySimB->invMass;+	float iB = bodySimB->invInertia;++	base->invMassA = mA;+	base->invMassB = mB;+	base->invIA = iA;+	base->invIB = iB;++	b2PogoJoint* joint = &base->pogoJoint;+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;++	// Compute joint anchor frames with world space rotation, relative to center of mass+	joint->frameA.q = b2MulRot( bodySimA->transform.q, base->localFrameA.q );+	joint->frameA.p = b2RotateVector( bodySimA->transform.q, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );+	joint->frameB.q = b2MulRot( bodySimB->transform.q, base->localFrameB.q );+	joint->frameB.p = b2RotateVector( bodySimB->transform.q, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );++	// Compute the initial center delta. Incremental position updates are relative to this.+	joint->deltaCenter = b2SubPos( bodySimB->center, bodySimA->center );++	b2Vec2 rA = joint->frameA.p;+	b2Vec2 rB = joint->frameB.p;++	// compute effective mass+	float crA = b2Cross( rA, joint->normal );+	float crB = b2Cross( rB, joint->normal );+	float k = mA + mB + iA * crA * crA + iB * crB * crB;+	joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f;++	if ( context->enableWarmStarting == false )+	{+		joint->impulse = 0.0f;+	}+}++void b2WarmStartPogoJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_pogoJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	b2PogoJoint* joint = &base->pogoJoint;++	if ( joint->hertz == 0.0f )+	{+		joint->impulse = 0.0f;+		return;+	}++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	b2Vec2 linearImpulse = b2MulSV( joint->impulse, joint->normal );++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, linearImpulse );+		stateA->angularVelocity -= iA * b2Cross( rA, linearImpulse );+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, linearImpulse );+		stateB->angularVelocity += iB * b2Cross( rB, linearImpulse );+	}+}++void b2SolvePogoJoint( b2JointSim* base, b2StepContext* context, bool useBias )+{+	B2_UNUSED( useBias );++	B2_ASSERT( base->type == b2_pogoJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2PogoJoint* joint = &base->pogoJoint;+	if ( joint->hertz == 0.0f )+	{+		joint->impulse = 0.0f;+		return;+	}++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 vA = stateA->linearVelocity;+	float wA = stateA->angularVelocity;+	b2Vec2 vB = stateB->linearVelocity;+	float wB = stateB->angularVelocity;++	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	float bias = 0.0f;+	if ( useBias )+	{+		// This is a custom pogo position correction designed to avoid feeding velocity into+		// the mover body. Otherwise the mover can make huge jumps going up stairs. This still+		// works with continuous collision.++		b2Vec2 dcA = stateA->deltaPosition;+		b2Vec2 dcB = stateB->deltaPosition;+		b2Vec2 d = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter );++		// The pogo axis is usually fixed as the world y-axis. Different than the ground normal.+		b2Vec2 pogoAxis = b2RotateVector( joint->frameB.q, b2_pogoAxis );+		float c = b2Dot( pogoAxis, d ) - joint->restLength;++		// This could be divided by dot(joint->normal, pogoAxis) to account for the constraint+		// direction, but I'd rather diminish the pogo recovery rate than risk making it huge.+		joint->velocity = b2SpringDamper( joint->hertz, joint->dampingRatio, c, joint->velocity, context->h );+		bias = -joint->velocity;+	}++	b2Vec2 vr = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );+	float cdot = b2Dot( joint->normal, vr );++	float maxTensionImpulse = context->h * joint->maxTensionForce;+	float maxCompressionImpulse = context->h * joint->maxCompressionForce;+	float oldImpulse = joint->impulse;+	float impulse = -joint->linearMass * (cdot + bias);+	joint->impulse = b2ClampFloat( joint->impulse + impulse, -maxTensionImpulse, maxCompressionImpulse );+	impulse = joint->impulse - oldImpulse;++	b2Vec2 P = b2MulSV( impulse, joint->normal );+	vA = b2MulSub( vA, mA, P );+	wA -= iA * b2Cross( rA, P );+	vB = b2MulAdd( vB, mB, P );+	wB += iB * b2Cross( rB, P );++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = vA;+		stateA->angularVelocity = wA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = vB;+		stateB->angularVelocity = wB;+	}+}
+ upstream/src/prismatic_joint.c view
@@ -0,0 +1,718 @@+// SPDX-FileCopyrightText: 2023 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 "box2d/box2d.h"++void b2PrismaticJoint_EnableSpring( b2JointId jointId, bool enableSpring )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PrismaticJointEnableSpring, jointId, enableSpring );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	if ( enableSpring != joint->prismaticJoint.enableSpring )+	{+		joint->prismaticJoint.enableSpring = enableSpring;+		joint->prismaticJoint.springImpulse = 0.0f;+	}+}++bool b2PrismaticJoint_IsSpringEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.enableSpring;+}++void b2PrismaticJoint_SetSpringHertz( b2JointId jointId, float hertz )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PrismaticJointSetSpringHertz, jointId, hertz );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	joint->prismaticJoint.hertz = hertz;+}++float b2PrismaticJoint_GetSpringHertz( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.hertz;+}++void b2PrismaticJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )+{+	B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PrismaticJointSetSpringDampingRatio, jointId, dampingRatio );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	joint->prismaticJoint.dampingRatio = dampingRatio;+}++float b2PrismaticJoint_GetSpringDampingRatio( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.dampingRatio;+}++void b2PrismaticJoint_SetTargetTranslation( b2JointId jointId, float translation )+{+	B2_CHECK_INPUT( b2IsValidFloat( translation ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PrismaticJointSetTargetTranslation, jointId, translation );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	joint->prismaticJoint.targetTranslation = translation;+}++float b2PrismaticJoint_GetTargetTranslation( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.targetTranslation;+}++void b2PrismaticJoint_EnableLimit( b2JointId jointId, bool enableLimit )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PrismaticJointEnableLimit, jointId, enableLimit );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	if ( enableLimit != joint->prismaticJoint.enableLimit )+	{+		joint->prismaticJoint.enableLimit = enableLimit;+		joint->prismaticJoint.lowerImpulse = 0.0f;+		joint->prismaticJoint.upperImpulse = 0.0f;+	}+}++bool b2PrismaticJoint_IsLimitEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.enableLimit;+}++float b2PrismaticJoint_GetLowerLimit( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.lowerTranslation;+}++float b2PrismaticJoint_GetUpperLimit( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.upperTranslation;+}++void b2PrismaticJoint_SetLimits( b2JointId jointId, float lower, float upper )+{+	B2_CHECK_INPUT( b2IsValidFloat( lower ) );+	B2_CHECK_INPUT( b2IsValidFloat( upper ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PrismaticJointSetLimits, jointId, lower, upper );+	B2_ASSERT( lower <= upper );++	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	joint->prismaticJoint.lowerTranslation = b2MinFloat( lower, upper );+	joint->prismaticJoint.upperTranslation = b2MaxFloat( lower, upper );+}++void b2PrismaticJoint_EnableMotor( b2JointId jointId, bool enableMotor )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PrismaticJointEnableMotor, jointId, enableMotor );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	if ( enableMotor != joint->prismaticJoint.enableMotor )+	{+		joint->prismaticJoint.enableMotor = enableMotor;+		joint->prismaticJoint.motorImpulse = 0.0f;+	}+}++bool b2PrismaticJoint_IsMotorEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.enableMotor;+}++void b2PrismaticJoint_SetTargetMotorSpeed( b2JointId jointId, float motorSpeed )+{+	B2_CHECK_INPUT( b2IsValidFloat( motorSpeed ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PrismaticJointSetTargetMotorSpeed, jointId, motorSpeed );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	joint->prismaticJoint.motorSpeed = motorSpeed;+}++float b2PrismaticJoint_GetTargetMotorSpeed( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.motorSpeed;+}++float b2PrismaticJoint_GetCurrentMotorForce( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return world->inv_h * base->prismaticJoint.motorImpulse;+}++void b2PrismaticJoint_SetMaxMotorForce( b2JointId jointId, float force )+{+	B2_CHECK_INPUT( b2IsValidFloat( force ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, PrismaticJointSetMaxMotorForce, jointId, force );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	joint->prismaticJoint.maxMotorForce = force;+}++float b2PrismaticJoint_GetMaxMotorForce( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	return joint->prismaticJoint.maxMotorForce;+}++float b2PrismaticJoint_GetCurrentTranslation( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_prismaticJoint );+	// Relative to body A so the difference stays in float precision far from the origin+	b2WorldTransform wxfA = b2GetBodyTransform( world, jointSim->bodyIdA );+	b2Transform transformA = b2ToRelativeTransform( wxfA, wxfA.p );+	b2Transform transformB = b2ToRelativeTransform( b2GetBodyTransform( world, jointSim->bodyIdB ), wxfA.p );++	b2Vec2 localAxisA = b2RotateVector( jointSim->localFrameA.q, (b2Vec2){ 1.0f, 0.0f } );+	b2Vec2 axisA = b2RotateVector( transformA.q, localAxisA );+	b2Vec2 pA = b2TransformPoint( transformA, jointSim->localFrameA.p );+	b2Vec2 pB = b2TransformPoint( transformB, jointSim->localFrameB.p );+	b2Vec2 d = b2Sub( pB, pA );+	float translation = b2Dot( d, axisA );+	return translation;+}++float b2PrismaticJoint_GetCurrentSpeed( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2Joint* joint = b2GetJointFullId( world, jointId );+	B2_ASSERT( joint->type == b2_prismaticJoint );+	b2JointSim* base = b2GetJointSim( world, joint );+	B2_ASSERT( base->type == b2_prismaticJoint );++	b2Body* bodyA = b2Array_Get( world->bodies, base->bodyIdA );+	b2Body* bodyB = b2Array_Get( world->bodies, base->bodyIdB );+	b2BodySim* bodySimA = b2GetBodySim( world, bodyA );+	b2BodySim* bodySimB = b2GetBodySim( world, bodyB );+	b2BodyState* bodyStateA = b2GetBodyState( world, bodyA );+	b2BodyState* bodyStateB = b2GetBodyState( world, bodyB );++	b2Rot qA = bodySimA->transform.q;+	b2Rot qB = bodySimB->transform.q;++	b2Vec2 localAxisA = b2RotateVector( base->localFrameA.q, (b2Vec2){ 1.0f, 0.0f } );+	b2Vec2 axisA = b2RotateVector( qA, localAxisA );+	b2Vec2 rA = b2RotateVector( qA, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );+	b2Vec2 rB = b2RotateVector( qB, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );++	// Difference the centers in double so the speed stays exact far from the origin+	b2Vec2 dc = b2SubPos( bodySimB->center, bodySimA->center );+	b2Vec2 d = b2Add( dc, b2Sub( rB, rA ) );++	b2Vec2 vA = bodyStateA ? bodyStateA->linearVelocity : b2Vec2_zero;+	b2Vec2 vB = bodyStateB ? bodyStateB->linearVelocity : b2Vec2_zero;+	float wA = bodyStateA ? bodyStateA->angularVelocity : 0.0f;+	float wB = bodyStateB ? bodyStateB->angularVelocity : 0.0f;++	b2Vec2 vRel = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );+	float speed = b2Dot( d, b2CrossSV( wA, axisA ) ) + b2Dot( axisA, vRel );+	return speed;+}++b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base )+{+	b2Rot qA = b2GetBodyTransform( world, base->bodyIdA ).q;++	b2PrismaticJoint* joint = &base->prismaticJoint;++	b2Vec2 localAxisA = b2RotateVector( base->localFrameA.q, (b2Vec2){ 1.0f, 0.0f } );+	b2Vec2 axisA = b2RotateVector( qA, localAxisA );+	b2Vec2 perpA = b2LeftPerp( axisA );++	float inv_h = world->inv_h;+	float perpForce = inv_h * joint->impulse.x;+	float axialForce = inv_h * ( joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse );++	b2Vec2 force = b2Add( b2MulSV( perpForce, perpA ), b2MulSV( axialForce, axisA ) );+	return force;+}++float b2GetPrismaticJointTorque( b2World* world, b2JointSim* base )+{+	return world->inv_h * base->prismaticJoint.impulse.y;+}++// Linear constraint (point-to-line)+// d = pB - pA = xB + rB - xA - rA+// C = dot(perp, d)+// Cdot = dot(d, cross(wA, perp)) + dot(perp, vB + cross(wB, rB) - vA - cross(wA, rA))+//      = -dot(perp, vA) - dot(cross(rA + d, perp), wA) + dot(perp, vB) + dot(cross(rB, perp), vB)+// J = [-perp, -cross(rA + d, perp), perp, cross(rB, perp)]+//+// Angular constraint+// C = aB - aA + a_initial+// Cdot = wB - wA+// J = [0 0 -1 0 0 1]+//+// K = J * invM * JT+//+// J = [-a -sA a sB]+//     [0  -1  0  1]+// a = perp+// sA = cross(rA + d, a) = cross(pB - xA, a)+// sB = cross(rB, a) = cross(pB - xB, a)++// Motor/Limit linear constraint+// C = dot(axA, d)+// Cdot = -dot(axA, vA) - dot(cross(rA + d, axA), wA) + dot(axA, vB) + dot(cross(rB, axA), vB)+// J = [-axA -cross(rA + d, axA) axA cross(rB, ax1)]++// 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++// Block Solver+// We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer.+//+// The Jacobian has 2 rows:+// J = [-uT -s1 uT s2] // linear+//     [0   -1   0  1] // angular+//+// u = perp+// s1 = cross(d + r1, u), s2 = cross(r2, u)+// a1 = cross(d + r1, v), a2 = cross(r2, v)++void b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_prismaticJoint );++	// chase body id to the solver set where the body lives+	int idA = base->bodyIdA;+	int idB = base->bodyIdB;++	b2World* world = context->world;++	b2Body* bodyA = b2Array_Get( world->bodies, idA );+	b2Body* bodyB = b2Array_Get( world->bodies, idB );++	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );+	b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex );+	b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex );++	int localIndexA = bodyA->localIndex;+	int localIndexB = bodyB->localIndex;++	b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA );+	b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB );++	float mA = bodySimA->invMass;+	float iA = bodySimA->invInertia;+	float mB = bodySimB->invMass;+	float iB = bodySimB->invInertia;++	base->invMassA = mA;+	base->invMassB = mB;+	base->invIA = iA;+	base->invIB = iB;++	b2PrismaticJoint* joint = &base->prismaticJoint;+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;++	// Compute joint anchor frames with world space rotation, relative to center of mass+	joint->frameA.q = b2MulRot( bodySimA->transform.q, base->localFrameA.q );+	joint->frameA.p = b2RotateVector( bodySimA->transform.q, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );+	joint->frameB.q = b2MulRot( bodySimB->transform.q, base->localFrameB.q );+	joint->frameB.p = b2RotateVector( bodySimB->transform.q, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );++	// Compute the initial center delta. Incremental position updates are relative to this.+	joint->deltaCenter = b2SubPos( bodySimB->center, bodySimA->center );++	joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );++	if ( context->enableWarmStarting == false )+	{+		joint->impulse = b2Vec2_zero;+		joint->springImpulse = 0.0f;+		joint->motorImpulse = 0.0f;+		joint->lowerImpulse = 0.0f;+		joint->upperImpulse = 0.0f;+	}+}++void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_prismaticJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2PrismaticJoint* joint = &base->prismaticJoint;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );++	b2Vec2 axisA = b2RotateVector( joint->frameA.q, (b2Vec2){ 1.0f, 0.0f } );+	axisA = b2RotateVector( stateA->deltaRotation, axisA );++	// impulse is applied at anchor point on body B+	float a1 = b2Cross( b2Add( rA, d ), axisA );+	float a2 = b2Cross( rB, axisA );+	float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;++	// perpendicular constraint+	b2Vec2 perpA = b2LeftPerp( axisA );+	float s1 = b2Cross( b2Add( rA, d ), perpA );+	float s2 = b2Cross( rB, perpA );+	float perpImpulse = joint->impulse.x;+	float angleImpulse = joint->impulse.y;++	b2Vec2 P = b2Add( b2MulSV( axialImpulse, axisA ), b2MulSV( perpImpulse, perpA ) );+	float LA = axialImpulse * a1 + perpImpulse * s1 + angleImpulse;+	float LB = axialImpulse * a2 + perpImpulse * s2 + angleImpulse;++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P );+		stateA->angularVelocity -= iA * LA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P );+		stateB->angularVelocity += iB * LB;+	}+}++void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias )+{+	B2_ASSERT( base->type == b2_prismaticJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2PrismaticJoint* joint = &base->prismaticJoint;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 vA = stateA->linearVelocity;+	float wA = stateA->angularVelocity;+	b2Vec2 vB = stateB->linearVelocity;+	float wB = stateB->angularVelocity;++	b2Rot qA = b2MulRot( stateA->deltaRotation, joint->frameA.q );+	b2Rot qB = b2MulRot( stateB->deltaRotation, joint->frameB.q );+	b2Rot relQ = b2InvMulRot( qA, qB );++	// current anchors+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );++	b2Vec2 axisA = b2RotateVector( joint->frameA.q, (b2Vec2){ 1.0f, 0.0f } );+	axisA = b2RotateVector( stateA->deltaRotation, axisA );+	float translation = b2Dot( axisA, d );++	// These scalars are for torques generated by axial forces+	float a1 = b2Cross( b2Add( rA, d ), axisA );+	float a2 = b2Cross( rB, axisA );++	float k = mA + mB + iA * a1 * a1 + iB * a2 * a2;+	float axialMass = k > 0.0f ? 1.0f / k : 0.0f;++	b2Softness softness = base->constraintSoftness;++	// spring constraint+	if ( joint->enableSpring )+	{+		// This is a real spring and should be applied even during relax+		float C = translation - joint->targetTranslation;+		float bias = joint->springSoftness.biasRate * C;+		float massScale = joint->springSoftness.massScale;+		float impulseScale = joint->springSoftness.impulseScale;++		float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;+		float deltaImpulse = -massScale * axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse;+		joint->springImpulse += deltaImpulse;++		b2Vec2 P = b2MulSV( deltaImpulse, axisA );+		float LA = deltaImpulse * a1;+		float LB = deltaImpulse * a2;++		vA = b2MulSub( vA, mA, P );+		wA -= iA * LA;+		vB = b2MulAdd( vB, mB, P );+		wB += iB * LB;+	}++	// Solve motor constraint+	if ( joint->enableMotor )+	{+		float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;+		float impulse = axialMass * ( joint->motorSpeed - Cdot );+		float oldImpulse = joint->motorImpulse;+		float maxImpulse = context->h * joint->maxMotorForce;+		joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse );+		impulse = joint->motorImpulse - oldImpulse;++		b2Vec2 P = b2MulSV( impulse, axisA );+		float LA = impulse * a1;+		float LB = impulse * a2;++		vA = b2MulSub( vA, mA, P );+		wA -= iA * LA;+		vB = b2MulAdd( vB, mB, P );+		wB += iB * LB;+	}++	if ( joint->enableLimit )+	{+		// Clamp the speculative distance to a reasonable value+		float speculativeDistance = 0.25f * ( joint->upperTranslation - joint->lowerTranslation );++		// Lower limit+		{+			float C = translation - joint->lowerTranslation;++			if ( C < speculativeDistance )+			{+				float bias = 0.0f;+				float massScale = 1.0f;+				float impulseScale = 0.0f;++				if ( C > 0.0f )+				{+					// speculation+					float safe = b2GetLengthUnitsPerMeter();+					bias = b2MinFloat( C, safe ) * context->inv_h;+				}+				else if ( useBias )+				{+					bias = softness.biasRate * C;+					massScale = softness.massScale;+					impulseScale = softness.impulseScale;+				}++				float oldImpulse = joint->lowerImpulse;+				float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;+				float deltaImpulse = -axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse;+				joint->lowerImpulse = b2MaxFloat( oldImpulse + deltaImpulse, 0.0f );+				deltaImpulse = joint->lowerImpulse - oldImpulse;++				b2Vec2 P = b2MulSV( deltaImpulse, axisA );+				float LA = deltaImpulse * a1;+				float LB = deltaImpulse * a2;++				vA = b2MulSub( vA, mA, P );+				wA -= iA * LA;+				vB = b2MulAdd( vB, mB, P );+				wB += iB * LB;+			}+			else+			{+				joint->lowerImpulse = 0.0f;+			}+		}++		// 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->upperTranslation - translation;++			if ( C < speculativeDistance )+			{+				float bias = 0.0f;+				float massScale = 1.0f;+				float impulseScale = 0.0f;++				if ( C > 0.0f )+				{+					// speculation+					float safe = b2GetLengthUnitsPerMeter();+					bias = b2MinFloat( C, safe ) * context->inv_h;+				}+				else if ( useBias )+				{+					bias = softness.biasRate * C;+					massScale = softness.massScale;+					impulseScale = softness.impulseScale;+				}++				float oldImpulse = joint->upperImpulse;++				// sign flipped+				float Cdot = b2Dot( axisA, b2Sub( vA, vB ) ) + a1 * wA - a2 * wB;+				float deltaImpulse = -axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse;+				joint->upperImpulse = b2MaxFloat( oldImpulse + deltaImpulse, 0.0f );+				deltaImpulse = joint->upperImpulse - oldImpulse;++				b2Vec2 P = b2MulSV( deltaImpulse, axisA );+				float LA = deltaImpulse * a1;+				float LB = deltaImpulse * a2;++				// sign flipped+				vA = b2MulAdd( vA, mA, P );+				wA += iA * LA;+				vB = b2MulSub( vB, mB, P );+				wB -= iB * LB;+			}+			else+			{+				joint->upperImpulse = 0.0f;+			}+		}+	}++	// Solve the prismatic constraint in block form+	{+		b2Vec2 perpA = b2LeftPerp( axisA );++		// These scalars are for torques generated by the perpendicular constraint force+		float s1 = b2Cross( b2Add( d, rA ), perpA );+		float s2 = b2Cross( rB, perpA );++		b2Vec2 Cdot;+		Cdot.x = b2Dot( perpA, b2Sub( vB, vA ) ) + s2 * wB - s1 * wA;+		Cdot.y = wB - wA;++		b2Vec2 bias = b2Vec2_zero;+		float massScale = 1.0f;+		float impulseScale = 0.0f;+		if ( useBias )+		{+			b2Vec2 C;+			C.x = b2Dot( perpA, d );+			C.y = b2Rot_GetAngle( relQ );++			bias = b2MulSV( softness.biasRate, C );+			massScale = softness.massScale;+			impulseScale = softness.impulseScale;+		}++		float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;+		float k12 = iA * s1 + iB * s2;+		float k22 = iA + iB;+		if ( k22 == 0.0f )+		{+			// For bodies with fixed rotation.+			k22 = 1.0f;+		}++		b2Mat22 K = { { k11, k12 }, { k12, k22 } };++		b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) );+		b2Vec2 deltaImpulse;+		deltaImpulse.x = -massScale * b.x - impulseScale * joint->impulse.x;+		deltaImpulse.y = -massScale * b.y - impulseScale * joint->impulse.y;++		joint->impulse.x += deltaImpulse.x;+		joint->impulse.y += deltaImpulse.y;++		b2Vec2 P = b2MulSV( deltaImpulse.x, perpA );+		float LA = deltaImpulse.x * s1 + deltaImpulse.y;+		float LB = deltaImpulse.x * s2 + deltaImpulse.y;++		vA = b2MulSub( vA, mA, P );+		wA -= iA * LA;+		vB = b2MulAdd( vB, mB, P );+		wB += iB * LB;+	}++	B2_ASSERT( b2IsValidVec2( vA ) );+	B2_ASSERT( b2IsValidFloat( wA ) );+	B2_ASSERT( b2IsValidVec2( vB ) );+	B2_ASSERT( b2IsValidFloat( wB ) );++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = vA;+		stateA->angularVelocity = wA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = vB;+		stateB->angularVelocity = wB;+	}+}++void b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB,+						   float drawScale )+{+	B2_ASSERT( base->type == b2_prismaticJoint );++	b2PrismaticJoint* joint = &base->prismaticJoint;++	b2WorldTransform frameA = b2MulWorldTransforms( transformA, base->localFrameA );+	b2WorldTransform frameB = b2MulWorldTransforms( transformB, base->localFrameB );+	b2Vec2 axisA = b2RotateVector( frameA.q, (b2Vec2){ 1.0f, 0.0f } );++	draw->DrawLineFcn( frameA.p, frameB.p, b2_colorDimGray, draw->context );++	if ( joint->enableLimit )+	{+		float b = 0.25f * drawScale;+		b2Pos lower = b2OffsetPos( frameA.p, b2MulSV( joint->lowerTranslation, axisA ) );+		b2Pos upper = b2OffsetPos( frameA.p, b2MulSV( joint->upperTranslation, axisA ) );+		b2Vec2 perp = b2LeftPerp( axisA );+		draw->DrawLineFcn( lower, upper, b2_colorGray, draw->context );+		draw->DrawLineFcn( b2OffsetPos( lower, b2MulSV( -b, perp ) ), b2OffsetPos( lower, b2MulSV( b, perp ) ), b2_colorGreen,+						   draw->context );+		draw->DrawLineFcn( b2OffsetPos( upper, b2MulSV( -b, perp ) ), b2OffsetPos( upper, b2MulSV( b, perp ) ), b2_colorRed,+						   draw->context );+	}+	else+	{+		draw->DrawLineFcn( b2OffsetPos( frameA.p, b2Neg( axisA ) ), b2OffsetPos( frameA.p, axisA ), b2_colorGray,+						   draw->context );+	}++	if ( joint->enableSpring )+	{+		b2Pos p = b2OffsetPos( frameA.p, b2MulSV( joint->targetTranslation, axisA ) );+		draw->DrawPointFcn( p, 8.0f, b2_colorViolet, draw->context );+	}++	draw->DrawPointFcn( frameA.p, 5.0f, b2_colorGray, draw->context );+	draw->DrawPointFcn( frameB.p, 5.0f, b2_colorBlue, draw->context );+}
+ upstream/src/recording.c view
@@ -0,0 +1,929 @@+// 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 "physics_world.h"+#include "snapshot.h"++#include "box2d/box2d.h"++#include <limits.h>+#include <stddef.h>+#include <stdio.h>++// Buffer helpers++void b2RecBufAppend( b2RecBuffer* buf, const void* data, int size )+{+	if ( size <= 0 )+	{+		return;+	}++	// Sizing pass: tally bytes without allocating or copying+	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 = b2Alloc( newCap );+		}+		else+		{+			buf->data = b2GrowAlloc( buf->data, buf->capacity, newCap );+		}+		buf->capacity = newCap;+	}++	memcpy( buf->data + buf->size, data, (size_t)size );+	buf->size += size;+}++void b2RecBufFree( b2RecBuffer* buf )+{+	if ( buf->data != NULL )+	{+		b2Free( buf->data, buf->capacity );+		buf->data = NULL;+		buf->capacity = 0;+		buf->size = 0;+	}+}++// Write primitives++void b2RecW_U8( b2RecBuffer* buf, uint8_t v )+{+	b2RecBufAppend( buf, &v, 1 );+}++void b2RecW_U16( b2RecBuffer* buf, uint16_t v )+{+	uint8_t b[2] = { (uint8_t)v, (uint8_t)( v >> 8 ) };+	b2RecBufAppend( buf, b, 2 );+}++void b2RecW_U32( b2RecBuffer* buf, uint32_t v )+{+	uint8_t b[4] = { (uint8_t)v, (uint8_t)( v >> 8 ), (uint8_t)( v >> 16 ), (uint8_t)( v >> 24 ) };+	b2RecBufAppend( buf, b, 4 );+}++void b2RecW_U64( b2RecBuffer* 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 ) };+	b2RecBufAppend( buf, b, 8 );+}++void b2RecW_I32( b2RecBuffer* buf, int32_t v )+{+	b2RecW_U32( buf, (uint32_t)v );+}++void b2RecW_F32( b2RecBuffer* buf, float v )+{+	uint32_t bits;+	memcpy( &bits, &v, 4 );+	b2RecW_U32( buf, bits );+}++void b2RecW_BOOL( b2RecBuffer* buf, bool v )+{+	b2RecW_U8( buf, v ? 1u : 0u );+}++void b2RecW_VEC2( b2RecBuffer* buf, b2Vec2 v )+{+	b2RecW_F32( buf, v.x );+	b2RecW_F32( buf, v.y );+}++void b2RecW_ROT( b2RecBuffer* buf, b2Rot v )+{+	b2RecW_F32( buf, v.c );+	b2RecW_F32( buf, v.s );+}++void b2RecW_XF( b2RecBuffer* buf, b2Transform v )+{+	b2RecW_VEC2( buf, v.p );+	b2RecW_ROT( buf, v.q );+}++void b2RecW_F64( b2RecBuffer* buf, double v )+{+	uint64_t bits;+	memcpy( &bits, &v, 8 );+	b2RecW_U64( buf, bits );+}++// A world position keeps full precision on the wire so a recording reproduces the simulation far+// from the origin. In the float build this is two floats, identical to VEC2.+void b2RecW_POSITION( b2RecBuffer* buf, b2Pos v )+{+#if defined( BOX2D_DOUBLE_PRECISION )+	b2RecW_F64( buf, v.x );+	b2RecW_F64( buf, v.y );+#else+	b2RecW_F32( buf, v.x );+	b2RecW_F32( buf, v.y );+#endif+}++void b2RecW_WORLDXF( b2RecBuffer* buf, b2WorldTransform v )+{+	b2RecW_POSITION( buf, v.p );+	b2RecW_ROT( buf, v.q );+}++void b2RecW_WORLDID( b2RecBuffer* buf, b2WorldId v )+{+	b2RecW_U32( buf, b2StoreWorldId( v ) );+}++void b2RecW_BODYID( b2RecBuffer* buf, b2BodyId v )+{+	b2RecW_U64( buf, b2StoreBodyId( v ) );+}++void b2RecW_SHAPEID( b2RecBuffer* buf, b2ShapeId v )+{+	b2RecW_U64( buf, b2StoreShapeId( v ) );+}++void b2RecW_CHAINID( b2RecBuffer* buf, b2ChainId v )+{+	b2RecW_U64( buf, b2StoreChainId( v ) );+}++void b2RecW_JOINTID( b2RecBuffer* buf, b2JointId v )+{+	b2RecW_U64( buf, b2StoreJointId( v ) );+}++// Geometry is pointer-free POD, pointerWidth in the header gates the layout++void b2RecW_CIRCLE( b2RecBuffer* buf, b2Circle v )+{+	b2RecBufAppend( buf, &v, (int)sizeof( b2Circle ) );+}++void b2RecW_CAPSULE( b2RecBuffer* buf, b2Capsule v )+{+	b2RecBufAppend( buf, &v, (int)sizeof( b2Capsule ) );+}++void b2RecW_SEGMENT( b2RecBuffer* buf, b2Segment v )+{+	b2RecBufAppend( buf, &v, (int)sizeof( b2Segment ) );+}++void b2RecW_POLYGON( b2RecBuffer* buf, b2Polygon v )+{+	b2RecBufAppend( buf, &v, (int)sizeof( b2Polygon ) );+}++void b2RecW_CHAINSEG( b2RecBuffer* buf, b2ChainSegment v )+{+	b2RecBufAppend( buf, &v, (int)sizeof( b2ChainSegment ) );+}++void b2RecW_FILTER( b2RecBuffer* buf, b2Filter v )+{+	b2RecW_U64( buf, v.categoryBits );+	b2RecW_U64( buf, v.maskBits );+	b2RecW_I32( buf, v.groupIndex );+}++void b2RecW_MATERIAL( b2RecBuffer* buf, b2SurfaceMaterial v )+{+	b2RecW_F32( buf, v.friction );+	b2RecW_F32( buf, v.restitution );+	b2RecW_F32( buf, v.rollingResistance );+	b2RecW_F32( buf, v.tangentSpeed );+	b2RecW_U64( buf, v.userMaterialId );+	b2RecW_U32( buf, v.customColor );+}++void b2RecW_MASSDATA( b2RecBuffer* buf, b2MassData v )+{+	b2RecW_F32( buf, v.mass );+	b2RecW_VEC2( buf, v.center );+	b2RecW_F32( buf, v.rotationalInertia );+}++void b2RecW_LOCKS( b2RecBuffer* buf, b2MotionLocks v )+{+	b2RecW_BOOL( buf, v.linearX );+	b2RecW_BOOL( buf, v.linearY );+	b2RecW_BOOL( buf, v.angularZ );+}++void b2RecW_STR( b2RecBuffer* buf, const char* s )+{+	if ( s == NULL )+	{+		b2RecW_U16( buf, 0xFFFFu );+		return;+	}+	int len = 0;+	while ( s[len] != '\0' && len < 65534 )+	{+		len++;+	}+	b2RecW_U16( buf, (uint16_t)len );+	if ( len > 0 )+	{+		b2RecBufAppend( buf, s, len );+	}+}++// Hand-written def helpers. Zero all pointer and cookie fields before serializing+// Readers call b2Default*Def() first to get the cookie, then overwrite fields++void b2RecW_BODYDEF( b2RecBuffer* buf, b2BodyDef v )+{+	b2RecW_I32( buf, (int32_t)v.type );+	b2RecW_POSITION( buf, v.position );+	b2RecW_ROT( buf, v.rotation );+	b2RecW_VEC2( buf, v.linearVelocity );+	b2RecW_F32( buf, v.angularVelocity );+	b2RecW_F32( buf, v.linearDamping );+	b2RecW_F32( buf, v.angularDamping );+	b2RecW_F32( buf, v.gravityScale );+	b2RecW_F32( buf, v.sleepThreshold );+	b2RecW_F32( buf, v.safetyFactor );+	b2RecW_STR( buf, v.name );+	// userData: not preserved+	b2RecW_U64( buf, 0u );+	b2RecW_LOCKS( buf, v.motionLocks );+	b2RecW_BOOL( buf, v.enableSleep );+	b2RecW_BOOL( buf, v.isAwake );+	b2RecW_BOOL( buf, v.isBullet );+	b2RecW_BOOL( buf, v.isEnabled );+	b2RecW_BOOL( buf, v.allowFastRotation );+	b2RecW_BOOL( buf, v.enableContactRecycling );+	// internalValue omitted+}++void b2RecW_SHAPEDEF( b2RecBuffer* buf, b2ShapeDef v )+{+	// userData: not preserved+	b2RecW_U64( buf, 0u );+	b2RecW_MATERIAL( buf, v.material );+	b2RecW_F32( buf, v.density );+	b2RecW_FILTER( buf, v.filter );+	b2RecW_BOOL( buf, v.enableCustomFiltering );+	b2RecW_BOOL( buf, v.isSensor );+	b2RecW_BOOL( buf, v.enableSensorEvents );+	b2RecW_BOOL( buf, v.enableContactEvents );+	b2RecW_BOOL( buf, v.enableHitEvents );+	b2RecW_BOOL( buf, v.enablePreSolveEvents );+	b2RecW_BOOL( buf, v.invokeContactCreation );+	b2RecW_BOOL( buf, v.updateBodyMass );+	// internalValue omitted+}++// Variable-length def: point and material arrays are length-prefixed and inlined.+// Arrays are cloned by b2CreateChain so they only need to outlive the dispatch call.+void b2RecW_CHAINDEF( b2RecBuffer* buf, b2ChainDef v )+{+	// userData: not preserved+	b2RecW_U64( buf, 0u );+	b2RecW_I32( buf, v.pointCount );+	for ( int i = 0; i < v.pointCount; ++i )+	{+		b2RecW_VEC2( buf, v.points[i] );+	}+	b2RecW_VEC2( buf, v.ghost1 );+	b2RecW_VEC2( buf, v.ghost2 );+	b2RecW_I32( buf, v.materialCount );+	for ( int i = 0; i < v.materialCount; ++i )+	{+		b2RecW_MATERIAL( buf, v.materials[i] );+	}+	b2RecW_FILTER( buf, v.filter );+	b2RecW_BOOL( buf, v.isLoop );+	b2RecW_BOOL( buf, v.enableSensorEvents );+	// internalValue omitted+}++void b2RecW_EXPLOSIONDEF( b2RecBuffer* buf, b2ExplosionDef v )+{+	b2RecW_U64( buf, v.maskBits );+	b2RecW_POSITION( buf, v.position );+	b2RecW_F32( buf, v.radius );+	b2RecW_F32( buf, v.falloff );+	b2RecW_F32( buf, v.impulsePerLength );+}++// Joint defs share a base. Body ids are written as ids and remapped to the replay world on+// read. userData and internalValue are not serialized.+static void b2RecW_JointBase( b2RecBuffer* buf, const b2JointDef* base )+{+	b2RecW_U64( buf, 0u ); // userData+	b2RecW_BODYID( buf, base->bodyIdA );+	b2RecW_BODYID( buf, base->bodyIdB );+	b2RecW_XF( buf, base->localFrameA );+	b2RecW_XF( buf, base->localFrameB );+	b2RecW_F32( buf, base->forceThreshold );+	b2RecW_F32( buf, base->torqueThreshold );+	b2RecW_F32( buf, base->constraintHertz );+	b2RecW_F32( buf, base->constraintDampingRatio );+	b2RecW_F32( buf, base->drawScale );+	b2RecW_BOOL( buf, base->collideConnected );+}++void b2RecW_DISTANCEJOINTDEF( b2RecBuffer* buf, b2DistanceJointDef v )+{+	b2RecW_JointBase( buf, &v.base );+	b2RecW_F32( buf, v.length );+	b2RecW_BOOL( buf, v.enableSpring );+	b2RecW_F32( buf, v.lowerSpringForce );+	b2RecW_F32( buf, v.upperSpringForce );+	b2RecW_F32( buf, v.hertz );+	b2RecW_F32( buf, v.dampingRatio );+	b2RecW_BOOL( buf, v.enableLimit );+	b2RecW_F32( buf, v.minLength );+	b2RecW_F32( buf, v.maxLength );+	b2RecW_BOOL( buf, v.enableMotor );+	b2RecW_F32( buf, v.maxMotorForce );+	b2RecW_F32( buf, v.motorSpeed );+}++void b2RecW_FILTERJOINTDEF( b2RecBuffer* buf, b2FilterJointDef v )+{+	b2RecW_JointBase( buf, &v.base );+}++void b2RecW_MOTORJOINTDEF( b2RecBuffer* buf, b2MotorJointDef v )+{+	b2RecW_JointBase( buf, &v.base );+	b2RecW_VEC2( buf, v.linearVelocity );+	b2RecW_F32( buf, v.maxVelocityForce );+	b2RecW_F32( buf, v.angularVelocity );+	b2RecW_F32( buf, v.maxVelocityTorque );+	b2RecW_F32( buf, v.linearHertz );+	b2RecW_F32( buf, v.linearDampingRatio );+	b2RecW_F32( buf, v.maxSpringForce );+	b2RecW_F32( buf, v.angularHertz );+	b2RecW_F32( buf, v.angularDampingRatio );+	b2RecW_F32( buf, v.maxSpringTorque );+}++void b2RecW_MOVERJOINTDEF( b2RecBuffer* buf, b2MoverJointDef v )+{+	b2RecW_JointBase( buf, &v.base );+	b2RecW_VEC2( buf, v.linearVelocity );+	b2RecW_VEC2( buf, v.maxVelocityForce );+}++void b2RecW_POGOJOINTDEF( b2RecBuffer* buf, b2PogoJointDef v )+{+	b2RecW_JointBase( buf, &v.base );+	b2RecW_VEC2( buf, v.normal );+	b2RecW_F32( buf, v.hertz );+	b2RecW_F32( buf, v.dampingRatio );+	b2RecW_F32( buf, v.restLength );+	b2RecW_F32( buf, v.maxTensionForce );+	b2RecW_F32( buf, v.maxCompressionForce );+	b2RecW_F32( buf, v.impulse );+	b2RecW_F32( buf, v.velocity );+}++void b2RecW_PRISMATICJOINTDEF( b2RecBuffer* buf, b2PrismaticJointDef v )+{+	b2RecW_JointBase( buf, &v.base );+	b2RecW_BOOL( buf, v.enableSpring );+	b2RecW_F32( buf, v.hertz );+	b2RecW_F32( buf, v.dampingRatio );+	b2RecW_F32( buf, v.targetTranslation );+	b2RecW_BOOL( buf, v.enableLimit );+	b2RecW_F32( buf, v.lowerTranslation );+	b2RecW_F32( buf, v.upperTranslation );+	b2RecW_BOOL( buf, v.enableMotor );+	b2RecW_F32( buf, v.maxMotorForce );+	b2RecW_F32( buf, v.motorSpeed );+}++void b2RecW_REVOLUTEJOINTDEF( b2RecBuffer* buf, b2RevoluteJointDef v )+{+	b2RecW_JointBase( buf, &v.base );+	b2RecW_F32( buf, v.targetAngle );+	b2RecW_BOOL( buf, v.enableSpring );+	b2RecW_F32( buf, v.hertz );+	b2RecW_F32( buf, v.dampingRatio );+	b2RecW_BOOL( buf, v.enableLimit );+	b2RecW_F32( buf, v.lowerAngle );+	b2RecW_F32( buf, v.upperAngle );+	b2RecW_BOOL( buf, v.enableMotor );+	b2RecW_F32( buf, v.maxMotorTorque );+	b2RecW_F32( buf, v.motorSpeed );+}++void b2RecW_WELDJOINTDEF( b2RecBuffer* buf, b2WeldJointDef v )+{+	b2RecW_JointBase( buf, &v.base );+	b2RecW_F32( buf, v.linearHertz );+	b2RecW_F32( buf, v.angularHertz );+	b2RecW_F32( buf, v.linearDampingRatio );+	b2RecW_F32( buf, v.angularDampingRatio );+}++void b2RecW_WHEELJOINTDEF( b2RecBuffer* buf, b2WheelJointDef v )+{+	b2RecW_JointBase( buf, &v.base );+	b2RecW_BOOL( buf, v.enableSpring );+	b2RecW_F32( buf, v.hertz );+	b2RecW_F32( buf, v.dampingRatio );+	b2RecW_BOOL( buf, v.enableLimit );+	b2RecW_F32( buf, v.lowerTranslation );+	b2RecW_F32( buf, v.upperTranslation );+	b2RecW_BOOL( buf, v.enableMotor );+	b2RecW_F32( buf, v.maxMotorTorque );+	b2RecW_F32( buf, v.motorSpeed );+}++void b2RecW_AABB( b2RecBuffer* buf, b2AABB v )+{+	b2RecW_VEC2( buf, v.lowerBound );+	b2RecW_VEC2( buf, v.upperBound );+}++void b2RecW_QUERYFILTER( b2RecBuffer* buf, b2QueryFilter v )+{+	b2RecW_U64( buf, v.categoryBits );+	b2RecW_U64( buf, v.maskBits );+}++void b2RecW_SHAPEPROXY( b2RecBuffer* buf, b2ShapeProxy v )+{+	int count = v.count;+	if ( count < 0 )+		count = 0;+	if ( count > B2_MAX_POLYGON_VERTICES )+		count = B2_MAX_POLYGON_VERTICES;+	b2RecW_I32( buf, count );+	for ( int i = 0; i < count; ++i )+	{+		b2RecW_VEC2( buf, v.points[i] );+	}+	b2RecW_F32( buf, v.radius );+}++void b2RecW_RAYRESULT( b2RecBuffer* buf, b2RayResult v )+{+	b2RecW_SHAPEID( buf, v.shapeId );+	b2RecW_POSITION( buf, v.point );+	b2RecW_VEC2( buf, v.normal );+	b2RecW_F32( buf, v.fraction );+	b2RecW_I32( buf, v.nodeVisits );+	b2RecW_I32( buf, v.leafVisits );+	b2RecW_BOOL( buf, v.hit );+}++void b2RecW_PLANERESULT( b2RecBuffer* buf, b2PlaneResult v )+{+	b2RecW_VEC2( buf, v.plane.normal );+	b2RecW_F32( buf, v.plane.offset );+	b2RecW_VEC2( buf, v.point );+	b2RecW_BOOL( buf, v.hit );+}++void b2RecW_TREESTATS( b2RecBuffer* buf, b2TreeStats v )+{+	b2RecW_I32( buf, v.nodeVisits );+	b2RecW_I32( buf, v.leafVisits );+}++// Patch helpers for query hit-count backfill++int b2RecReserveU32( b2RecBuffer* buf )+{+	int offset = buf->size;+	uint8_t zero[4] = { 0, 0, 0, 0 };+	b2RecBufAppend( buf, zero, 4 );+	return offset;+}++void b2RecPatchU32( b2RecBuffer* buf, int offset, uint32_t v )+{+	B2_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 );+}++// Concurrent query commits are serialized so records never interleave in the shared buffer+void b2RecCommitRecord( b2Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize )+{+	B2_ASSERT( payloadSize >= 0 && payloadSize < ( 1 << 24 ) );+	b2LockMutex( rec->lock );+	b2RecW_U8( &rec->buffer, opcode );+	uint8_t sz[3] = { (uint8_t)payloadSize, (uint8_t)( payloadSize >> 8 ), (uint8_t)( payloadSize >> 16 ) };+	b2RecBufAppend( &rec->buffer, sz, 3 );+	b2RecBufAppend( &rec->buffer, payload, payloadSize );+	b2UnlockMutex( rec->lock );+}++void b2RecQueryBegin( b2RecQueryWriter* w, void* context )+{+	w->buf = (b2RecBuffer){ 0 };+	w->userFcn.overlapFcn = NULL;+	w->userContext = context;+	w->hitCount = 0;+	w->countOffset = 0;+}++void b2RecQueryCommit( b2Recording* rec, uint8_t opcode, b2RecQueryWriter* w )+{+	b2RecCommitRecord( rec, opcode, w->buf.data, w->buf.size );+	b2RecBufFree( &w->buf );+}++bool b2RecOverlapTrampoline( b2ShapeId id, void* ctx )+{+	b2RecQueryWriter* w = (b2RecQueryWriter*)ctx;+	bool ret = w->userFcn.overlapFcn( id, w->userContext );+	b2RecW_SHAPEID( &w->buf, id );+	b2RecW_BOOL( &w->buf, ret );+	w->hitCount++;+	return ret;+}++float b2RecCastTrampoline( b2ShapeId id, b2Pos point, b2Vec2 normal, float fraction, void* ctx )+{+	b2RecQueryWriter* w = (b2RecQueryWriter*)ctx;+	float ret = w->userFcn.castFcn( id, point, normal, fraction, w->userContext );+	b2RecW_SHAPEID( &w->buf, id );+	b2RecW_POSITION( &w->buf, point );+	b2RecW_VEC2( &w->buf, normal );+	b2RecW_F32( &w->buf, fraction );+	b2RecW_F32( &w->buf, ret );+	w->hitCount++;+	return ret;+}++bool b2RecPlaneTrampoline( b2ShapeId id, const b2PlaneResult* plane, void* ctx )+{+	b2RecQueryWriter* w = (b2RecQueryWriter*)ctx;+	bool ret = w->userFcn.planeFcn( id, plane, w->userContext );+	b2RecW_SHAPEID( &w->buf, id );+	b2RecW_PLANERESULT( &w->buf, *plane );+	b2RecW_BOOL( &w->buf, ret );+	w->hitCount++;+	return ret;+}++// Record framing++void b2RecBeginRecord( b2Recording* rec, uint8_t opcode )+{+	b2RecW_U8( &rec->buffer, opcode );+	rec->recordStart = rec->buffer.size;+	// Make space to hold a 24-bit payload size, which isn't known until b2RecEndRecord is called.+	uint8_t zero[3] = { 0, 0, 0 };+	b2RecBufAppend( &rec->buffer, zero, 3 );+}++void b2RecEndRecord( b2Recording* rec )+{+	// Compute the final payload size and record it in the 24-bit space reserved right after the opcode.+	int payloadSize = rec->buffer.size - rec->recordStart - 3;+	B2_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. Each generated function writes its struct fields to the buffer+// Example:+// B2_REC_OP( 0x80, Step, RET_NONE, ARG( WORLDID, world ) ARG( F32, dt ) ARG( I32, subStepCount ) )+// Becomes:+// void b2RecWriteArgs_Step( b2Recording* rec, const b2RecArgs_Step* a)+// {+//   b2RecW_WORLDID( &rec->buffer, a->world );+//   b2RecW_F32( &rec->buffer, a->dt );+//   b2RecW_I32( &rec->buffer, a->subStepCount );+// }+#define ARG( TAG, field ) b2RecW_##TAG( &rec->buffer, a->field );+#define B2_REC_OP( op, Name, RET, ... )                                                                                          \+	void b2RecWriteArgs_##Name( b2Recording* rec, const b2RecArgs_##Name* a )                                                    \+	{                                                                                                                            \+		__VA_ARGS__                                                                                                              \+	}+#include "recording_ops.inl"+#undef B2_REC_OP+#undef ARG++// Codegen: full writers wrapping begin, arg writer, end+// Setters and creates may run on threads that each own a distinct object, so hold the lock across+// the whole record. Without it a concurrent writer splices its bytes between our begin and end and+// the record desyncs replay. Same lock the query commit path takes.+// Example:+// B2_REC_OP( 0x80, Step, RET_NONE, ARG( WORLDID, world ) ARG( F32, dt ) ARG( I32, subStepCount ) )+// Becomes:+// void b2RecWrite_Step( b2Recording* rec, const b2RecArgs_Step* a)+// {+//   b2LockMutex( rec->lock );+//   b2RecBeginRecord( rec, (uint8_t)( 0x80 ) );+//   b2RecWriteArgs_Step( rec, a );+//   b2RecEndRecord( rec );+//   b2UnlockMutex( rec->lock );+// }+#define B2_REC_OP( op, Name, RET, ... )                                                                                          \+	void b2RecWrite_##Name( b2Recording* rec, const b2RecArgs_##Name* a )                                                        \+	{                                                                                                                            \+		b2LockMutex( rec->lock );                                                                                                \+		b2RecBeginRecord( rec, (uint8_t)( op ) );                                                                                \+		b2RecWriteArgs_##Name( rec, a );                                                                                         \+		b2RecEndRecord( rec );                                                                                                   \+		b2UnlockMutex( rec->lock );                                                                                              \+	}+#include "recording_ops.inl"+#undef B2_REC_OP++// Codegen: create-op writers that append the returned id inside the record. The RET tag+// selects the id type and its write primitive. RET_NONE ops generate nothing.+// Example:+// B2_REC_OP( 0x40, CreateCircleShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( CIRCLE, circle ) )+// Becomes:+// 	void b2RecWriteRet_CreateCircleShape( b2Recording* rec, const b2RecArgs_CreateCircleShape* a, idType id )+//	{+//		b2LockMutex( rec->lock );+//		b2RecBeginRecord( rec, (uint8_t)( 0x40 ) );+//		b2RecWriteArgs_CreateCircleShape( rec, a );+//		b2RecW_SHAPEID( &rec->buffer, id );+//		b2RecEndRecord( rec );+//		b2UnlockMutex( rec->lock );+//	}++#define B2_REC_RETWRITE( op, Name, idType, idW )                                                                                 \+	void b2RecWriteRet_##Name( b2Recording* rec, const b2RecArgs_##Name* a, idType id )                                          \+	{                                                                                                                            \+		b2LockMutex( rec->lock );                                                                                                \+		b2RecBeginRecord( rec, (uint8_t)( op ) );                                                                                \+		b2RecWriteArgs_##Name( rec, a );                                                                                         \+		idW( &rec->buffer, id );                                                                                                 \+		b2RecEndRecord( rec );                                                                                                   \+		b2UnlockMutex( rec->lock );                                                                                              \+	}+#define B2_REC_RETWRITE_RET_NONE( op, Name )+#define B2_REC_RETWRITE_RET_BODYID( op, Name ) B2_REC_RETWRITE( op, Name, b2BodyId, b2RecW_BODYID )+#define B2_REC_RETWRITE_RET_SHAPEID( op, Name ) B2_REC_RETWRITE( op, Name, b2ShapeId, b2RecW_SHAPEID )+#define B2_REC_RETWRITE_RET_CHAINID( op, Name ) B2_REC_RETWRITE( op, Name, b2ChainId, b2RecW_CHAINID )+#define B2_REC_RETWRITE_RET_JOINTID( op, Name ) B2_REC_RETWRITE( op, Name, b2JointId, b2RecW_JOINTID )+#define B2_REC_OP( op, Name, RET, ... ) B2_REC_RETWRITE_##RET( op, Name )+#include "recording_ops.inl"+#undef B2_REC_OP+#undef B2_REC_RETWRITE_RET_NONE+#undef B2_REC_RETWRITE_RET_BODYID+#undef B2_REC_RETWRITE_RET_SHAPEID+#undef B2_REC_RETWRITE_RET_CHAINID+#undef B2_REC_RETWRITE_RET_JOINTID+#undef B2_REC_RETWRITE++// Lifecycle++b2Recording* b2CreateRecording( int byteCapacity )+{+	b2Recording* rec = b2Alloc( (int)sizeof( b2Recording ) );+	*rec = (b2Recording){ 0 };++	// Pre-size to taste; the buffer still doubles on demand. A few hundred KiB is plenty for a+	// short session and avoids early reallocations.+	int initCap = byteCapacity > 0 ? byteCapacity : 65536;+	rec->buffer.data = b2Alloc( initCap );+	rec->buffer.capacity = initCap;+	rec->buffer.size = 0;+	rec->lock = b2CreateMutex();+	return rec;+}++void b2DestroyRecording( b2Recording* recording )+{+	if ( recording == NULL )+	{+		return;+	}++	b2RecBufFree( &recording->buffer );+	b2DestroyMutex( recording->lock );+	b2Free( recording, (int)sizeof( b2Recording ) );+}++const uint8_t* b2Recording_GetData( const b2Recording* recording )+{+	return recording->buffer.data;+}++int b2Recording_GetSize( const b2Recording* recording )+{+	return recording->buffer.size;+}++void b2RecAccumulateBounds( b2Recording* rec, b2AABB bounds )+{+	rec->accumulatedBounds = rec->haveBounds ? b2AABB_Union( rec->accumulatedBounds, bounds ) : bounds;+	rec->haveBounds = true;+}++void b2StartRecordingIntoBuffer( b2World* world, b2Recording* recording )+{+	// Reset so a recording handle can be reused for a fresh session+	recording->buffer.size = 0;+	recording->recordStart = 0;+	recording->haveBounds = false;++	// Serialize the live world into a blob that follows the header and seeds replay.+	b2RecBuffer blob = { 0 };+	b2SerializeWorld( world, &blob );++	b2RecHeader hdr = { 0 };+	hdr.magic = B2_REC_MAGIC;+	hdr.versionMajor = B2_REC_VERSION_MAJOR;+	hdr.versionMinor = B2_REC_VERSION_MINOR;+	hdr.lengthScale = b2GetLengthUnitsPerMeter();+	hdr.pointerWidth = (uint8_t)sizeof( void* );+	hdr.bigEndian = 0;+	hdr.validationEnabled = B2_ENABLE_VALIDATION ? 1u : 0u;+	hdr.snapshotSize = (uint64_t)blob.size;+	b2RecBufAppend( &recording->buffer, &hdr, (int)sizeof( hdr ) );+	b2RecBufAppend( &recording->buffer, blob.data, blob.size );+	b2RecBufFree( &blob );++	world->recording = recording;++	// Seed the bounds with the snapshot state so frame 0 is framed even if nothing moves+	b2AABB seed;+	if ( b2ComputeWorldBounds( world, &seed ) )+	{+		b2RecAccumulateBounds( recording, seed );+	}++	// Anchor the recorded state hash so replay verifies the blob deserialized to the same world.+	b2WorldId worldId = { (uint16_t)( world->worldId + 1 ), world->generation };+	b2RecArgs_StateHash stateHash = { worldId, b2HashWorldState( world ) };+	b2RecWrite_StateHash( recording, &stateHash );+}++void b2StopRecordingInternal( b2World* world )+{+	if ( world->recording == NULL )+	{+		return;+	}++	b2Recording* rec = world->recording;+	world->recording = NULL;++	// Stash the accumulated bounds so a viewer can frame the whole motion at open time. Sits in+	// the op stream ahead of the end marker; absent in older recordings.+	b2RecArgs_RecordingBounds rb = { 0 };+	if ( rec->haveBounds )+	{+		rb.bounds = rec->accumulatedBounds;+	}+	b2RecWrite_RecordingBounds( rec, &rb );++	// Write DestroyWorld so the buffer is self-contained, an end marker the viewer reads. The+	// buffer and handle belong to the user, freed with b2DestroyRecording.+	b2WorldId wid = { (uint16_t)( world->worldId + 1 ), world->generation };+	b2RecArgs_DestroyWorld a = { wid };+	b2RecWrite_DestroyWorld( rec, &a );+}++// Convenience file I/O. The library never opens files while recording. These let a host persist+// or reload a recording buffer without writing its own I/O. fopen precedent: b2World_DumpMemoryStats.++bool b2Recording_SaveToFile( const b2Recording* 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;+}++b2Recording* b2CreateRecordingFromFile( 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, so reject it here rather than+	// hand back a malformed buffer that only fails later+	if ( fileSize < (long)sizeof( b2RecHeader ) || fileSize > INT_MAX )+	{+		fclose( f );+		return NULL;+	}+	fseek( f, 0, SEEK_SET );++	b2Recording* rec = b2CreateRecording( (int)fileSize );+	size_t readSize = fread( rec->buffer.data, 1, (size_t)fileSize, f );+	fclose( f );++	if ( (long)readSize != fileSize )+	{+		b2DestroyRecording( rec );+		return NULL;+	}++	// Validate the magic so a wrong file fails at load instead of deep in the player+	b2RecHeader hdr;+	memcpy( &hdr, rec->buffer.data, sizeof( hdr ) );+	if ( hdr.magic != B2_REC_MAGIC )+	{+		b2DestroyRecording( rec );+		return NULL;+	}++	rec->buffer.size = (int)fileSize;+	return rec;+}++// Hash transforms and velocities.+uint64_t b2HashWorldState( b2World* world )+{+	uint64_t hash = B2_SNAP_FNV_INIT;+	const uint64_t prime = B2_SNAP_FNV_PRIME;++	int bodyCount = world->bodies.count;+	for ( int i = 0; i < bodyCount; ++i )+	{+		b2Body* body = world->bodies.data + i;+		if ( body->id != i )+		{+			// Free or never-used slot+			continue;+		}++		b2BodySim* sim = b2GetBodySim( world, body );++		uint32_t bits;++#define B2_HASH_FLOAT( f )                                                                                                       \+	memcpy( &bits, &( f ), 4 );                                                                                                  \+	hash = ( hash ^ (uint64_t)bits ) * prime;++		hash = b2FnvMixPosition( hash, sim->transform.p );+		B2_HASH_FLOAT( sim->transform.q.c )+		B2_HASH_FLOAT( sim->transform.q.s )++		b2BodyState* state = b2GetBodyState( world, body );+		if ( state != NULL )+		{+			B2_HASH_FLOAT( state->linearVelocity.x )+			B2_HASH_FLOAT( state->linearVelocity.y )+			B2_HASH_FLOAT( state->angularVelocity )+		}++#undef B2_HASH_FLOAT+	}++	return hash;+}
+ upstream/src/recording.h view
@@ -0,0 +1,312 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "core.h"++#include "box2d/id.h"+#include "box2d/math_types.h"+#include "box2d/types.h"++#include <stdbool.h>+#include <stdint.h>+#if defined( _MSC_VER )+#include <vcruntime_string.h>+#else+#include <string.h>+#endif++// FNV-1a 64-bit constants+#define B2_SNAP_FNV_INIT 14695981039346656037ull+#define B2_SNAP_FNV_PRIME 1099511628211ull++// Mix a world position at full width, or the determinism gates would validate only to float+// precision and pass vacuously far from the origin+static inline uint64_t b2FnvMixPosition( uint64_t hash, b2Pos p )+{+#if defined( BOX2D_DOUBLE_PRECISION )+	uint64_t bx, by;+	memcpy( &bx, &p.x, 8 );+	memcpy( &by, &p.y, 8 );+#else+	uint32_t fx, fy;+	memcpy( &fx, &p.x, 4 );+	memcpy( &fy, &p.y, 4 );+	uint64_t bx = fx, by = fy;+#endif+	hash = ( hash ^ bx ) * B2_SNAP_FNV_PRIME;+	hash = ( hash ^ by ) * B2_SNAP_FNV_PRIME;+	return hash;+}++typedef struct b2World b2World;++// Magic value 'B2RC' in little-endian: bytes B2, R, C yield 0x43523242+#define B2_REC_MAGIC 0x43523242u++// Recording format version. Any mismatch refuses to load. The minor tracks op stream layout+// changes that keep the 32 byte header shape.+#define B2_REC_VERSION_MAJOR 3+#define B2_REC_VERSION_MINOR 13 // shape queries not recorded++// File header, fixed 32 bytes, little-endian+typedef struct b2RecHeader+{+	uint32_t magic;		   // 'B2RC' = 0x43523242+	uint16_t versionMajor; // B2_REC_VERSION_MAJOR+	uint16_t versionMinor; // B2_REC_VERSION_MINOR+	uint32_t reserved2;+	float lengthScale; // The world length scale+	uint8_t reserved3;+	uint8_t pointerWidth;	   // sizeof(void*), gates POD-def memcpy+	uint8_t bigEndian;		   // 0 on all supported targets+	uint8_t validationEnabled; // 1 if built with validation, only for diagnostics on a layout mismatch+	uint32_t reserved1;+	uint64_t snapshotSize; // bytes of snapshot blob after the header+} b2RecHeader;++_Static_assert( sizeof( b2RecHeader ) == 32, "recording header must be 32 bytes" );++// Growable append-only byte buffer. Doubles on demand. In countOnly mode it tallies size without+// allocating, so a serialize can be sized cheaply before a second pass fills a real buffer.+typedef struct b2RecBuffer+{+	uint8_t* data;+	int capacity;+	int size;+	bool countOnly;+} b2RecBuffer;++// User-owned recording buffer. The world appends into it while recording; the user saves and+// destroys it. Opaque across the public API.+typedef struct b2Recording+{+	b2RecBuffer buffer;+	int recordStart; // offset of the 3-byte size field for u24 backpatch+	b2Mutex* lock;	 // serializes query record commits across concurrent query threads++	// Union of world bounds over every recorded step, written out at stop so a replay can frame+	// the whole motion. haveBounds gates the first union the same way b2World_GetBounds does.+	b2AABB accumulatedBounds;+	bool haveBounds;+} b2Recording;++// C type aliases per TAG, used in codegen arg structs+typedef bool b2RecCType_BOOL;+typedef int32_t b2RecCType_I32;+typedef uint32_t b2RecCType_U32;+typedef uint64_t b2RecCType_U64;+typedef float b2RecCType_F32;+typedef b2Vec2 b2RecCType_VEC2;+typedef b2Rot b2RecCType_ROT;+typedef b2Transform b2RecCType_XF;+typedef b2Pos b2RecCType_POSITION;+typedef b2WorldTransform b2RecCType_WORLDXF;+typedef const char* b2RecCType_STR;+typedef b2WorldId b2RecCType_WORLDID;+typedef b2BodyId b2RecCType_BODYID;+typedef b2ShapeId b2RecCType_SHAPEID;+typedef b2ChainId b2RecCType_CHAINID;+typedef b2JointId b2RecCType_JOINTID;+typedef b2Circle b2RecCType_CIRCLE;+typedef b2Capsule b2RecCType_CAPSULE;+typedef b2Segment b2RecCType_SEGMENT;+typedef b2Polygon b2RecCType_POLYGON;+typedef b2ChainSegment b2RecCType_CHAINSEG;+typedef b2Filter b2RecCType_FILTER;+typedef b2SurfaceMaterial b2RecCType_MATERIAL;+typedef b2MassData b2RecCType_MASSDATA;+typedef b2MotionLocks b2RecCType_LOCKS;+typedef b2ExplosionDef b2RecCType_EXPLOSIONDEF;+typedef b2BodyDef b2RecCType_BODYDEF;+typedef b2ShapeDef b2RecCType_SHAPEDEF;+typedef b2ChainDef b2RecCType_CHAINDEF;+typedef b2DistanceJointDef b2RecCType_DISTANCEJOINTDEF;+typedef b2FilterJointDef b2RecCType_FILTERJOINTDEF;+typedef b2MotorJointDef b2RecCType_MOTORJOINTDEF;+typedef b2MoverJointDef b2RecCType_MOVERJOINTDEF;+typedef b2PogoJointDef b2RecCType_POGOJOINTDEF;+typedef b2PrismaticJointDef b2RecCType_PRISMATICJOINTDEF;+typedef b2RevoluteJointDef b2RecCType_REVOLUTEJOINTDEF;+typedef b2WeldJointDef b2RecCType_WELDJOINTDEF;+typedef b2WheelJointDef b2RecCType_WHEELJOINTDEF;+typedef b2AABB b2RecCType_AABB;+typedef b2QueryFilter b2RecCType_QUERYFILTER;+typedef b2ShapeProxy b2RecCType_SHAPEPROXY;++// Codegen pass 1a: arg structs, generated in recording.c, declared here for call sites.+// These are typedef'd in recording.c before the write helpers, but must be visible+// in body.c and shape.c which use B2_REC. We generate them via the X-macro here.+// IMPORTANT: this block must not expand ARG or B2_REC_OP as functions.+//+// For example, this:+// B2_REC_OP( 0x80, Step, RET_NONE, ARG( WORLDID, world ) ARG( F32, dt ) ARG( I32, subStepCount ) )+// Becomes:+// typedef struct+// {+//   b2RecCType_WORLDID world;+//   b2RecCType_F32 dt;+//   b2RecCType_I32 subStepCount;+// } b2RecArgs_Step;+// Which are the arguments to b2World_Step+#define ARG( TAG, field ) b2RecCType_##TAG field;+#define B2_REC_OP( op, Name, RET, ... )                                                                                          \+	typedef struct                                                                                                               \+	{                                                                                                                            \+		__VA_ARGS__                                                                                                              \+	} b2RecArgs_##Name;+#include "recording_ops.inl"+#undef B2_REC_OP+#undef ARG++// Low level buffer helpers+void b2RecBufAppend( b2RecBuffer* buf, const void* data, int size );+void b2RecBufFree( b2RecBuffer* buf );++// Write primitives+void b2RecW_U8( b2RecBuffer* buf, uint8_t v );+void b2RecW_U16( b2RecBuffer* buf, uint16_t v );+void b2RecW_U32( b2RecBuffer* buf, uint32_t v );+void b2RecW_U64( b2RecBuffer* buf, uint64_t v );+void b2RecW_I32( b2RecBuffer* buf, int32_t v );+void b2RecW_F32( b2RecBuffer* buf, float v );+void b2RecW_BOOL( b2RecBuffer* buf, bool v );+void b2RecW_VEC2( b2RecBuffer* buf, b2Vec2 v );+void b2RecW_ROT( b2RecBuffer* buf, b2Rot v );+void b2RecW_XF( b2RecBuffer* buf, b2Transform v );+void b2RecW_F64( b2RecBuffer* buf, double v );+// World position and world transform. Two doubles per position in large world mode, two floats+// otherwise so the wire stays byte-identical to VEC2 / XF in the float build.+void b2RecW_POSITION( b2RecBuffer* buf, b2Pos v );+void b2RecW_WORLDXF( b2RecBuffer* buf, b2WorldTransform v );+void b2RecW_WORLDID( b2RecBuffer* buf, b2WorldId v );+void b2RecW_BODYID( b2RecBuffer* buf, b2BodyId v );+void b2RecW_SHAPEID( b2RecBuffer* buf, b2ShapeId v );+void b2RecW_CHAINID( b2RecBuffer* buf, b2ChainId v );+void b2RecW_JOINTID( b2RecBuffer* buf, b2JointId v );+void b2RecW_CIRCLE( b2RecBuffer* buf, b2Circle v );+void b2RecW_CAPSULE( b2RecBuffer* buf, b2Capsule v );+void b2RecW_SEGMENT( b2RecBuffer* buf, b2Segment v );+void b2RecW_POLYGON( b2RecBuffer* buf, b2Polygon v );+void b2RecW_CHAINSEG( b2RecBuffer* buf, b2ChainSegment v );+void b2RecW_FILTER( b2RecBuffer* buf, b2Filter v );+void b2RecW_MATERIAL( b2RecBuffer* buf, b2SurfaceMaterial v );+void b2RecW_MASSDATA( b2RecBuffer* buf, b2MassData v );+void b2RecW_LOCKS( b2RecBuffer* buf, b2MotionLocks v );+void b2RecW_STR( b2RecBuffer* buf, const char* s );+void b2RecW_EXPLOSIONDEF( b2RecBuffer* buf, b2ExplosionDef v );+void b2RecW_BODYDEF( b2RecBuffer* buf, b2BodyDef v );+void b2RecW_SHAPEDEF( b2RecBuffer* buf, b2ShapeDef v );+void b2RecW_CHAINDEF( b2RecBuffer* buf, b2ChainDef v );+void b2RecW_DISTANCEJOINTDEF( b2RecBuffer* buf, b2DistanceJointDef v );+void b2RecW_FILTERJOINTDEF( b2RecBuffer* buf, b2FilterJointDef v );+void b2RecW_MOTORJOINTDEF( b2RecBuffer* buf, b2MotorJointDef v );+void b2RecW_MOVERJOINTDEF( b2RecBuffer* buf, b2MoverJointDef v );+void b2RecW_POGOJOINTDEF( b2RecBuffer* buf, b2PogoJointDef v );+void b2RecW_PRISMATICJOINTDEF( b2RecBuffer* buf, b2PrismaticJointDef v );+void b2RecW_REVOLUTEJOINTDEF( b2RecBuffer* buf, b2RevoluteJointDef v );+void b2RecW_WELDJOINTDEF( b2RecBuffer* buf, b2WeldJointDef v );+void b2RecW_WHEELJOINTDEF( b2RecBuffer* buf, b2WheelJointDef v );+void b2RecW_AABB( b2RecBuffer* buf, b2AABB v );+void b2RecW_QUERYFILTER( b2RecBuffer* buf, b2QueryFilter v );+void b2RecW_SHAPEPROXY( b2RecBuffer* buf, b2ShapeProxy v );+void b2RecW_RAYRESULT( b2RecBuffer* buf, b2RayResult v );+void b2RecW_PLANERESULT( b2RecBuffer* buf, b2PlaneResult v );+void b2RecW_TREESTATS( b2RecBuffer* buf, b2TreeStats v );++// Record framing+void b2RecBeginRecord( b2Recording* rec, uint8_t opcode );+void b2RecEndRecord( b2Recording* rec );++// Per op arg writers (no framing) and full writers (framing plus args), generated from the+// manifest. Create ops reach the arg writer directly so the call site can append the returned+// id inside the same record; void ops reach the full writer through B2_REC.+#define B2_REC_OP( op, Name, RET, ... )                                                                                          \+	void b2RecWriteArgs_##Name( b2Recording* rec, const b2RecArgs_##Name* a );                                                   \+	void b2RecWrite_##Name( b2Recording* rec, const b2RecArgs_##Name* a );+#include "recording_ops.inl"+#undef B2_REC_OP++// Create ops also need a writer that appends the returned id inside the same record.+// Generated only for ops carrying a RET tag. The id type comes from that tag.+// Recording the returned ids allows verification of deterministic replay.+#define B2_REC_RETDECL_RET_NONE( Name )+#define B2_REC_RETDECL_RET_BODYID( Name ) void b2RecWriteRet_##Name( b2Recording* rec, const b2RecArgs_##Name* a, b2BodyId id );+#define B2_REC_RETDECL_RET_SHAPEID( Name ) void b2RecWriteRet_##Name( b2Recording* rec, const b2RecArgs_##Name* a, b2ShapeId id );+#define B2_REC_RETDECL_RET_CHAINID( Name ) void b2RecWriteRet_##Name( b2Recording* rec, const b2RecArgs_##Name* a, b2ChainId id );+#define B2_REC_RETDECL_RET_JOINTID( Name ) void b2RecWriteRet_##Name( b2Recording* rec, const b2RecArgs_##Name* a, b2JointId id );+#define B2_REC_OP( op, Name, RET, ... ) B2_REC_RETDECL_##RET( Name )+#include "recording_ops.inl"+#undef B2_REC_OP+#undef B2_REC_RETDECL_RET_NONE+#undef B2_REC_RETDECL_RET_BODYID+#undef B2_REC_RETDECL_RET_SHAPEID+#undef B2_REC_RETDECL_RET_CHAINID+#undef B2_REC_RETDECL_RET_JOINTID++// Record a void op. One branch when recording is off, args built inside the branch+#define B2_REC( world, Name, ... )                                                                                               \+	do                                                                                                                           \+	{                                                                                                                            \+		if ( ( world )->recording != NULL )                                                                                      \+		{                                                                                                                        \+			b2RecArgs_##Name _a = { __VA_ARGS__ };                                                                               \+			b2RecWrite_##Name( ( world )->recording, &_a );                                                                      \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++// Record a create op and its returned id in one framed record. The id is appended after+// the args so replay can assert it matches. Place this after the real create call.+#define B2_REC_CREATE( world, Name, id, ... )                                                                                    \+	do                                                                                                                           \+	{                                                                                                                            \+		if ( ( world )->recording != NULL )                                                                                      \+		{                                                                                                                        \+			b2RecArgs_##Name _ca = { __VA_ARGS__ };                                                                              \+			b2RecWriteRet_##Name( ( world )->recording, &_ca, id );                                                              \+		}                                                                                                                        \+	}                                                                                                                            \+	while ( 0 )++// Patch helpers for the query hit-count backfill+int b2RecReserveU32( b2RecBuffer* buf );+void b2RecPatchU32( b2RecBuffer* buf, int offset, uint32_t v );++// Commit a finished query record under the lock. The local buffer is still owned by the caller.+void b2RecCommitRecord( b2Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize );++// Per-query writer context: holds user fcn+ctx, the local payload buffer, and the hit counter+typedef struct b2RecQueryWriter+{+	union+	{+		b2OverlapResultCallbackFcn* overlapFcn;+		b2CastResultCallbackFcn* castFcn;+		b2PlaneResultCallbackFcn* planeFcn;+	} userFcn;+	void* userContext;+	b2RecBuffer buf; // per-call local payload, heap-backed+	int countOffset; // offset of the reserved u32 hit-count slot+	uint32_t hitCount;+} b2RecQueryWriter;++void b2RecQueryBegin( b2RecQueryWriter* w, void* context );+void b2RecQueryCommit( b2Recording* rec, uint8_t opcode, b2RecQueryWriter* w );++// Recording trampolines: replace the user fcn pointer so hits are captured before dispatch+bool b2RecOverlapTrampoline( b2ShapeId id, void* ctx );+float b2RecCastTrampoline( b2ShapeId id, b2Pos point, b2Vec2 normal, float fraction, void* ctx );+bool b2RecPlaneTrampoline( b2ShapeId id, const b2PlaneResult* plane, void* ctx );++// Lifecycle. Public create/destroy/save/load live in box2d.h; these are the engine-side hooks.+void b2StartRecordingIntoBuffer( b2World* world, b2Recording* recording );+void b2StopRecordingInternal( b2World* world );++// Fold one step's world bounds into the running union the recorder writes out at stop.+void b2RecAccumulateBounds( b2Recording* rec, b2AABB bounds );++// Deterministic hash over all body transforms and velocities.+// Called by both recorder and replayer to verify simulation reproduces exactly.+uint64_t b2HashWorldState( b2World* world );
+ upstream/src/recording_ops.inl view
@@ -0,0 +1,227 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++// X-macro manifest for the recording system.+// Include with B2_REC_OP and ARG defined. No commas between ARG tokens.+//+// B2_REC_OP( opcode, Name, RET, ARGS )+//   RET in { RET_NONE, RET_BODYID, RET_SHAPEID }+//   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+//   0x70-0x7F  chain+//   0x80       step+//   0x90-0xDF  joints (create, generic, per-type)+//   0xE0-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.+B2_REC_OP( 0x01, DestroyWorld, RET_NONE, ARG( WORLDID, world ) )+B2_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.+B2_REC_OP( 0x02, WorldEnableSleeping, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )+B2_REC_OP( 0x03, WorldEnableContinuous, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )+B2_REC_OP( 0x04, WorldSetRestitutionThreshold, RET_NONE, ARG( WORLDID, world ) ARG( F32, value ) )+B2_REC_OP( 0x05, WorldSetHitEventThreshold, RET_NONE, ARG( WORLDID, world ) ARG( F32, value ) )+B2_REC_OP( 0x06, WorldSetGravity, RET_NONE, ARG( WORLDID, world ) ARG( VEC2, gravity ) )+B2_REC_OP( 0x07, WorldExplode, RET_NONE, ARG( WORLDID, world ) ARG( EXPLOSIONDEF, def ) )+B2_REC_OP( 0x08, WorldSetContactTuning, RET_NONE,+		   ARG( WORLDID, world ) ARG( F32, hertz ) ARG( F32, dampingRatio ) ARG( F32, pushSpeed ) )+B2_REC_OP( 0x09, WorldSetContactRecycleDistance, RET_NONE, ARG( WORLDID, world ) ARG( F32, recycleDistance ) )+B2_REC_OP( 0x0A, WorldSetMaxLinearSpeed, RET_NONE, ARG( WORLDID, world ) ARG( F32, maximumLinearSpeed ) )+B2_REC_OP( 0x0B, WorldEnableWarmStarting, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )+B2_REC_OP( 0x0C, WorldRebuildStaticTree, RET_NONE, ARG( WORLDID, world ) )+B2_REC_OP( 0x0D, WorldSetRestitutionIterations, RET_NONE, ARG( WORLDID, world ) ARG( I32, iterations ) )+B2_REC_OP( 0x0E, WorldEnableRestitutionPropagation, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )++// Body+B2_REC_OP( 0x10, CreateBody, RET_BODYID, ARG( WORLDID, world ) ARG( BODYDEF, def ) )+B2_REC_OP( 0x11, DestroyBody, RET_NONE, ARG( BODYID, body ) )+B2_REC_OP( 0x20, BodySetTransform, RET_NONE, ARG( BODYID, body ) ARG( POSITION, position ) ARG( ROT, rotation ) )+B2_REC_OP( 0x21, BodySetLinearVelocity, RET_NONE, ARG( BODYID, body ) ARG( VEC2, v ) )+B2_REC_OP( 0x22, BodySetType, RET_NONE, ARG( BODYID, body ) ARG( I32, type ) )+B2_REC_OP( 0x23, BodySetName, RET_NONE, ARG( BODYID, body ) ARG( STR, name ) )+B2_REC_OP( 0x24, BodySetAngularVelocity, RET_NONE, ARG( BODYID, body ) ARG( F32, w ) )+B2_REC_OP( 0x25, BodySetTargetTransform, RET_NONE, ARG( BODYID, body ) ARG( WORLDXF, target ) ARG( F32, timeStep ) ARG( BOOL, wake ) )+B2_REC_OP( 0x26, BodyApplyForce, RET_NONE, ARG( BODYID, body ) ARG( VEC2, force ) ARG( POSITION, point ) ARG( BOOL, wake ) )+B2_REC_OP( 0x27, BodyApplyForceToCenter, RET_NONE, ARG( BODYID, body ) ARG( VEC2, force ) ARG( BOOL, wake ) )+B2_REC_OP( 0x28, BodyApplyTorque, RET_NONE, ARG( BODYID, body ) ARG( F32, torque ) ARG( BOOL, wake ) )+B2_REC_OP( 0x29, BodyClearForces, RET_NONE, ARG( BODYID, body ) )+B2_REC_OP( 0x2A, BodyApplyLinearImpulse, RET_NONE, ARG( BODYID, body ) ARG( VEC2, impulse ) ARG( POSITION, point ) ARG( BOOL, wake ) )+B2_REC_OP( 0x2B, BodyApplyLinearImpulseToCenter, RET_NONE, ARG( BODYID, body ) ARG( VEC2, impulse ) ARG( BOOL, wake ) )+B2_REC_OP( 0x2C, BodyApplyAngularImpulse, RET_NONE, ARG( BODYID, body ) ARG( F32, impulse ) ARG( BOOL, wake ) )+B2_REC_OP( 0x2D, BodySetMassData, RET_NONE, ARG( BODYID, body ) ARG( MASSDATA, massData ) )+B2_REC_OP( 0x2E, BodyUpdateMassFromShapes, RET_NONE, ARG( BODYID, body ) )+B2_REC_OP( 0x2F, BodySetLinearDamping, RET_NONE, ARG( BODYID, body ) ARG( F32, damping ) )+B2_REC_OP( 0x30, BodySetAngularDamping, RET_NONE, ARG( BODYID, body ) ARG( F32, damping ) )+B2_REC_OP( 0x31, BodySetGravityScale, RET_NONE, ARG( BODYID, body ) ARG( F32, scale ) )+B2_REC_OP( 0x32, BodySetAwake, RET_NONE, ARG( BODYID, body ) ARG( BOOL, awake ) )+B2_REC_OP( 0x33, BodyWakeTouching, RET_NONE, ARG( BODYID, body ) )+B2_REC_OP( 0x34, BodyEnableSleeping, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )+B2_REC_OP( 0x35, BodySetSleepThreshold, RET_NONE, ARG( BODYID, body ) ARG( F32, threshold ) )+B2_REC_OP( 0x36, BodyDisable, RET_NONE, ARG( BODYID, body ) )+B2_REC_OP( 0x37, BodyEnable, RET_NONE, ARG( BODYID, body ) )+B2_REC_OP( 0x38, BodySetMotionLocks, RET_NONE, ARG( BODYID, body ) ARG( LOCKS, locks ) )+B2_REC_OP( 0x39, BodySetBullet, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )+B2_REC_OP( 0x3A, BodyEnableContactRecycling, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )+B2_REC_OP( 0x3B, BodyEnableContactEvents, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )+B2_REC_OP( 0x3C, BodyEnableHitEvents, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )+B2_REC_OP( 0x3D, BodySetSafetyFactor, RET_NONE, ARG( BODYID, body ) ARG( F32, value ) )++// Shape create/destroy+B2_REC_OP( 0x40, CreateCircleShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( CIRCLE, circle ) )+B2_REC_OP( 0x41, CreateCapsuleShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( CAPSULE, capsule ) )+B2_REC_OP( 0x42, CreateSegmentShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( SEGMENT, segment ) )+B2_REC_OP( 0x43, CreatePolygonShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( POLYGON, polygon ) )+B2_REC_OP( 0x44, CreateChainSegmentShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( CHAINSEG, chainSegment ) )+B2_REC_OP( 0x45, DestroyShape, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, updateBodyMass ) )++// Shape mutators+B2_REC_OP( 0x50, ShapeSetDensity, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, density ) ARG( BOOL, updateBodyMass ) )+B2_REC_OP( 0x51, ShapeSetFriction, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, friction ) )+B2_REC_OP( 0x52, ShapeSetRestitution, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, restitution ) )+B2_REC_OP( 0x53, ShapeSetUserMaterial, RET_NONE, ARG( SHAPEID, shape ) ARG( U64, material ) )+B2_REC_OP( 0x54, ShapeSetSurfaceMaterial, RET_NONE, ARG( SHAPEID, shape ) ARG( MATERIAL, material ) )+B2_REC_OP( 0x55, ShapeSetFilter, RET_NONE, ARG( SHAPEID, shape ) ARG( FILTER, filter ) )+B2_REC_OP( 0x56, ShapeEnableSensorEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )+B2_REC_OP( 0x57, ShapeEnableContactEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )+B2_REC_OP( 0x58, ShapeEnablePreSolveEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )+B2_REC_OP( 0x59, ShapeEnableHitEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )+B2_REC_OP( 0x5A, ShapeSetCircle, RET_NONE, ARG( SHAPEID, shape ) ARG( CIRCLE, circle ) )+B2_REC_OP( 0x5B, ShapeSetCapsule, RET_NONE, ARG( SHAPEID, shape ) ARG( CAPSULE, capsule ) )+B2_REC_OP( 0x5C, ShapeSetSegment, RET_NONE, ARG( SHAPEID, shape ) ARG( SEGMENT, segment ) )+B2_REC_OP( 0x5D, ShapeSetPolygon, RET_NONE, ARG( SHAPEID, shape ) ARG( POLYGON, polygon ) )+B2_REC_OP( 0x5E, ShapeSetChainSegment, RET_NONE, ARG( SHAPEID, shape ) ARG( CHAINSEG, chainSegment ) )+B2_REC_OP( 0x5F, ShapeApplyWind, RET_NONE,+		   ARG( SHAPEID, shape ) ARG( VEC2, wind ) ARG( F32, drag ) ARG( F32, lift ) ARG( BOOL, wake ) )++// Chain+B2_REC_OP( 0x70, CreateChain, RET_CHAINID, ARG( BODYID, body ) ARG( CHAINDEF, def ) )+B2_REC_OP( 0x71, DestroyChain, RET_NONE, ARG( CHAINID, chain ) )+B2_REC_OP( 0x72, ChainSetSurfaceMaterial, RET_NONE, ARG( CHAINID, chain ) ARG( MATERIAL, material ) ARG( I32, segmentIndex ) )+B2_REC_OP( 0x73, ChainSetAllSurfaceMaterials, RET_NONE, ARG( CHAINID, chain ) ARG( MATERIAL, material ) )++// Joint destroy+B2_REC_OP( 0x90, DestroyJoint, RET_NONE, ARG( JOINTID, joint ) )++// Generic joint mutators+B2_REC_OP( 0x91, JointSetLocalFrameA, RET_NONE, ARG( JOINTID, joint ) ARG( XF, localFrame ) )+B2_REC_OP( 0x92, JointSetLocalFrameB, RET_NONE, ARG( JOINTID, joint ) ARG( XF, localFrame ) )+B2_REC_OP( 0x93, JointSetCollideConnected, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, shouldCollide ) )+B2_REC_OP( 0x94, JointWakeBodies, RET_NONE, ARG( JOINTID, joint ) )+B2_REC_OP( 0x95, JointSetConstraintTuning, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ARG( F32, dampingRatio ) )+B2_REC_OP( 0x96, JointSetForceThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) )+B2_REC_OP( 0x97, JointSetTorqueThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) )++// Distance joint+B2_REC_OP( 0x98, CreateDistanceJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( DISTANCEJOINTDEF, def ) )+B2_REC_OP( 0x99, DistanceJointSetRestLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) )+B2_REC_OP( 0x9A, DistanceJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )+B2_REC_OP( 0x9B, DistanceJointSetSpringForceRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lowerForce ) ARG( F32, upperForce ) )+B2_REC_OP( 0x9C, DistanceJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B2_REC_OP( 0x9D, DistanceJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B2_REC_OP( 0x9E, DistanceJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )+B2_REC_OP( 0x9F, DistanceJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, minLength ) ARG( F32, maxLength ) )+B2_REC_OP( 0xA0, DistanceJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )+B2_REC_OP( 0xA1, DistanceJointSetTargetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )+B2_REC_OP( 0xA2, DistanceJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) )++// Filter joint+B2_REC_OP( 0xA3, CreateFilterJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( FILTERJOINTDEF, def ) )++// Motor joint+B2_REC_OP( 0xA4, CreateMotorJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOTORJOINTDEF, def ) )+B2_REC_OP( 0xA5, MotorJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC2, velocity ) )+B2_REC_OP( 0xA6, MotorJointSetAngularVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( F32, velocity ) )+B2_REC_OP( 0xA7, MotorJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) )+B2_REC_OP( 0xA8, MotorJointSetMaxVelocityTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) )+B2_REC_OP( 0xA9, MotorJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B2_REC_OP( 0xAA, MotorJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) )+B2_REC_OP( 0xAB, MotorJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B2_REC_OP( 0xAC, MotorJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) )+B2_REC_OP( 0xAD, MotorJointSetMaxSpringForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) )+B2_REC_OP( 0xAE, MotorJointSetMaxSpringTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) )++// Mover joint+B2_REC_OP( 0xAF, CreateMoverJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOVERJOINTDEF, def ) )+B2_REC_OP( 0xB0, MoverJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC2, velocity ) )+B2_REC_OP( 0xB1, MoverJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( VEC2, maxForce ) )++// Pogo joint+B2_REC_OP( 0xB2, CreatePogoJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( POGOJOINTDEF, def ) )+B2_REC_OP( 0xB3, PogoJointSetRestLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) )+B2_REC_OP( 0xB4, PogoJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B2_REC_OP( 0xB5, PogoJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )++// Prismatic joint+B2_REC_OP( 0xB6, CreatePrismaticJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( PRISMATICJOINTDEF, def ) )+B2_REC_OP( 0xB7, PrismaticJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )+B2_REC_OP( 0xB8, PrismaticJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B2_REC_OP( 0xB9, PrismaticJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B2_REC_OP( 0xBA, PrismaticJointSetTargetTranslation, RET_NONE, ARG( JOINTID, joint ) ARG( F32, translation ) )+B2_REC_OP( 0xBB, PrismaticJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )+B2_REC_OP( 0xBC, PrismaticJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )+B2_REC_OP( 0xBD, PrismaticJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )+B2_REC_OP( 0xBE, PrismaticJointSetTargetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )+B2_REC_OP( 0xBF, PrismaticJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) )++// Revolute joint+B2_REC_OP( 0xC0, CreateRevoluteJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( REVOLUTEJOINTDEF, def ) )+B2_REC_OP( 0xC1, RevoluteJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )+B2_REC_OP( 0xC2, RevoluteJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B2_REC_OP( 0xC3, RevoluteJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B2_REC_OP( 0xC4, RevoluteJointSetTargetAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, angle ) )+B2_REC_OP( 0xC5, RevoluteJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )+B2_REC_OP( 0xC6, RevoluteJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )+B2_REC_OP( 0xC7, RevoluteJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )+B2_REC_OP( 0xC8, RevoluteJointSetTargetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )+B2_REC_OP( 0xC9, RevoluteJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )++// Weld joint+B2_REC_OP( 0xCA, CreateWeldJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WELDJOINTDEF, def ) )+B2_REC_OP( 0xCB, WeldJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B2_REC_OP( 0xCC, WeldJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B2_REC_OP( 0xCD, WeldJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B2_REC_OP( 0xCE, WeldJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )++// Wheel joint+B2_REC_OP( 0xCF, CreateWheelJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WHEELJOINTDEF, def ) )+B2_REC_OP( 0xD0, WheelJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )+B2_REC_OP( 0xD1, WheelJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )+B2_REC_OP( 0xD2, WheelJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )+B2_REC_OP( 0xD3, WheelJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )+B2_REC_OP( 0xD4, WheelJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )+B2_REC_OP( 0xD5, WheelJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )+B2_REC_OP( 0xD6, WheelJointSetTargetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )+B2_REC_OP( 0xD7, WheelJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )++// Spatial queries. Inputs through the manifest (reader side). Hit tail / results hand-written.+// Query geometry is relative to the origin so recordings reproduce queries far from the world origin.+B2_REC_OP( 0xE0, QueryOverlapAABB, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( AABB, aabb ) ARG( QUERYFILTER, filter ) )+B2_REC_OP( 0xE1, QueryOverlapShape, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( SHAPEPROXY, proxy ) ARG( QUERYFILTER, filter ) )+B2_REC_OP( 0xE2, QueryCastRay, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( VEC2, translation ) ARG( QUERYFILTER, filter ) )+B2_REC_OP( 0xE3, QueryCastShape, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( SHAPEPROXY, proxy ) ARG( VEC2, translation )+			   ARG( QUERYFILTER, filter ) )+B2_REC_OP( 0xE4, QueryCollideMover, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( CAPSULE, mover ) ARG( QUERYFILTER, filter ) )+B2_REC_OP( 0xE5, QueryCastRayClosest, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( VEC2, translation ) ARG( QUERYFILTER, filter ) )+B2_REC_OP( 0xE6, QueryCastMover, RET_NONE,+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( CAPSULE, mover ) ARG( VEC2, translation )+			   ARG( QUERYFILTER, filter ) )++B2_REC_OP( 0xF1, StateHash, RET_NONE, ARG( WORLDID, world ) ARG( U64, hash ) )++// Accumulated world bounds over the whole recording, written once at stop. Informational.+B2_REC_OP( 0xF2, RecordingBounds, RET_NONE, ARG( AABB, bounds ) )
+ upstream/src/replay.c view
@@ -0,0 +1,2848 @@+// 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 "replay.h"++#include "body.h"+#include "physics_world.h"+#include "snapshot.h"++#include "box2d/box2d.h"++#include <limits.h>+#include <stdio.h>+#include <string.h>++// Keyframe ring tuning. A memory budget caps the snapshots kept. The spacing starts at the min and+// doubles when adding the next keyframe would exceed the budget, so memory stays bounded and seek+// cost grows only once a recording outgrows the budget.+#define B2_REC_KEYFRAME_INTERVAL_DEFAULT 16+#define B2_REC_KEYFRAME_BUDGET_DEFAULT ( 512 * 1024 * 1024 )++// Read primitives++static void b2RecRdrCheck( b2RecReader* rdr, int size )+{+	// 64-bit compare so a corrupt or oversized size can't overflow the cursor add+	if ( size < 0 || (int64_t)rdr->cursor + (int64_t)size > (int64_t)rdr->size )+	{+		rdr->ok = false;+	}+}++static bool b2RecReserveScratch( b2RecReader* rdr, void** data, int* cap, int need, int elemSize );++uint8_t b2RecR_U8( b2RecReader* rdr )+{+	b2RecRdrCheck( rdr, 1 );+	if ( !rdr->ok )+	{+		return 0;+	}+	return rdr->data[rdr->cursor++];+}++uint16_t b2RecR_U16( b2RecReader* rdr )+{+	b2RecRdrCheck( 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 b2RecR_U24( b2RecReader* rdr )+{+	b2RecRdrCheck( 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 b2RecR_U32( b2RecReader* rdr )+{+	b2RecRdrCheck( 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 b2RecR_U64( b2RecReader* rdr )+{+	b2RecRdrCheck( 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 b2RecR_I32( b2RecReader* rdr )+{+	return (int32_t)b2RecR_U32( rdr );+}++float b2RecR_F32( b2RecReader* rdr )+{+	uint32_t bits = b2RecR_U32( rdr );+	float v;+	memcpy( &v, &bits, 4 );+	return v;+}++bool b2RecR_BOOL( b2RecReader* rdr )+{+	return b2RecR_U8( rdr ) != 0u;+}++b2Vec2 b2RecR_VEC2( b2RecReader* rdr )+{+	b2Vec2 v;+	v.x = b2RecR_F32( rdr );+	v.y = b2RecR_F32( rdr );+	return v;+}++b2Rot b2RecR_ROT( b2RecReader* rdr )+{+	b2Rot r;+	r.c = b2RecR_F32( rdr );+	r.s = b2RecR_F32( rdr );+	return r;+}++b2Transform b2RecR_XF( b2RecReader* rdr )+{+	b2Transform xf;+	xf.p = b2RecR_VEC2( rdr );+	xf.q = b2RecR_ROT( rdr );+	return xf;+}++double b2RecR_F64( b2RecReader* rdr )+{+	uint64_t bits = b2RecR_U64( rdr );+	double v;+	memcpy( &v, &bits, 8 );+	return v;+}++b2Pos b2RecR_POSITION( b2RecReader* rdr )+{+	b2Pos p;+#if defined( BOX2D_DOUBLE_PRECISION )+	p.x = b2RecR_F64( rdr );+	p.y = b2RecR_F64( rdr );+#else+	p.x = b2RecR_F32( rdr );+	p.y = b2RecR_F32( rdr );+#endif+	return p;+}++b2WorldTransform b2RecR_WORLDXF( b2RecReader* rdr )+{+	b2WorldTransform t;+	t.p = b2RecR_POSITION( rdr );+	t.q = b2RecR_ROT( rdr );+	return t;+}++b2WorldId b2RecR_WORLDID( b2RecReader* rdr )+{+	return b2LoadWorldId( b2RecR_U32( rdr ) );+}++b2BodyId b2RecR_BODYID( b2RecReader* rdr )+{+	return b2LoadBodyId( b2RecR_U64( rdr ) );+}++b2ShapeId b2RecR_SHAPEID( b2RecReader* rdr )+{+	return b2LoadShapeId( b2RecR_U64( rdr ) );+}++b2ChainId b2RecR_CHAINID( b2RecReader* rdr )+{+	return b2LoadChainId( b2RecR_U64( rdr ) );+}++b2JointId b2RecR_JOINTID( b2RecReader* rdr )+{+	return b2LoadJointId( b2RecR_U64( rdr ) );+}++// Read a pointer-free POD blob of the given size into out, advancing the cursor.+// Zeroes out on overrun so a truncated file fails the read check rather than reading garbage.+static void b2RecRdrBlob( b2RecReader* rdr, void* out, int size )+{+	b2RecRdrCheck( rdr, size );+	if ( !rdr->ok )+	{+		memset( out, 0, (size_t)size );+		return;+	}+	memcpy( out, rdr->data + rdr->cursor, (size_t)size );+	rdr->cursor += size;+}++b2Circle b2RecR_CIRCLE( b2RecReader* rdr )+{+	b2Circle c;+	b2RecRdrBlob( rdr, &c, (int)sizeof( c ) );+	return c;+}++b2Capsule b2RecR_CAPSULE( b2RecReader* rdr )+{+	b2Capsule c;+	b2RecRdrBlob( rdr, &c, (int)sizeof( c ) );+	return c;+}++b2Segment b2RecR_SEGMENT( b2RecReader* rdr )+{+	b2Segment s;+	b2RecRdrBlob( rdr, &s, (int)sizeof( s ) );+	return s;+}++b2Polygon b2RecR_POLYGON( b2RecReader* rdr )+{+	b2Polygon p;+	b2RecRdrBlob( rdr, &p, (int)sizeof( p ) );+	return p;+}++b2ChainSegment b2RecR_CHAINSEG( b2RecReader* rdr )+{+	b2ChainSegment cs;+	b2RecRdrBlob( rdr, &cs, (int)sizeof( cs ) );+	return cs;+}++b2Filter b2RecR_FILTER( b2RecReader* rdr )+{+	b2Filter f;+	f.categoryBits = b2RecR_U64( rdr );+	f.maskBits = b2RecR_U64( rdr );+	f.groupIndex = b2RecR_I32( rdr );+	return f;+}++b2SurfaceMaterial b2RecR_MATERIAL( b2RecReader* rdr )+{+	b2SurfaceMaterial m = b2DefaultSurfaceMaterial();+	m.friction = b2RecR_F32( rdr );+	m.restitution = b2RecR_F32( rdr );+	m.rollingResistance = b2RecR_F32( rdr );+	m.tangentSpeed = b2RecR_F32( rdr );+	m.userMaterialId = b2RecR_U64( rdr );+	m.customColor = b2RecR_U32( rdr );+	return m;+}++b2MassData b2RecR_MASSDATA( b2RecReader* rdr )+{+	b2MassData md;+	md.mass = b2RecR_F32( rdr );+	md.center = b2RecR_VEC2( rdr );+	md.rotationalInertia = b2RecR_F32( rdr );+	return md;+}++b2MotionLocks b2RecR_LOCKS( b2RecReader* rdr )+{+	b2MotionLocks locks;+	locks.linearX = b2RecR_BOOL( rdr );+	locks.linearY = b2RecR_BOOL( rdr );+	locks.angularZ = b2RecR_BOOL( rdr );+	return locks;+}++// Returns a pointer into a rotating set of static buffers, valid until the next several+// STR reads. Only used to pass a name straight into a create/setter call during dispatch.+const char* b2RecR_STR( b2RecReader* rdr )+{+	static char s_bufs[4][B2_NAME_LENGTH + 1];+	static int s_next = 0;+	char* buf = s_bufs[s_next];+	s_next = ( s_next + 1 ) & 3;++	uint16_t len = b2RecR_U16( rdr );+	if ( len == 0xFFFFu )+	{+		return NULL;+	}++	int n = (int)len;+	if ( n > B2_NAME_LENGTH )+	{+		n = B2_NAME_LENGTH;+	}+	b2RecRdrCheck( rdr, (int)len );+	if ( rdr->ok && n > 0 )+	{+		memcpy( buf, rdr->data + rdr->cursor, (size_t)n );+	}+	// Skip the full recorded length even if it exceeds the clamp+	rdr->cursor += (int)len;+	buf[n] = '\0';+	return buf;+}++// Def readers: start from b2Default*Def() to get cookie/internalValue, then overlay fields++b2BodyDef b2RecR_BODYDEF( b2RecReader* rdr )+{+	b2BodyDef def = b2DefaultBodyDef();+	def.type = (b2BodyType)b2RecR_I32( rdr );+	def.position = b2RecR_POSITION( rdr );+	def.rotation = b2RecR_ROT( rdr );+	def.linearVelocity = b2RecR_VEC2( rdr );+	def.angularVelocity = b2RecR_F32( rdr );+	def.linearDamping = b2RecR_F32( rdr );+	def.angularDamping = b2RecR_F32( rdr );+	def.gravityScale = b2RecR_F32( rdr );+	def.sleepThreshold = b2RecR_F32( rdr );+	def.safetyFactor = b2RecR_F32( rdr );++	// b2RecR_STR handles the over-length clamp and skips the full recorded length, so the+	// cursor stays aligned even for names longer than B2_NAME_LENGTH. Valid until the create call.+	def.name = b2RecR_STR( rdr );++	(void)b2RecR_U64( rdr ); // userData (not preserved)+	def.motionLocks = b2RecR_LOCKS( rdr );+	def.enableSleep = b2RecR_BOOL( rdr );+	def.isAwake = b2RecR_BOOL( rdr );+	def.isBullet = b2RecR_BOOL( rdr );+	def.isEnabled = b2RecR_BOOL( rdr );+	def.allowFastRotation = b2RecR_BOOL( rdr );+	def.enableContactRecycling = b2RecR_BOOL( rdr );+	def.userData = NULL;+	return def;+}++b2ShapeDef b2RecR_SHAPEDEF( b2RecReader* rdr )+{+	b2ShapeDef def = b2DefaultShapeDef();+	(void)b2RecR_U64( rdr ); // userData (not preserved)+	def.material = b2RecR_MATERIAL( rdr );+	def.density = b2RecR_F32( rdr );+	def.filter = b2RecR_FILTER( rdr );+	def.enableCustomFiltering = b2RecR_BOOL( rdr );+	def.isSensor = b2RecR_BOOL( rdr );+	def.enableSensorEvents = b2RecR_BOOL( rdr );+	def.enableContactEvents = b2RecR_BOOL( rdr );+	def.enableHitEvents = b2RecR_BOOL( rdr );+	def.enablePreSolveEvents = b2RecR_BOOL( rdr );+	def.invokeContactCreation = b2RecR_BOOL( rdr );+	def.updateBodyMass = b2RecR_BOOL( rdr );+	def.userData = NULL;+	return def;+}++b2ChainDef b2RecR_CHAINDEF( b2RecReader* rdr )+{+	b2ChainDef def = b2DefaultChainDef();+	(void)b2RecR_U64( rdr ); // userData (not preserved)++	int pointCount = b2RecR_I32( rdr );+	if ( pointCount < 0 )+	{+		pointCount = 0;+	}+	if ( b2RecReserveScratch( rdr, (void**)&rdr->chainPoints, &rdr->chainPointCap, pointCount, (int)sizeof( b2Vec2 ) ) == false )+	{+		pointCount = 0; // corrupt count, the read has already failed+	}+	for ( int i = 0; i < pointCount; ++i )+	{+		rdr->chainPoints[i] = b2RecR_VEC2( rdr );+	}+	def.points = pointCount > 0 ? rdr->chainPoints : NULL;+	def.pointCount = pointCount;+	def.ghost1 = b2RecR_VEC2( rdr );+	def.ghost2 = b2RecR_VEC2( rdr );++	int materialCount = b2RecR_I32( rdr );+	if ( materialCount < 0 )+	{+		materialCount = 0;+	}+	if ( b2RecReserveScratch( rdr, (void**)&rdr->chainMaterials, &rdr->chainMaterialCap, materialCount,+							  (int)sizeof( b2SurfaceMaterial ) ) == false )+	{+		materialCount = 0;+	}+	for ( int i = 0; i < materialCount; ++i )+	{+		rdr->chainMaterials[i] = b2RecR_MATERIAL( rdr );+	}+	def.materials = materialCount > 0 ? rdr->chainMaterials : NULL;+	def.materialCount = materialCount;++	def.filter = b2RecR_FILTER( rdr );+	def.isLoop = b2RecR_BOOL( rdr );+	def.enableSensorEvents = b2RecR_BOOL( rdr );+	def.userData = NULL;+	return def;+}++b2ExplosionDef b2RecR_EXPLOSIONDEF( b2RecReader* rdr )+{+	b2ExplosionDef def = b2DefaultExplosionDef();+	def.maskBits = b2RecR_U64( rdr );+	def.position = b2RecR_POSITION( rdr );+	def.radius = b2RecR_F32( rdr );+	def.falloff = b2RecR_F32( rdr );+	def.impulsePerLength = b2RecR_F32( rdr );+	return def;+}++// Body ids are read with their recorded world0; the create dispatcher remaps them.+static void b2RecR_JointBase( b2RecReader* rdr, b2JointDef* base )+{+	(void)b2RecR_U64( rdr ); // userData+	base->bodyIdA = b2RecR_BODYID( rdr );+	base->bodyIdB = b2RecR_BODYID( rdr );+	base->localFrameA = b2RecR_XF( rdr );+	base->localFrameB = b2RecR_XF( rdr );+	base->forceThreshold = b2RecR_F32( rdr );+	base->torqueThreshold = b2RecR_F32( rdr );+	base->constraintHertz = b2RecR_F32( rdr );+	base->constraintDampingRatio = b2RecR_F32( rdr );+	base->drawScale = b2RecR_F32( rdr );+	base->collideConnected = b2RecR_BOOL( rdr );+	base->userData = NULL;+}++b2DistanceJointDef b2RecR_DISTANCEJOINTDEF( b2RecReader* rdr )+{+	b2DistanceJointDef def = b2DefaultDistanceJointDef();+	b2RecR_JointBase( rdr, &def.base );+	def.length = b2RecR_F32( rdr );+	def.enableSpring = b2RecR_BOOL( rdr );+	def.lowerSpringForce = b2RecR_F32( rdr );+	def.upperSpringForce = b2RecR_F32( rdr );+	def.hertz = b2RecR_F32( rdr );+	def.dampingRatio = b2RecR_F32( rdr );+	def.enableLimit = b2RecR_BOOL( rdr );+	def.minLength = b2RecR_F32( rdr );+	def.maxLength = b2RecR_F32( rdr );+	def.enableMotor = b2RecR_BOOL( rdr );+	def.maxMotorForce = b2RecR_F32( rdr );+	def.motorSpeed = b2RecR_F32( rdr );+	return def;+}++b2FilterJointDef b2RecR_FILTERJOINTDEF( b2RecReader* rdr )+{+	b2FilterJointDef def = b2DefaultFilterJointDef();+	b2RecR_JointBase( rdr, &def.base );+	return def;+}++b2MotorJointDef b2RecR_MOTORJOINTDEF( b2RecReader* rdr )+{+	b2MotorJointDef def = b2DefaultMotorJointDef();+	b2RecR_JointBase( rdr, &def.base );+	def.linearVelocity = b2RecR_VEC2( rdr );+	def.maxVelocityForce = b2RecR_F32( rdr );+	def.angularVelocity = b2RecR_F32( rdr );+	def.maxVelocityTorque = b2RecR_F32( rdr );+	def.linearHertz = b2RecR_F32( rdr );+	def.linearDampingRatio = b2RecR_F32( rdr );+	def.maxSpringForce = b2RecR_F32( rdr );+	def.angularHertz = b2RecR_F32( rdr );+	def.angularDampingRatio = b2RecR_F32( rdr );+	def.maxSpringTorque = b2RecR_F32( rdr );+	return def;+}++b2MoverJointDef b2RecR_MOVERJOINTDEF( b2RecReader* rdr )+{+	b2MoverJointDef def = b2DefaultMoverJointDef();+	b2RecR_JointBase( rdr, &def.base );+	def.linearVelocity = b2RecR_VEC2( rdr );+	def.maxVelocityForce = b2RecR_VEC2( rdr );+	return def;+}++b2PrismaticJointDef b2RecR_PRISMATICJOINTDEF( b2RecReader* rdr )+{+	b2PrismaticJointDef def = b2DefaultPrismaticJointDef();+	b2RecR_JointBase( rdr, &def.base );+	def.enableSpring = b2RecR_BOOL( rdr );+	def.hertz = b2RecR_F32( rdr );+	def.dampingRatio = b2RecR_F32( rdr );+	def.targetTranslation = b2RecR_F32( rdr );+	def.enableLimit = b2RecR_BOOL( rdr );+	def.lowerTranslation = b2RecR_F32( rdr );+	def.upperTranslation = b2RecR_F32( rdr );+	def.enableMotor = b2RecR_BOOL( rdr );+	def.maxMotorForce = b2RecR_F32( rdr );+	def.motorSpeed = b2RecR_F32( rdr );+	return def;+}++b2PogoJointDef b2RecR_POGOJOINTDEF( b2RecReader* rdr )+{+	b2PogoJointDef def = b2DefaultPogoJointDef();+	b2RecR_JointBase( rdr, &def.base );+	def.normal = b2RecR_VEC2( rdr );+	def.hertz = b2RecR_F32( rdr );+	def.dampingRatio = b2RecR_F32( rdr );+	def.restLength = b2RecR_F32( rdr );+	def.maxTensionForce = b2RecR_F32( rdr );+	def.maxCompressionForce = b2RecR_F32( rdr );+	def.impulse = b2RecR_F32( rdr );+	def.velocity = b2RecR_F32( rdr );+	return def;+}++b2RevoluteJointDef b2RecR_REVOLUTEJOINTDEF( b2RecReader* rdr )+{+	b2RevoluteJointDef def = b2DefaultRevoluteJointDef();+	b2RecR_JointBase( rdr, &def.base );+	def.targetAngle = b2RecR_F32( rdr );+	def.enableSpring = b2RecR_BOOL( rdr );+	def.hertz = b2RecR_F32( rdr );+	def.dampingRatio = b2RecR_F32( rdr );+	def.enableLimit = b2RecR_BOOL( rdr );+	def.lowerAngle = b2RecR_F32( rdr );+	def.upperAngle = b2RecR_F32( rdr );+	def.enableMotor = b2RecR_BOOL( rdr );+	def.maxMotorTorque = b2RecR_F32( rdr );+	def.motorSpeed = b2RecR_F32( rdr );+	return def;+}++b2WeldJointDef b2RecR_WELDJOINTDEF( b2RecReader* rdr )+{+	b2WeldJointDef def = b2DefaultWeldJointDef();+	b2RecR_JointBase( rdr, &def.base );+	def.linearHertz = b2RecR_F32( rdr );+	def.angularHertz = b2RecR_F32( rdr );+	def.linearDampingRatio = b2RecR_F32( rdr );+	def.angularDampingRatio = b2RecR_F32( rdr );+	return def;+}++b2WheelJointDef b2RecR_WHEELJOINTDEF( b2RecReader* rdr )+{+	b2WheelJointDef def = b2DefaultWheelJointDef();+	b2RecR_JointBase( rdr, &def.base );+	def.enableSpring = b2RecR_BOOL( rdr );+	def.hertz = b2RecR_F32( rdr );+	def.dampingRatio = b2RecR_F32( rdr );+	def.enableLimit = b2RecR_BOOL( rdr );+	def.lowerTranslation = b2RecR_F32( rdr );+	def.upperTranslation = b2RecR_F32( rdr );+	def.enableMotor = b2RecR_BOOL( rdr );+	def.maxMotorTorque = b2RecR_F32( rdr );+	def.motorSpeed = b2RecR_F32( rdr );+	return def;+}++b2AABB b2RecR_AABB( b2RecReader* rdr )+{+	b2AABB v;+	v.lowerBound = b2RecR_VEC2( rdr );+	v.upperBound = b2RecR_VEC2( rdr );+	return v;+}++b2QueryFilter b2RecR_QUERYFILTER( b2RecReader* rdr )+{+	b2QueryFilter f;+	f.categoryBits = b2RecR_U64( rdr );+	f.maskBits = b2RecR_U64( rdr );+	return f;+}++b2ShapeProxy b2RecR_SHAPEPROXY( b2RecReader* rdr )+{+	b2ShapeProxy p;+	memset( &p, 0, sizeof( p ) );+	int count = b2RecR_I32( rdr );+	if ( count < 0 )+		count = 0;+	if ( count > B2_MAX_POLYGON_VERTICES )+		count = B2_MAX_POLYGON_VERTICES;+	p.count = count;+	for ( int i = 0; i < count; ++i )+	{+		p.points[i] = b2RecR_VEC2( rdr );+	}+	p.radius = b2RecR_F32( rdr );+	return p;+}++b2RayResult b2RecR_RAYRESULT( b2RecReader* rdr )+{+	b2RayResult v;+	// shapeId keeps the recorded world0; b2RecMakeShapeId is applied at compare time+	v.shapeId = b2RecR_SHAPEID( rdr );+	v.point = b2RecR_POSITION( rdr );+	v.normal = b2RecR_VEC2( rdr );+	v.fraction = b2RecR_F32( rdr );+	v.nodeVisits = b2RecR_I32( rdr );+	v.leafVisits = b2RecR_I32( rdr );+	v.hit = b2RecR_BOOL( rdr );+	return v;+}++b2PlaneResult b2RecR_PLANERESULT( b2RecReader* rdr )+{+	b2PlaneResult v;+	v.plane.normal = b2RecR_VEC2( rdr );+	v.plane.offset = b2RecR_F32( rdr );+	v.point = b2RecR_VEC2( rdr );+	v.hit = b2RecR_BOOL( rdr );+	return v;+}++b2TreeStats b2RecR_TREESTATS( b2RecReader* rdr )+{+	b2TreeStats v;+	v.nodeVisits = b2RecR_I32( rdr );+	v.leafVisits = b2RecR_I32( rdr );+	return v;+}++// 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. Contents are not preserved; callers overwrite before use.+static bool b2RecReserveScratch( b2RecReader* 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;+	if ( *data != NULL )+	{+		b2Free( *data, (size_t)*cap * (size_t)elemSize );+	}+	*data = b2Alloc( (size_t)newCap * (size_t)elemSize );+	*cap = newCap;+	return true;+}++// Overflow-safe growth for the player's accumulating draw arrays. Counts come from the replay+// itself, not the file, so this only guards the byte-size multiply. Preserves keep elements.+static void b2RecGrow( 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 = b2Alloc( (size_t)newCap * (size_t)elemSize );+	if ( *data != NULL )+	{+		if ( keep > 0 )+		{+			memcpy( grown, *data, (size_t)keep * (size_t)elemSize );+		}+		b2Free( *data, (size_t)*capacity * (size_t)elemSize );+	}+	*data = grown;+	*capacity = newCap;+}++void b2RecEnsureHits( b2RecReader* rdr, int n )+{+	b2RecReserveScratch( rdr, (void**)&rdr->hits, &rdr->hitCap, n, (int)sizeof( b2RecRecordedHit ) );+}++// Per op dispatch, the only place real public API names appear+// Body and shape ids have world0 replaced with the replay world's slot index++static b2BodyId b2RecMakeBodyId( b2RecReader* rdr, b2BodyId recorded )+{+	b2BodyId id;+	id.index1 = recorded.index1;+	id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u );+	id.generation = recorded.generation;+	return id;+}++static b2ShapeId b2RecMakeShapeId( b2RecReader* rdr, b2ShapeId recorded )+{+	b2ShapeId id;+	id.index1 = recorded.index1;+	id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u );+	id.generation = recorded.generation;+	return id;+}++static b2ChainId b2RecMakeChainId( b2RecReader* rdr, b2ChainId recorded )+{+	b2ChainId id;+	id.index1 = recorded.index1;+	id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u );+	id.generation = recorded.generation;+	return id;+}++static b2JointId b2RecMakeJointId( b2RecReader* rdr, b2JointId recorded )+{+	b2JointId id;+	id.index1 = recorded.index1;+	id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u );+	id.generation = recorded.generation;+	return id;+}++// A create op appends its returned id after the args. Replay compares index1 and generation+// only, since world0 differs between record and replay. A mismatch means structural drift.++static void b2RecCheckId( b2RecReader* rdr, const char* kind, int gotIndex, unsigned gotGen, int recIndex, unsigned recGen )+{+	if ( gotIndex != recIndex || gotGen != recGen )+	{+		printf( "b2ReplayFile: %s id mismatch (rec index1=%d gen=%u, got index1=%d gen=%u)\n", kind, recIndex, recGen, gotIndex,+				gotGen );+		rdr->ok = false;+	}+}++static void b2RecCheckBodyId( b2RecReader* rdr, b2BodyId got, b2BodyId rec )+{+	b2RecCheckId( rdr, "body", got.index1, got.generation, rec.index1, rec.generation );+}++static void b2RecCheckShapeId( b2RecReader* rdr, b2ShapeId got, b2ShapeId rec )+{+	b2RecCheckId( rdr, "shape", got.index1, got.generation, rec.index1, rec.generation );+}++static void b2RecCheckChainId( b2RecReader* rdr, b2ChainId got, b2ChainId rec )+{+	b2RecCheckId( rdr, "chain", got.index1, got.generation, rec.index1, rec.generation );+}++static void b2RecCheckJointId( b2RecReader* rdr, b2JointId got, b2JointId rec )+{+	b2RecCheckId( rdr, "joint", got.index1, got.generation, rec.index1, rec.generation );+}++static void b2RecDispatch_DestroyWorld( const b2RecArgs_DestroyWorld* a, b2RecReader* rdr )+{+	(void)a;+	(void)rdr;+	// The recorded session ended here. The player owns the replay world's lifetime and tears it+	// down in b2DestroyReplay/Restart, so a viewer can keep drawing the final step. There is+	// one world per recording and this is always the last record, so leaving it alive is safe.+}++static void b2RecDispatch_Step( const b2RecArgs_Step* a, b2RecReader* rdr )+{+	(void)a;+	b2World_Step( rdr->replayWorldId, a->dt, a->subStepCount );+}++static void b2RecDispatch_WorldEnableSleeping( const b2RecArgs_WorldEnableSleeping* a, b2RecReader* rdr )+{+	b2World_EnableSleeping( rdr->replayWorldId, a->flag );+}++static void b2RecDispatch_WorldEnableContinuous( const b2RecArgs_WorldEnableContinuous* a, b2RecReader* rdr )+{+	b2World_EnableContinuous( rdr->replayWorldId, a->flag );+}++static void b2RecDispatch_WorldSetRestitutionThreshold( const b2RecArgs_WorldSetRestitutionThreshold* a, b2RecReader* rdr )+{+	b2World_SetRestitutionThreshold( rdr->replayWorldId, a->value );+}++static void b2RecDispatch_WorldSetHitEventThreshold( const b2RecArgs_WorldSetHitEventThreshold* a, b2RecReader* rdr )+{+	b2World_SetHitEventThreshold( rdr->replayWorldId, a->value );+}++static void b2RecDispatch_WorldSetGravity( const b2RecArgs_WorldSetGravity* a, b2RecReader* rdr )+{+	b2World_SetGravity( rdr->replayWorldId, a->gravity );+}++static void b2RecDispatch_WorldExplode( const b2RecArgs_WorldExplode* a, b2RecReader* rdr )+{+	b2World_Explode( rdr->replayWorldId, &a->def );+}++static void b2RecDispatch_WorldSetContactTuning( const b2RecArgs_WorldSetContactTuning* a, b2RecReader* rdr )+{+	b2World_SetContactTuning( rdr->replayWorldId, a->hertz, a->dampingRatio, a->pushSpeed );+}++static void b2RecDispatch_WorldSetContactRecycleDistance( const b2RecArgs_WorldSetContactRecycleDistance* a, b2RecReader* rdr )+{+	b2World_SetContactRecycleDistance( rdr->replayWorldId, a->recycleDistance );+}++static void b2RecDispatch_WorldSetMaxLinearSpeed( const b2RecArgs_WorldSetMaxLinearSpeed* a, b2RecReader* rdr )+{+	b2World_SetMaxLinearSpeed( rdr->replayWorldId, a->maximumLinearSpeed );+}++static void b2RecDispatch_WorldEnableWarmStarting( const b2RecArgs_WorldEnableWarmStarting* a, b2RecReader* rdr )+{+	b2World_EnableWarmStarting( rdr->replayWorldId, a->flag );+}++static void b2RecDispatch_WorldRebuildStaticTree( const b2RecArgs_WorldRebuildStaticTree* a, b2RecReader* rdr )+{+	(void)a;+	b2World_RebuildStaticTree( rdr->replayWorldId );+}++static void b2RecDispatch_WorldSetRestitutionIterations( const b2RecArgs_WorldSetRestitutionIterations* a, b2RecReader* rdr )+{+	b2World_SetRestitutionIterations( rdr->replayWorldId, a->iterations );+}++static void b2RecDispatch_WorldEnableRestitutionPropagation( const b2RecArgs_WorldEnableRestitutionPropagation* a,+															 b2RecReader* rdr )+{+	b2World_EnableRestitutionPropagation( rdr->replayWorldId, a->flag );+}++// Append a created body to the outliner tracking list. Ordinals are creation order and never reused.+static void b2RecTrackBodyCreate( b2Replay* player, b2BodyId id )+{+	b2RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, player->bodyIdCount + 1, player->bodyIdCount,+			   (int)sizeof( b2BodyId ) );+	player->bodyIds[player->bodyIdCount] = id;+	player->bodyIdCount += 1;+}++// Leave a hole so later ordinals do not shift, keeping a stored selection stable across the playthrough+static void b2RecTrackBodyDestroy( b2Replay* player, b2BodyId id )+{+	for ( int i = 0; i < player->bodyIdCount; ++i )+	{+		if ( B2_ID_EQUALS( player->bodyIds[i], id ) )+		{+			player->bodyIds[i] = b2_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 b2RecSeedBodyIds( b2Replay* player )+{+	b2World* world = b2GetWorldFromId( 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+		}+		b2RecTrackBodyCreate( player, b2MakeBodyId( world, i ) );+	}+}++static void b2RecDispatch_CreateBody( const b2RecArgs_CreateBody* a, b2RecReader* rdr )+{+	// Recorded id is appended after args (written before b2RecEndRecord)+	b2BodyId recId = b2RecR_BODYID( rdr );+	b2BodyId gotId = b2CreateBody( rdr->replayWorldId, &a->def );+	b2RecCheckBodyId( rdr, gotId, recId );+	if ( rdr->owner != NULL )+	{+		b2RecTrackBodyCreate( rdr->owner, gotId );+	}+}++static void b2RecDispatch_DestroyBody( const b2RecArgs_DestroyBody* a, b2RecReader* rdr )+{+	b2BodyId id = b2RecMakeBodyId( rdr, a->body );+	if ( rdr->owner != NULL )+	{+		b2RecTrackBodyDestroy( rdr->owner, id );+	}+	b2DestroyBody( id );+}++static void b2RecDispatch_BodySetTransform( const b2RecArgs_BodySetTransform* a, b2RecReader* rdr )+{+	b2BodyId id = b2RecMakeBodyId( rdr, a->body );+	b2Body_SetTransform( id, a->position, a->rotation );+}++static void b2RecDispatch_BodySetLinearVelocity( const b2RecArgs_BodySetLinearVelocity* a, b2RecReader* rdr )+{+	b2BodyId id = b2RecMakeBodyId( rdr, a->body );+	b2Body_SetLinearVelocity( id, a->v );+}++static void b2RecDispatch_BodySetType( const b2RecArgs_BodySetType* a, b2RecReader* rdr )+{+	b2Body_SetType( b2RecMakeBodyId( rdr, a->body ), (b2BodyType)a->type );+}++static void b2RecDispatch_BodySetName( const b2RecArgs_BodySetName* a, b2RecReader* rdr )+{+	b2Body_SetName( b2RecMakeBodyId( rdr, a->body ), a->name );+}++static void b2RecDispatch_BodySetAngularVelocity( const b2RecArgs_BodySetAngularVelocity* a, b2RecReader* rdr )+{+	b2Body_SetAngularVelocity( b2RecMakeBodyId( rdr, a->body ), a->w );+}++static void b2RecDispatch_BodySetTargetTransform( const b2RecArgs_BodySetTargetTransform* a, b2RecReader* rdr )+{+	b2Body_SetTargetTransform( b2RecMakeBodyId( rdr, a->body ), a->target, a->timeStep, a->wake );+}++static void b2RecDispatch_BodyApplyForce( const b2RecArgs_BodyApplyForce* a, b2RecReader* rdr )+{+	b2Body_ApplyForce( b2RecMakeBodyId( rdr, a->body ), a->force, a->point, a->wake );+}++static void b2RecDispatch_BodyApplyForceToCenter( const b2RecArgs_BodyApplyForceToCenter* a, b2RecReader* rdr )+{+	b2Body_ApplyForceToCenter( b2RecMakeBodyId( rdr, a->body ), a->force, a->wake );+}++static void b2RecDispatch_BodyApplyTorque( const b2RecArgs_BodyApplyTorque* a, b2RecReader* rdr )+{+	b2Body_ApplyTorque( b2RecMakeBodyId( rdr, a->body ), a->torque, a->wake );+}++static void b2RecDispatch_BodyClearForces( const b2RecArgs_BodyClearForces* a, b2RecReader* rdr )+{+	b2Body_ClearForces( b2RecMakeBodyId( rdr, a->body ) );+}++static void b2RecDispatch_BodyApplyLinearImpulse( const b2RecArgs_BodyApplyLinearImpulse* a, b2RecReader* rdr )+{+	b2Body_ApplyLinearImpulse( b2RecMakeBodyId( rdr, a->body ), a->impulse, a->point, a->wake );+}++static void b2RecDispatch_BodyApplyLinearImpulseToCenter( const b2RecArgs_BodyApplyLinearImpulseToCenter* a, b2RecReader* rdr )+{+	b2Body_ApplyLinearImpulseToCenter( b2RecMakeBodyId( rdr, a->body ), a->impulse, a->wake );+}++static void b2RecDispatch_BodyApplyAngularImpulse( const b2RecArgs_BodyApplyAngularImpulse* a, b2RecReader* rdr )+{+	b2Body_ApplyAngularImpulse( b2RecMakeBodyId( rdr, a->body ), a->impulse, a->wake );+}++static void b2RecDispatch_BodySetMassData( const b2RecArgs_BodySetMassData* a, b2RecReader* rdr )+{+	b2Body_SetMassData( b2RecMakeBodyId( rdr, a->body ), a->massData );+}++static void b2RecDispatch_BodyUpdateMassFromShapes( const b2RecArgs_BodyUpdateMassFromShapes* a, b2RecReader* rdr )+{+	b2Body_UpdateMassFromShapes( b2RecMakeBodyId( rdr, a->body ) );+}++static void b2RecDispatch_BodySetLinearDamping( const b2RecArgs_BodySetLinearDamping* a, b2RecReader* rdr )+{+	b2Body_SetLinearDamping( b2RecMakeBodyId( rdr, a->body ), a->damping );+}++static void b2RecDispatch_BodySetAngularDamping( const b2RecArgs_BodySetAngularDamping* a, b2RecReader* rdr )+{+	b2Body_SetAngularDamping( b2RecMakeBodyId( rdr, a->body ), a->damping );+}++static void b2RecDispatch_BodySetGravityScale( const b2RecArgs_BodySetGravityScale* a, b2RecReader* rdr )+{+	b2Body_SetGravityScale( b2RecMakeBodyId( rdr, a->body ), a->scale );+}++static void b2RecDispatch_BodySetAwake( const b2RecArgs_BodySetAwake* a, b2RecReader* rdr )+{+	b2Body_SetAwake( b2RecMakeBodyId( rdr, a->body ), a->awake );+}++static void b2RecDispatch_BodyWakeTouching( const b2RecArgs_BodyWakeTouching* a, b2RecReader* rdr )+{+	b2Body_WakeTouching( b2RecMakeBodyId( rdr, a->body ) );+}++static void b2RecDispatch_BodyEnableSleeping( const b2RecArgs_BodyEnableSleeping* a, b2RecReader* rdr )+{+	b2Body_EnableSleeping( b2RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b2RecDispatch_BodySetSleepThreshold( const b2RecArgs_BodySetSleepThreshold* a, b2RecReader* rdr )+{+	b2Body_SetSleepThreshold( b2RecMakeBodyId( rdr, a->body ), a->threshold );+}++static void b2RecDispatch_BodySetSafetyFactor( const b2RecArgs_BodySetSafetyFactor* a, b2RecReader* rdr )+{+	b2Body_SetSafetyFactor( b2RecMakeBodyId( rdr, a->body ), a->value );+}++static void b2RecDispatch_BodyDisable( const b2RecArgs_BodyDisable* a, b2RecReader* rdr )+{+	b2Body_Disable( b2RecMakeBodyId( rdr, a->body ) );+}++static void b2RecDispatch_BodyEnable( const b2RecArgs_BodyEnable* a, b2RecReader* rdr )+{+	b2Body_Enable( b2RecMakeBodyId( rdr, a->body ) );+}++static void b2RecDispatch_BodySetMotionLocks( const b2RecArgs_BodySetMotionLocks* a, b2RecReader* rdr )+{+	b2Body_SetMotionLocks( b2RecMakeBodyId( rdr, a->body ), a->locks );+}++static void b2RecDispatch_BodySetBullet( const b2RecArgs_BodySetBullet* a, b2RecReader* rdr )+{+	b2Body_SetBullet( b2RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b2RecDispatch_BodyEnableContactRecycling( const b2RecArgs_BodyEnableContactRecycling* a, b2RecReader* rdr )+{+	b2Body_EnableContactRecycling( b2RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b2RecDispatch_BodyEnableContactEvents( const b2RecArgs_BodyEnableContactEvents* a, b2RecReader* rdr )+{+	b2Body_EnableContactEvents( b2RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b2RecDispatch_BodyEnableHitEvents( const b2RecArgs_BodyEnableHitEvents* a, b2RecReader* rdr )+{+	b2Body_EnableHitEvents( b2RecMakeBodyId( rdr, a->body ), a->flag );+}++static void b2RecDispatch_CreateCircleShape( const b2RecArgs_CreateCircleShape* a, b2RecReader* rdr )+{+	b2ShapeId recId = b2RecR_SHAPEID( rdr );+	b2BodyId bodyId = b2RecMakeBodyId( rdr, a->body );+	b2ShapeId gotId = b2CreateCircleShape( bodyId, &a->def, &a->circle );+	b2RecCheckShapeId( rdr, gotId, recId );+}++static void b2RecDispatch_CreateCapsuleShape( const b2RecArgs_CreateCapsuleShape* a, b2RecReader* rdr )+{+	b2ShapeId recId = b2RecR_SHAPEID( rdr );+	b2BodyId bodyId = b2RecMakeBodyId( rdr, a->body );+	b2ShapeId gotId = b2CreateCapsuleShape( bodyId, &a->def, &a->capsule );+	b2RecCheckShapeId( rdr, gotId, recId );+}++static void b2RecDispatch_CreateSegmentShape( const b2RecArgs_CreateSegmentShape* a, b2RecReader* rdr )+{+	b2ShapeId recId = b2RecR_SHAPEID( rdr );+	b2BodyId bodyId = b2RecMakeBodyId( rdr, a->body );+	b2ShapeId gotId = b2CreateSegmentShape( bodyId, &a->def, &a->segment );+	b2RecCheckShapeId( rdr, gotId, recId );+}++static void b2RecDispatch_CreatePolygonShape( const b2RecArgs_CreatePolygonShape* a, b2RecReader* rdr )+{+	b2ShapeId recId = b2RecR_SHAPEID( rdr );+	b2BodyId bodyId = b2RecMakeBodyId( rdr, a->body );+	b2ShapeId gotId = b2CreatePolygonShape( bodyId, &a->def, &a->polygon );+	b2RecCheckShapeId( rdr, gotId, recId );+}++static void b2RecDispatch_CreateChainSegmentShape( const b2RecArgs_CreateChainSegmentShape* a, b2RecReader* rdr )+{+	b2ShapeId recId = b2RecR_SHAPEID( rdr );+	b2BodyId bodyId = b2RecMakeBodyId( rdr, a->body );+	b2ShapeId gotId = b2CreateChainSegmentShape( bodyId, &a->def, &a->chainSegment );+	b2RecCheckShapeId( rdr, gotId, recId );+}++static void b2RecDispatch_DestroyShape( const b2RecArgs_DestroyShape* a, b2RecReader* rdr )+{+	b2DestroyShape( b2RecMakeShapeId( rdr, a->shape ), a->updateBodyMass );+}++static void b2RecDispatch_ShapeSetDensity( const b2RecArgs_ShapeSetDensity* a, b2RecReader* rdr )+{+	b2Shape_SetDensity( b2RecMakeShapeId( rdr, a->shape ), a->density, a->updateBodyMass );+}++static void b2RecDispatch_ShapeSetFriction( const b2RecArgs_ShapeSetFriction* a, b2RecReader* rdr )+{+	b2Shape_SetFriction( b2RecMakeShapeId( rdr, a->shape ), a->friction );+}++static void b2RecDispatch_ShapeSetRestitution( const b2RecArgs_ShapeSetRestitution* a, b2RecReader* rdr )+{+	b2Shape_SetRestitution( b2RecMakeShapeId( rdr, a->shape ), a->restitution );+}++static void b2RecDispatch_ShapeSetUserMaterial( const b2RecArgs_ShapeSetUserMaterial* a, b2RecReader* rdr )+{+	b2Shape_SetUserMaterial( b2RecMakeShapeId( rdr, a->shape ), a->material );+}++static void b2RecDispatch_ShapeSetSurfaceMaterial( const b2RecArgs_ShapeSetSurfaceMaterial* a, b2RecReader* rdr )+{+	b2Shape_SetSurfaceMaterial( b2RecMakeShapeId( rdr, a->shape ), &a->material );+}++static void b2RecDispatch_ShapeSetFilter( const b2RecArgs_ShapeSetFilter* a, b2RecReader* rdr )+{+	b2Shape_SetFilter( b2RecMakeShapeId( rdr, a->shape ), a->filter );+}++static void b2RecDispatch_ShapeEnableSensorEvents( const b2RecArgs_ShapeEnableSensorEvents* a, b2RecReader* rdr )+{+	b2Shape_EnableSensorEvents( b2RecMakeShapeId( rdr, a->shape ), a->flag );+}++static void b2RecDispatch_ShapeEnableContactEvents( const b2RecArgs_ShapeEnableContactEvents* a, b2RecReader* rdr )+{+	b2Shape_EnableContactEvents( b2RecMakeShapeId( rdr, a->shape ), a->flag );+}++static void b2RecDispatch_ShapeEnablePreSolveEvents( const b2RecArgs_ShapeEnablePreSolveEvents* a, b2RecReader* rdr )+{+	b2Shape_EnablePreSolveEvents( b2RecMakeShapeId( rdr, a->shape ), a->flag );+}++static void b2RecDispatch_ShapeEnableHitEvents( const b2RecArgs_ShapeEnableHitEvents* a, b2RecReader* rdr )+{+	b2Shape_EnableHitEvents( b2RecMakeShapeId( rdr, a->shape ), a->flag );+}++static void b2RecDispatch_ShapeSetCircle( const b2RecArgs_ShapeSetCircle* a, b2RecReader* rdr )+{+	b2Shape_SetCircle( b2RecMakeShapeId( rdr, a->shape ), &a->circle );+}++static void b2RecDispatch_ShapeSetCapsule( const b2RecArgs_ShapeSetCapsule* a, b2RecReader* rdr )+{+	b2Shape_SetCapsule( b2RecMakeShapeId( rdr, a->shape ), &a->capsule );+}++static void b2RecDispatch_ShapeSetSegment( const b2RecArgs_ShapeSetSegment* a, b2RecReader* rdr )+{+	b2Shape_SetSegment( b2RecMakeShapeId( rdr, a->shape ), &a->segment );+}++static void b2RecDispatch_ShapeSetPolygon( const b2RecArgs_ShapeSetPolygon* a, b2RecReader* rdr )+{+	b2Shape_SetPolygon( b2RecMakeShapeId( rdr, a->shape ), &a->polygon );+}++static void b2RecDispatch_ShapeSetChainSegment( const b2RecArgs_ShapeSetChainSegment* a, b2RecReader* rdr )+{+	b2Shape_SetChainSegment( b2RecMakeShapeId( rdr, a->shape ), &a->chainSegment );+}++static void b2RecDispatch_ShapeApplyWind( const b2RecArgs_ShapeApplyWind* a, b2RecReader* rdr )+{+	b2Shape_ApplyWind( b2RecMakeShapeId( rdr, a->shape ), a->wind, a->drag, a->lift, a->wake );+}++static void b2RecDispatch_CreateChain( const b2RecArgs_CreateChain* a, b2RecReader* rdr )+{+	b2ChainId recId = b2RecR_CHAINID( rdr );+	if ( !rdr->ok )+	{+		// A corrupt point/material count left the def degenerate, do not build a chain from it+		return;+	}+	b2BodyId bodyId = b2RecMakeBodyId( rdr, a->body );+	b2ChainId gotId = b2CreateChain( bodyId, &a->def );+	b2RecCheckChainId( rdr, gotId, recId );+}++static void b2RecDispatch_DestroyChain( const b2RecArgs_DestroyChain* a, b2RecReader* rdr )+{+	b2DestroyChain( b2RecMakeChainId( rdr, a->chain ) );+}++static void b2RecDispatch_ChainSetSurfaceMaterial( const b2RecArgs_ChainSetSurfaceMaterial* a, b2RecReader* rdr )+{+	b2Chain_SetSurfaceMaterial( b2RecMakeChainId( rdr, a->chain ), &a->material, a->segmentIndex );+}++static void b2RecDispatch_ChainSetAllSurfaceMaterials( const b2RecArgs_ChainSetAllSurfaceMaterials* a, b2RecReader* rdr )+{+	b2Chain_SetAllSurfaceMaterials( b2RecMakeChainId( rdr, a->chain ), &a->material );+}++// Joint create: body ids in the def are remapped to the replay world before the call.++static void b2RecDispatch_CreateDistanceJoint( const b2RecArgs_CreateDistanceJoint* a, b2RecReader* rdr )+{+	b2JointId recId = b2RecR_JOINTID( rdr );+	b2DistanceJointDef def = a->def;+	def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB );+	b2RecCheckJointId( rdr, b2CreateDistanceJoint( rdr->replayWorldId, &def ), recId );+}++static void b2RecDispatch_CreateFilterJoint( const b2RecArgs_CreateFilterJoint* a, b2RecReader* rdr )+{+	b2JointId recId = b2RecR_JOINTID( rdr );+	b2FilterJointDef def = a->def;+	def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB );+	b2RecCheckJointId( rdr, b2CreateFilterJoint( rdr->replayWorldId, &def ), recId );+}++static void b2RecDispatch_CreateMotorJoint( const b2RecArgs_CreateMotorJoint* a, b2RecReader* rdr )+{+	b2JointId recId = b2RecR_JOINTID( rdr );+	b2MotorJointDef def = a->def;+	def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB );+	b2RecCheckJointId( rdr, b2CreateMotorJoint( rdr->replayWorldId, &def ), recId );+}++static void b2RecDispatch_CreateMoverJoint( const b2RecArgs_CreateMoverJoint* a, b2RecReader* rdr )+{+	b2JointId recId = b2RecR_JOINTID( rdr );+	b2MoverJointDef def = a->def;+	def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB );+	b2RecCheckJointId( rdr, b2CreateMoverJoint( rdr->replayWorldId, &def ), recId );+}++static void b2RecDispatch_CreatePrismaticJoint( const b2RecArgs_CreatePrismaticJoint* a, b2RecReader* rdr )+{+	b2JointId recId = b2RecR_JOINTID( rdr );+	b2PrismaticJointDef def = a->def;+	def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB );+	b2RecCheckJointId( rdr, b2CreatePrismaticJoint( rdr->replayWorldId, &def ), recId );+}++static void b2RecDispatch_CreatePogoJoint( const b2RecArgs_CreatePogoJoint* a, b2RecReader* rdr )+{+	b2JointId recId = b2RecR_JOINTID( rdr );+	b2PogoJointDef def = a->def;+	def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB );+	b2RecCheckJointId( rdr, b2CreatePogoJoint( rdr->replayWorldId, &def ), recId );+}++static void b2RecDispatch_CreateRevoluteJoint( const b2RecArgs_CreateRevoluteJoint* a, b2RecReader* rdr )+{+	b2JointId recId = b2RecR_JOINTID( rdr );+	b2RevoluteJointDef def = a->def;+	def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB );+	b2RecCheckJointId( rdr, b2CreateRevoluteJoint( rdr->replayWorldId, &def ), recId );+}++static void b2RecDispatch_CreateWeldJoint( const b2RecArgs_CreateWeldJoint* a, b2RecReader* rdr )+{+	b2JointId recId = b2RecR_JOINTID( rdr );+	b2WeldJointDef def = a->def;+	def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB );+	b2RecCheckJointId( rdr, b2CreateWeldJoint( rdr->replayWorldId, &def ), recId );+}++static void b2RecDispatch_CreateWheelJoint( const b2RecArgs_CreateWheelJoint* a, b2RecReader* rdr )+{+	b2JointId recId = b2RecR_JOINTID( rdr );+	b2WheelJointDef def = a->def;+	def.base.bodyIdA = b2RecMakeBodyId( rdr, def.base.bodyIdA );+	def.base.bodyIdB = b2RecMakeBodyId( rdr, def.base.bodyIdB );+	b2RecCheckJointId( rdr, b2CreateWheelJoint( rdr->replayWorldId, &def ), recId );+}++static void b2RecDispatch_DestroyJoint( const b2RecArgs_DestroyJoint* a, b2RecReader* rdr )+{+	b2DestroyJoint( b2RecMakeJointId( rdr, a->joint ) );+}++// Generic joint mutators++static void b2RecDispatch_JointSetLocalFrameA( const b2RecArgs_JointSetLocalFrameA* a, b2RecReader* rdr )+{+	b2Joint_SetLocalFrameA( b2RecMakeJointId( rdr, a->joint ), a->localFrame );+}++static void b2RecDispatch_JointSetLocalFrameB( const b2RecArgs_JointSetLocalFrameB* a, b2RecReader* rdr )+{+	b2Joint_SetLocalFrameB( b2RecMakeJointId( rdr, a->joint ), a->localFrame );+}++static void b2RecDispatch_JointSetCollideConnected( const b2RecArgs_JointSetCollideConnected* a, b2RecReader* rdr )+{+	b2Joint_SetCollideConnected( b2RecMakeJointId( rdr, a->joint ), a->shouldCollide );+}++static void b2RecDispatch_JointWakeBodies( const b2RecArgs_JointWakeBodies* a, b2RecReader* rdr )+{+	b2Joint_WakeBodies( b2RecMakeJointId( rdr, a->joint ) );+}++static void b2RecDispatch_JointSetConstraintTuning( const b2RecArgs_JointSetConstraintTuning* a, b2RecReader* rdr )+{+	b2Joint_SetConstraintTuning( b2RecMakeJointId( rdr, a->joint ), a->hertz, a->dampingRatio );+}++static void b2RecDispatch_JointSetForceThreshold( const b2RecArgs_JointSetForceThreshold* a, b2RecReader* rdr )+{+	b2Joint_SetForceThreshold( b2RecMakeJointId( rdr, a->joint ), a->threshold );+}++static void b2RecDispatch_JointSetTorqueThreshold( const b2RecArgs_JointSetTorqueThreshold* a, b2RecReader* rdr )+{+	b2Joint_SetTorqueThreshold( b2RecMakeJointId( rdr, a->joint ), a->threshold );+}++// Distance joint++static void b2RecDispatch_DistanceJointSetRestLength( const b2RecArgs_DistanceJointSetRestLength* a, b2RecReader* rdr )+{+	b2DistanceJoint_SetRestLength( b2RecMakeJointId( rdr, a->joint ), a->length );+}++static void b2RecDispatch_DistanceJointEnableSpring( const b2RecArgs_DistanceJointEnableSpring* a, b2RecReader* rdr )+{+	b2DistanceJoint_EnableSpring( b2RecMakeJointId( rdr, a->joint ), a->enableSpring );+}++static void b2RecDispatch_DistanceJointSetSpringForceRange( const b2RecArgs_DistanceJointSetSpringForceRange* a,+															b2RecReader* rdr )+{+	b2DistanceJoint_SetSpringForceRange( b2RecMakeJointId( rdr, a->joint ), a->lowerForce, a->upperForce );+}++static void b2RecDispatch_DistanceJointSetSpringHertz( const b2RecArgs_DistanceJointSetSpringHertz* a, b2RecReader* rdr )+{+	b2DistanceJoint_SetSpringHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b2RecDispatch_DistanceJointSetSpringDampingRatio( const b2RecArgs_DistanceJointSetSpringDampingRatio* a,+															  b2RecReader* rdr )+{+	b2DistanceJoint_SetSpringDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b2RecDispatch_DistanceJointEnableLimit( const b2RecArgs_DistanceJointEnableLimit* a, b2RecReader* rdr )+{+	b2DistanceJoint_EnableLimit( b2RecMakeJointId( rdr, a->joint ), a->enableLimit );+}++static void b2RecDispatch_DistanceJointSetLimits( const b2RecArgs_DistanceJointSetLimits* a, b2RecReader* rdr )+{+	b2DistanceJoint_SetLimits( b2RecMakeJointId( rdr, a->joint ), a->minLength, a->maxLength );+}++static void b2RecDispatch_DistanceJointEnableMotor( const b2RecArgs_DistanceJointEnableMotor* a, b2RecReader* rdr )+{+	b2DistanceJoint_EnableMotor( b2RecMakeJointId( rdr, a->joint ), a->enableMotor );+}++static void b2RecDispatch_DistanceJointSetTargetMotorSpeed( const b2RecArgs_DistanceJointSetTargetMotorSpeed* a, b2RecReader* rdr )+{+	b2DistanceJoint_SetTargetMotorSpeed( b2RecMakeJointId( rdr, a->joint ), a->motorSpeed );+}++static void b2RecDispatch_DistanceJointSetMaxMotorForce( const b2RecArgs_DistanceJointSetMaxMotorForce* a, b2RecReader* rdr )+{+	b2DistanceJoint_SetMaxMotorForce( b2RecMakeJointId( rdr, a->joint ), a->force );+}++// Motor joint++static void b2RecDispatch_MotorJointSetLinearVelocity( const b2RecArgs_MotorJointSetLinearVelocity* a, b2RecReader* rdr )+{+	b2MotorJoint_SetLinearVelocity( b2RecMakeJointId( rdr, a->joint ), a->velocity );+}++static void b2RecDispatch_MotorJointSetAngularVelocity( const b2RecArgs_MotorJointSetAngularVelocity* a, b2RecReader* rdr )+{+	b2MotorJoint_SetAngularVelocity( b2RecMakeJointId( rdr, a->joint ), a->velocity );+}++static void b2RecDispatch_MotorJointSetMaxVelocityForce( const b2RecArgs_MotorJointSetMaxVelocityForce* a, b2RecReader* rdr )+{+	b2MotorJoint_SetMaxVelocityForce( b2RecMakeJointId( rdr, a->joint ), a->maxForce );+}++static void b2RecDispatch_MotorJointSetMaxVelocityTorque( const b2RecArgs_MotorJointSetMaxVelocityTorque* a, b2RecReader* rdr )+{+	b2MotorJoint_SetMaxVelocityTorque( b2RecMakeJointId( rdr, a->joint ), a->maxTorque );+}++static void b2RecDispatch_MotorJointSetLinearHertz( const b2RecArgs_MotorJointSetLinearHertz* a, b2RecReader* rdr )+{+	b2MotorJoint_SetLinearHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b2RecDispatch_MotorJointSetLinearDampingRatio( const b2RecArgs_MotorJointSetLinearDampingRatio* a, b2RecReader* rdr )+{+	b2MotorJoint_SetLinearDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->damping );+}++static void b2RecDispatch_MotorJointSetAngularHertz( const b2RecArgs_MotorJointSetAngularHertz* a, b2RecReader* rdr )+{+	b2MotorJoint_SetAngularHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b2RecDispatch_MotorJointSetAngularDampingRatio( const b2RecArgs_MotorJointSetAngularDampingRatio* a,+															b2RecReader* rdr )+{+	b2MotorJoint_SetAngularDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->damping );+}++static void b2RecDispatch_MotorJointSetMaxSpringForce( const b2RecArgs_MotorJointSetMaxSpringForce* a, b2RecReader* rdr )+{+	b2MotorJoint_SetMaxSpringForce( b2RecMakeJointId( rdr, a->joint ), a->maxForce );+}++static void b2RecDispatch_MotorJointSetMaxSpringTorque( const b2RecArgs_MotorJointSetMaxSpringTorque* a, b2RecReader* rdr )+{+	b2MotorJoint_SetMaxSpringTorque( b2RecMakeJointId( rdr, a->joint ), a->maxTorque );+}++// Mover joint+static void b2RecDispatch_MoverJointSetLinearVelocity( const b2RecArgs_MoverJointSetLinearVelocity* a, b2RecReader* rdr )+{+	b2MoverJoint_SetLinearVelocity( b2RecMakeJointId( rdr, a->joint ), a->velocity );+}++static void b2RecDispatch_MoverJointSetMaxVelocityForce( const b2RecArgs_MoverJointSetMaxVelocityForce* a, b2RecReader* rdr )+{+	b2MoverJoint_SetMaxVelocityForce( b2RecMakeJointId( rdr, a->joint ), a->maxForce );+}++// Pogo joint++static void b2RecDispatch_PogoJointSetRestLength( const b2RecArgs_PogoJointSetRestLength* a, b2RecReader* rdr )+{+	b2PogoJoint_SetRestLength( b2RecMakeJointId( rdr, a->joint ), a->length );+}++static void b2RecDispatch_PogoJointSetSpringHertz( const b2RecArgs_PogoJointSetSpringHertz* a, b2RecReader* rdr )+{+	b2PogoJoint_SetSpringHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b2RecDispatch_PogoJointSetSpringDampingRatio( const b2RecArgs_PogoJointSetSpringDampingRatio* a, b2RecReader* rdr )+{+	b2PogoJoint_SetSpringDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++// Prismatic joint++static void b2RecDispatch_PrismaticJointEnableSpring( const b2RecArgs_PrismaticJointEnableSpring* a, b2RecReader* rdr )+{+	b2PrismaticJoint_EnableSpring( b2RecMakeJointId( rdr, a->joint ), a->enableSpring );+}++static void b2RecDispatch_PrismaticJointSetSpringHertz( const b2RecArgs_PrismaticJointSetSpringHertz* a, b2RecReader* rdr )+{+	b2PrismaticJoint_SetSpringHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b2RecDispatch_PrismaticJointSetSpringDampingRatio( const b2RecArgs_PrismaticJointSetSpringDampingRatio* a,+															   b2RecReader* rdr )+{+	b2PrismaticJoint_SetSpringDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b2RecDispatch_PrismaticJointSetTargetTranslation( const b2RecArgs_PrismaticJointSetTargetTranslation* a,+															  b2RecReader* rdr )+{+	b2PrismaticJoint_SetTargetTranslation( b2RecMakeJointId( rdr, a->joint ), a->translation );+}++static void b2RecDispatch_PrismaticJointEnableLimit( const b2RecArgs_PrismaticJointEnableLimit* a, b2RecReader* rdr )+{+	b2PrismaticJoint_EnableLimit( b2RecMakeJointId( rdr, a->joint ), a->enableLimit );+}++static void b2RecDispatch_PrismaticJointSetLimits( const b2RecArgs_PrismaticJointSetLimits* a, b2RecReader* rdr )+{+	b2PrismaticJoint_SetLimits( b2RecMakeJointId( rdr, a->joint ), a->lower, a->upper );+}++static void b2RecDispatch_PrismaticJointEnableMotor( const b2RecArgs_PrismaticJointEnableMotor* a, b2RecReader* rdr )+{+	b2PrismaticJoint_EnableMotor( b2RecMakeJointId( rdr, a->joint ), a->enableMotor );+}++static void b2RecDispatch_PrismaticJointSetTargetMotorSpeed( const b2RecArgs_PrismaticJointSetTargetMotorSpeed* a, b2RecReader* rdr )+{+	b2PrismaticJoint_SetTargetMotorSpeed( b2RecMakeJointId( rdr, a->joint ), a->motorSpeed );+}++static void b2RecDispatch_PrismaticJointSetMaxMotorForce( const b2RecArgs_PrismaticJointSetMaxMotorForce* a, b2RecReader* rdr )+{+	b2PrismaticJoint_SetMaxMotorForce( b2RecMakeJointId( rdr, a->joint ), a->force );+}++// Revolute joint++static void b2RecDispatch_RevoluteJointEnableSpring( const b2RecArgs_RevoluteJointEnableSpring* a, b2RecReader* rdr )+{+	b2RevoluteJoint_EnableSpring( b2RecMakeJointId( rdr, a->joint ), a->enableSpring );+}++static void b2RecDispatch_RevoluteJointSetSpringHertz( const b2RecArgs_RevoluteJointSetSpringHertz* a, b2RecReader* rdr )+{+	b2RevoluteJoint_SetSpringHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b2RecDispatch_RevoluteJointSetSpringDampingRatio( const b2RecArgs_RevoluteJointSetSpringDampingRatio* a,+															  b2RecReader* rdr )+{+	b2RevoluteJoint_SetSpringDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b2RecDispatch_RevoluteJointSetTargetAngle( const b2RecArgs_RevoluteJointSetTargetAngle* a, b2RecReader* rdr )+{+	b2RevoluteJoint_SetTargetAngle( b2RecMakeJointId( rdr, a->joint ), a->angle );+}++static void b2RecDispatch_RevoluteJointEnableLimit( const b2RecArgs_RevoluteJointEnableLimit* a, b2RecReader* rdr )+{+	b2RevoluteJoint_EnableLimit( b2RecMakeJointId( rdr, a->joint ), a->enableLimit );+}++static void b2RecDispatch_RevoluteJointSetLimits( const b2RecArgs_RevoluteJointSetLimits* a, b2RecReader* rdr )+{+	b2RevoluteJoint_SetLimits( b2RecMakeJointId( rdr, a->joint ), a->lower, a->upper );+}++static void b2RecDispatch_RevoluteJointEnableMotor( const b2RecArgs_RevoluteJointEnableMotor* a, b2RecReader* rdr )+{+	b2RevoluteJoint_EnableMotor( b2RecMakeJointId( rdr, a->joint ), a->enableMotor );+}++static void b2RecDispatch_RevoluteJointSetTargetMotorSpeed( const b2RecArgs_RevoluteJointSetTargetMotorSpeed* a, b2RecReader* rdr )+{+	b2RevoluteJoint_SetTargetMotorSpeed( b2RecMakeJointId( rdr, a->joint ), a->motorSpeed );+}++static void b2RecDispatch_RevoluteJointSetMaxMotorTorque( const b2RecArgs_RevoluteJointSetMaxMotorTorque* a, b2RecReader* rdr )+{+	b2RevoluteJoint_SetMaxMotorTorque( b2RecMakeJointId( rdr, a->joint ), a->torque );+}++// Weld joint++static void b2RecDispatch_WeldJointSetLinearHertz( const b2RecArgs_WeldJointSetLinearHertz* a, b2RecReader* rdr )+{+	b2WeldJoint_SetLinearHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b2RecDispatch_WeldJointSetLinearDampingRatio( const b2RecArgs_WeldJointSetLinearDampingRatio* a, b2RecReader* rdr )+{+	b2WeldJoint_SetLinearDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b2RecDispatch_WeldJointSetAngularHertz( const b2RecArgs_WeldJointSetAngularHertz* a, b2RecReader* rdr )+{+	b2WeldJoint_SetAngularHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b2RecDispatch_WeldJointSetAngularDampingRatio( const b2RecArgs_WeldJointSetAngularDampingRatio* a, b2RecReader* rdr )+{+	b2WeldJoint_SetAngularDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++// Wheel joint++static void b2RecDispatch_WheelJointEnableSpring( const b2RecArgs_WheelJointEnableSpring* a, b2RecReader* rdr )+{+	b2WheelJoint_EnableSpring( b2RecMakeJointId( rdr, a->joint ), a->enableSpring );+}++static void b2RecDispatch_WheelJointSetSpringHertz( const b2RecArgs_WheelJointSetSpringHertz* a, b2RecReader* rdr )+{+	b2WheelJoint_SetSpringHertz( b2RecMakeJointId( rdr, a->joint ), a->hertz );+}++static void b2RecDispatch_WheelJointSetSpringDampingRatio( const b2RecArgs_WheelJointSetSpringDampingRatio* a, b2RecReader* rdr )+{+	b2WheelJoint_SetSpringDampingRatio( b2RecMakeJointId( rdr, a->joint ), a->dampingRatio );+}++static void b2RecDispatch_WheelJointEnableLimit( const b2RecArgs_WheelJointEnableLimit* a, b2RecReader* rdr )+{+	b2WheelJoint_EnableLimit( b2RecMakeJointId( rdr, a->joint ), a->enableLimit );+}++static void b2RecDispatch_WheelJointSetLimits( const b2RecArgs_WheelJointSetLimits* a, b2RecReader* rdr )+{+	b2WheelJoint_SetLimits( b2RecMakeJointId( rdr, a->joint ), a->lower, a->upper );+}++static void b2RecDispatch_WheelJointEnableMotor( const b2RecArgs_WheelJointEnableMotor* a, b2RecReader* rdr )+{+	b2WheelJoint_EnableMotor( b2RecMakeJointId( rdr, a->joint ), a->enableMotor );+}++static void b2RecDispatch_WheelJointSetTargetMotorSpeed( const b2RecArgs_WheelJointSetTargetMotorSpeed* a, b2RecReader* rdr )+{+	b2WheelJoint_SetTargetMotorSpeed( b2RecMakeJointId( rdr, a->joint ), a->motorSpeed );+}++static void b2RecDispatch_WheelJointSetMaxMotorTorque( const b2RecArgs_WheelJointSetMaxMotorTorque* a, b2RecReader* rdr )+{+	b2WheelJoint_SetMaxMotorTorque( b2RecMakeJointId( rdr, a->joint ), a->torque );+}++// Float bit comparators: compare raw bits so NaN != NaN is handled consistently++static bool b2RecF32Differs( float a, float b )+{+	uint32_t ua, ub;+	memcpy( &ua, &a, 4 );+	memcpy( &ub, &b, 4 );+	return ua != ub;+}++static bool b2RecVec2Differs( b2Vec2 a, b2Vec2 b )+{+	return b2RecF32Differs( a.x, b.x ) || b2RecF32Differs( a.y, b.y );+}++// Per-frame query stash: push a draw record and copy its hits into frameHits.+// Ids in hits[] are already remapped to the replay world by the caller.++static void b2RecGrowFrameQueries( b2Replay* player )+{+	b2RecGrow( (void**)&player->frameQueries, &player->frameQueryCap, player->frameQueryCount + 1, player->frameQueryCount,+			   (int)sizeof( b2RecDrawQuery ) );+}++static void b2RecGrowFrameHits( b2Replay* player, int need )+{+	b2RecGrow( (void**)&player->frameHits, &player->frameHitCap, player->frameHitCount + need, player->frameHitCount,+			   (int)sizeof( b2RecRecordedHit ) );+}++static b2RecDrawQuery* b2RecStashQueryBegin( b2Replay* player, int kind, const b2RecRecordedHit* hits, int hitCount )+{+	b2RecGrowFrameQueries( player );+	b2RecDrawQuery* q = &player->frameQueries[player->frameQueryCount];+	memset( q, 0, sizeof( *q ) );+	q->kind = kind;+	q->hitStart = player->frameHitCount;+	q->hitCount = hitCount;+	b2RecGrowFrameHits( player, hitCount );+	for ( int i = 0; i < hitCount; ++i )+	{+		player->frameHits[player->frameHitCount + i] = hits[i];+	}+	player->frameHitCount += hitCount;+	player->frameQueryCount++;+	return q;+}++// Overlap AABB dispatcher++// Shared context for the query replay trampolines: walks the recorded hits in order and flags+// any divergence from the re-issued query+typedef struct b2RecReplayQueryCtx+{+	b2RecReader* rdr;+	const b2RecRecordedHit* hits;+	int count;+	int cursor;+} b2RecReplayQueryCtx;++static bool b2RecReplayOverlapTrampoline( b2ShapeId id, void* ctx )+{+	b2RecReplayQueryCtx* rc = ctx;+	if ( rc->cursor >= rc->count )+	{+		rc->rdr->diverged = true;+		return false;+	}+	const b2RecRecordedHit* h = &rc->hits[rc->cursor++];+	if ( id.index1 != h->id.index1 || id.generation != h->id.generation )+	{+		rc->rdr->diverged = true;+	}+	return h->userReturnB;+}++static void b2RecDispatch_QueryOverlapAABB( const b2RecArgs_QueryOverlapAABB* a, b2RecReader* rdr )+{+	uint32_t n = b2RecR_U32( rdr );+	b2RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+	{+		return;+	}+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b2RecMakeShapeId( rdr, b2RecR_SHAPEID( rdr ) );+		rdr->hits[i].userReturnB = b2RecR_BOOL( rdr );+	}+	(void)b2RecR_TREESTATS( rdr );+	if ( !rdr->ok )+		return;+	b2RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	b2World_OverlapAABB( rdr->replayWorldId, a->origin, a->aabb, a->filter, b2RecReplayOverlapTrampoline, &rc );+	if ( rc.cursor != (int)n )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b2RecDrawQuery* q = b2RecStashQueryBegin( rdr->owner, B2_RECQ_OVERLAP_AABB, rdr->hits, (int)n );+		q->filter = a->filter;+		q->origin = a->origin;+		q->aabb = a->aabb;+	}+}++static void b2RecDispatch_QueryOverlapShape( const b2RecArgs_QueryOverlapShape* a, b2RecReader* rdr )+{+	uint32_t n = b2RecR_U32( rdr );+	b2RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+	{+		return;+	}+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b2RecMakeShapeId( rdr, b2RecR_SHAPEID( rdr ) );+		rdr->hits[i].userReturnB = b2RecR_BOOL( rdr );+	}+	(void)b2RecR_TREESTATS( rdr );+	if ( !rdr->ok )+		return;+	b2RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	b2World_OverlapShape( rdr->replayWorldId, a->origin, &a->proxy, a->filter, b2RecReplayOverlapTrampoline, &rc );+	if ( rc.cursor != (int)n )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b2RecDrawQuery* q = b2RecStashQueryBegin( rdr->owner, B2_RECQ_OVERLAP_SHAPE, rdr->hits, (int)n );+		q->filter = a->filter;+		q->origin = a->origin;+		q->proxy = a->proxy;+	}+}++// Cast ray dispatcher++static float b2RecReplayCastTrampoline( b2ShapeId id, b2Pos point, b2Vec2 normal, float fraction, void* ctx )+{+	b2RecReplayQueryCtx* rc = ctx;+	if ( rc->cursor >= rc->count )+	{+		rc->rdr->diverged = true;+		return 0.0f;+	}+	const b2RecRecordedHit* h = &rc->hits[rc->cursor++];+	// Compare positions through the full width delta, truncating both sides would pass vacuously+	// far from the origin+	if ( id.index1 != h->id.index1 || id.generation != h->id.generation ||+		 b2RecVec2Differs( b2SubPos( point, h->point ), b2Vec2_zero ) || b2RecVec2Differs( normal, h->normal ) ||+		 b2RecF32Differs( fraction, h->fraction ) )+	{+		rc->rdr->diverged = true;+	}+	return h->userReturnF;+}++static void b2RecDispatch_QueryCastRay( const b2RecArgs_QueryCastRay* a, b2RecReader* rdr )+{+	uint32_t n = b2RecR_U32( rdr );+	b2RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+	{+		return;+	}+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b2RecMakeShapeId( rdr, b2RecR_SHAPEID( rdr ) );+		rdr->hits[i].point = b2RecR_POSITION( rdr );+		rdr->hits[i].normal = b2RecR_VEC2( rdr );+		rdr->hits[i].fraction = b2RecR_F32( rdr );+		rdr->hits[i].userReturnF = b2RecR_F32( rdr );+	}+	(void)b2RecR_TREESTATS( rdr );+	if ( !rdr->ok )+		return;+	b2RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	b2World_CastRay( rdr->replayWorldId, a->origin, a->translation, a->filter, b2RecReplayCastTrampoline, &rc );+	if ( rc.cursor != (int)n )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b2RecDrawQuery* q = b2RecStashQueryBegin( rdr->owner, B2_RECQ_CAST_RAY, rdr->hits, (int)n );+		q->filter = a->filter;+		q->origin = a->origin;+		q->translation = a->translation;+	}+}++static void b2RecDispatch_QueryCastShape( const b2RecArgs_QueryCastShape* a, b2RecReader* rdr )+{+	uint32_t n = b2RecR_U32( rdr );+	b2RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+	{+		return;+	}+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b2RecMakeShapeId( rdr, b2RecR_SHAPEID( rdr ) );+		rdr->hits[i].point = b2RecR_POSITION( rdr );+		rdr->hits[i].normal = b2RecR_VEC2( rdr );+		rdr->hits[i].fraction = b2RecR_F32( rdr );+		rdr->hits[i].userReturnF = b2RecR_F32( rdr );+	}+	(void)b2RecR_TREESTATS( rdr );+	if ( !rdr->ok )+		return;+	b2RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	b2World_CastShape( rdr->replayWorldId, a->origin, &a->proxy, a->translation, a->filter, b2RecReplayCastTrampoline, &rc );+	if ( rc.cursor != (int)n )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b2RecDrawQuery* q = b2RecStashQueryBegin( rdr->owner, B2_RECQ_CAST_SHAPE, rdr->hits, (int)n );+		q->filter = a->filter;+		q->origin = a->origin;+		q->proxy = a->proxy;+		q->translation = a->translation;+	}+}++// CollideMover dispatcher++static bool b2RecReplayPlaneTrampoline( b2ShapeId id, const b2PlaneResult* plane, void* ctx )+{+	b2RecReplayQueryCtx* rc = ctx;+	if ( rc->cursor >= rc->count )+	{+		rc->rdr->diverged = true;+		return true;+	}+	const b2RecRecordedHit* h = &rc->hits[rc->cursor++];+	if ( id.index1 != h->id.index1 || id.generation != h->id.generation ||+		 b2RecVec2Differs( plane->plane.normal, h->plane.plane.normal ) ||+		 b2RecF32Differs( plane->plane.offset, h->plane.plane.offset ) || b2RecVec2Differs( plane->point, h->plane.point ) ||+		 plane->hit != h->plane.hit )+	{+		rc->rdr->diverged = true;+	}+	return h->userReturnB;+}++static void b2RecDispatch_QueryCollideMover( const b2RecArgs_QueryCollideMover* a, b2RecReader* rdr )+{+	uint32_t n = b2RecR_U32( rdr );+	b2RecEnsureHits( rdr, (int)n );+	if ( !rdr->ok )+	{+		return;+	}+	for ( uint32_t i = 0; i < n; ++i )+	{+		rdr->hits[i].id = b2RecMakeShapeId( rdr, b2RecR_SHAPEID( rdr ) );+		rdr->hits[i].plane = b2RecR_PLANERESULT( rdr );+		rdr->hits[i].userReturnB = b2RecR_BOOL( rdr );+	}+	// CollideMover has no TREESTATS tail (returns void)+	if ( !rdr->ok )+		return;+	b2RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 };+	b2World_CollideMover( rdr->replayWorldId, a->origin, &a->mover, a->filter, b2RecReplayPlaneTrampoline, &rc );+	if ( rc.cursor != (int)n )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b2RecDrawQuery* q = b2RecStashQueryBegin( rdr->owner, B2_RECQ_COLLIDE_MOVER, rdr->hits, (int)n );+		q->filter = a->filter;+		q->origin = a->origin;+		q->mover = a->mover;+	}+}++// CastRayClosest dispatcher++static void b2RecDispatch_QueryCastRayClosest( const b2RecArgs_QueryCastRayClosest* a, b2RecReader* rdr )+{+	b2RayResult rec = b2RecR_RAYRESULT( rdr );+	if ( !rdr->ok )+		return;+	b2RayResult got = b2World_CastRayClosest( rdr->replayWorldId, a->origin, a->translation, a->filter );+	if ( got.hit != rec.hit ||+		 ( got.hit && ( got.shapeId.index1 != rec.shapeId.index1 || got.shapeId.generation != rec.shapeId.generation ||+						b2RecVec2Differs( b2SubPos( got.point, rec.point ), b2Vec2_zero ) ||+						b2RecVec2Differs( got.normal, rec.normal ) || b2RecF32Differs( got.fraction, rec.fraction ) ) ) )+	{+		rdr->diverged = true;+	}+	if ( rdr->owner )+	{+		// Stash the closest result as a single pooled hit so the shared draw loop renders its point+		b2RecRecordedHit h = { 0 };+		h.id = b2RecMakeShapeId( rdr, rec.shapeId );+		h.point = rec.point;+		h.normal = rec.normal;+		h.fraction = rec.fraction;+		b2RecDrawQuery* q = b2RecStashQueryBegin( rdr->owner, B2_RECQ_CAST_RAY_CLOSEST, &h, rec.hit ? 1 : 0 );+		q->filter = a->filter;+		q->origin = a->origin;+		q->translation = a->translation;+	}+}++// CastMover dispatcher++static void b2RecDispatch_QueryCastMover( const b2RecArgs_QueryCastMover* a, b2RecReader* rdr )+{+	float rec = b2RecR_F32( rdr );+	if ( !rdr->ok )+		return;+	float got = b2World_CastMover( rdr->replayWorldId, a->origin, &a->mover, a->translation, a->filter );+	if ( b2RecF32Differs( got, rec ) )+		rdr->diverged = true;+	if ( rdr->owner )+	{+		b2RecDrawQuery* q = b2RecStashQueryBegin( rdr->owner, B2_RECQ_CAST_MOVER, NULL, 0 );+		q->filter = a->filter;+		q->origin = a->origin;+		q->mover = a->mover;+		q->translation = a->translation;+		q->castFraction = rec;+	}+}++static void b2RecDispatch_StateHash( const b2RecArgs_StateHash* a, b2RecReader* rdr )+{+	b2World* world = b2GetWorldFromId( rdr->replayWorldId );+	uint64_t computed = b2HashWorldState( world );+	if ( computed != a->hash )+	{+		printf( "b2ReplayFile: StateHash mismatch (recorded=0x%llX, computed=0x%llX)\n", (unsigned long long)a->hash,+				(unsigned long long)computed );+		// Non-fatal: reading continues so a viewer can show where divergence begins+		rdr->diverged = true;+	}+}++static void b2RecDispatch_RecordingBounds( const b2RecArgs_RecordingBounds* a, b2RecReader* rdr )+{+	// Primary resolve is the open-time scan, this keeps the value right if it ever moves earlier+	rdr->owner->bounds = a->bounds;+}++// Codegen pass 2 builds the read-and-dispatch switch cases. Each case reads the ARG fields+// into a b2RecArgs_<Name> then dispatches. Create ops read the returned id in their dispatcher.+// Returns the opcode just dispatched, or -1 at end of file or on a fatal read error.++static int b2RecDispatchOne( b2Replay* player )+{+	b2RecReader* rdr = &player->rdr;+	if ( rdr->cursor >= rdr->size || !rdr->ok )+	{+		return -1;+	}++	uint8_t opcode = b2RecR_U8( rdr );+	if ( !rdr->ok )+	{+		return -1;+	}+	uint32_t payloadSize = b2RecR_U24( rdr );+	if ( !rdr->ok )+	{+		return -1;+	}+	int payloadStart = rdr->cursor;++	switch ( opcode )+	{+#define ARG( TAG, field ) a.field = b2RecR_##TAG( rdr );+#define B2_REC_OP( op, Name, RET, ... )                                                                                          \+	case op:                                                                                                                     \+	{                                                                                                                            \+		b2RecArgs_##Name a;                                                                                                      \+		memset( &a, 0, sizeof( a ) );                                                                                            \+		__VA_ARGS__                                                                                                              \+		if ( rdr->ok )                                                                                                           \+		{                                                                                                                        \+			b2RecDispatch_##Name( &a, rdr );                                                                                     \+		}                                                                                                                        \+		break;                                                                                                                   \+	}+#include "recording_ops.inl"+#undef B2_REC_OP+#undef ARG+		default:+			printf( "b2ReplayFile: 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)opcode;+}++// Walk the records once without dispatching to count steps and read the first step's tuning.+// The framing is opcode u8 + payload u24 + payload, so we can skip records blind.+static void b2RecScanFile( b2Replay* player )+{+	const uint8_t* data = player->data;+	int size = player->size;+	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 == 0x80 ) // Step: [u32 world][f32 dt][i32 subStepCount]+		{+			frameCount += 1;+			if ( gotStep == false && 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 >= 16 ) // RecordingBounds: [f32 lo.x][lo.y][hi.x][hi.y]+		{+			// Little-endian floats match b2AABB layout, header already rejected big-endian files+			memcpy( &player->bounds, data + payloadStart, 16 );+		}++		cursor = payloadStart + (int)payloadSize;+	}++	player->frameCount = frameCount;+}++b2Replay* b2CreateReplay( const void* data, int size, int workerCount )+{+	if ( data == NULL || size < 32 )+	{+		printf( "b2CreateReplay: recording too small\n" );+		return NULL;+	}++	// Validate the header before copying anything+	b2RecHeader hdr;+	memcpy( &hdr, data, sizeof( b2RecHeader ) );++	if ( hdr.magic != B2_REC_MAGIC )+	{+		printf( "b2CreateReplay: bad magic (got 0x%08X)\n", hdr.magic );+		return NULL;+	}++	if ( hdr.versionMajor != B2_REC_VERSION_MAJOR || hdr.versionMinor != B2_REC_VERSION_MINOR )+	{+		printf( "b2CreateReplay: version mismatch (file=%u.%u, runtime=%u.%u)\n", hdr.versionMajor, hdr.versionMinor,+				B2_REC_VERSION_MAJOR, B2_REC_VERSION_MINOR );+		return NULL;+	}++	if ( hdr.pointerWidth != (uint8_t)sizeof( void* ) )+	{+		printf( "b2CreateReplay: pointer width mismatch (file=%u, runtime=%u)\n", hdr.pointerWidth,+				(unsigned)sizeof( void* ) );+		return NULL;+	}++	if ( hdr.bigEndian != 0 )+	{+		printf( "b2CreateReplay: big-endian recording not supported\n" );+		return NULL;+	}++	// Every recording is snapshot-seeded: the blob sits between the header and the op stream+	if ( hdr.snapshotSize == 0 || hdr.snapshotSize > (uint64_t)( size - 32 ) )+	{+		printf( "b2CreateReplay: missing or oversized snapshot\n" );+		return NULL;+	}+	int headerEnd = 32 + (int)hdr.snapshotSize;++	// Own a private copy of the bytes so the caller can free its buffer right after this call+	uint8_t* copy = b2Alloc( size );+	memcpy( copy, data, (size_t)size );++	b2Replay* player = b2Alloc( (int)sizeof( b2Replay ) );+	player->data = copy;+	player->size = size;+	player->headerEnd = headerEnd;+	player->lengthScale = hdr.lengthScale;+	player->previousLengthScale = b2GetLengthUnitsPerMeter();+	player->frame = 0;+	player->frameCount = 0;+	player->recordedWorkerCount = 0;+	player->recordedDt = 0.0f;+	player->recordedSubStepCount = 0;+	player->bounds = (b2AABB){ 0 };+	player->divergeFrame = -1;+	player->atEnd = false;+	player->rdr.data = copy;+	player->rdr.size = size;+	player->rdr.cursor = headerEnd; // past header and the snapshot blob+	player->rdr.replayWorldId = b2_nullWorldId;+	player->rdr.workerCount = workerCount;+	player->rdr.ok = true;+	player->rdr.diverged = false;+	player->rdr.chainPoints = NULL;+	player->rdr.chainPointCap = 0;+	player->rdr.chainMaterials = NULL;+	player->rdr.chainMaterialCap = 0;+	player->rdr.hits = NULL;+	player->rdr.hitCap = 0;+	player->rdr.owner = player;+	player->frameQueries = NULL;+	player->frameQueryCount = 0;+	player->frameQueryCap = 0;+	player->frameHits = NULL;+	player->frameHitCount = 0;+	player->frameHitCap = 0;+	player->bodyIds = NULL;+	player->bodyIdCount = 0;+	player->bodyIdCap = 0;+	player->frame0Image = NULL;+	player->frame0Size = 0;+	player->frame0BodyIds = NULL;+	player->frame0BodyIdCount = 0;+	player->keyframes = NULL;+	player->keyframeCount = 0;+	player->keyframeCapacity = 0;+	player->keyframeBudget = B2_REC_KEYFRAME_BUDGET_DEFAULT;+	player->keyframeBytes = 0;+	player->keyframeMinInterval = B2_REC_KEYFRAME_INTERVAL_DEFAULT;+	player->keyframeInterval = B2_REC_KEYFRAME_INTERVAL_DEFAULT;+	player->lastKeyframeFrame = 0;++	// Override the global length scale with the recording's so replay reproduces the same constants.+	// This is global engine state and affects the caller's other worlds, so the previous value was+	// captured above and is restored in b2DestroyReplay.+	if ( hdr.lengthScale > 0.0f )+	{+		b2SetLengthUnitsPerMeter( hdr.lengthScale );+	}++	// Count steps and read the first step's tuning so the viewer can show length and hz up front+	b2RecScanFile( player );++	// Deserialize the seed snapshot to stand up the replay world. The op stream that follows is the+	// hook log. The blob doubles as the frame-0 restore image, owned by the copy we hold.+	player->rdr.replayWorldId = b2CreateWorldFromSnapshot( copy + 32, (int)hdr.snapshotSize, workerCount );+	if ( b2World_IsValid( player->rdr.replayWorldId ) == false )+	{+		printf( "b2CreateReplay: snapshot deserialize failed\n" );+		b2DestroyReplay( player );+		return NULL;+	}+	player->recordedWorkerCount = workerCount;+	player->frame0Image = copy + 32;+	player->frame0Size = (int)hdr.snapshotSize;++	// The seed snapshot holds the bodies present when recording began; only post-snapshot creates+	// reach the tracker, so seed the outliner list directly from the restored world+	b2RecSeedBodyIds( player );++	// Stash the frame-0 list so a restart or backward scrub rolls the outliner back to it+	player->frame0BodyIdCount = player->bodyIdCount;+	if ( player->bodyIdCount > 0 )+	{+		player->frame0BodyIds = b2Alloc( player->bodyIdCount * (int)sizeof( b2BodyId ) );+		memcpy( player->frame0BodyIds, player->bodyIds, player->bodyIdCount * (int)sizeof( b2BodyId ) );+	}++	return player;+}++// Free a keyframe's heap. image is freed at its allocation size, which over-allocates the logical+// image, so the free size matches the alloc.+static void b2FreeKeyframe( b2RecKeyframe* kf )+{+	b2Free( kf->image, kf->imageCapacity );+	if ( kf->bodyIds != NULL )+	{+		b2Free( kf->bodyIds, kf->bodyIdCount * (int)sizeof( b2BodyId ) );+	}+}++// Capture a restore point for the just-completed frame. rdr.cursor already sits at the next frame's+// Step, so this records the exact resume position next to a full world image plus the outliner and+// divergence state forward stepping would otherwise have to rebuild.+static void b2RecCaptureKeyframe( b2Replay* player )+{+	// Serialize into a buffer the keyframe takes ownership of, so there is no second full-size alloc+	// and copy. The buffer over-allocates, so the budget and free track its capacity, not its size.+	b2World* world = b2GetWorldFromId( player->rdr.replayWorldId );+	b2RecBuffer buf = { 0 };+	b2SerializeWorld( world, &buf );++	size_t bodyBytes = (size_t)player->bodyIdCount * sizeof( b2BodyId );+	size_t newBytes = (size_t)buf.capacity + bodyBytes;++	// Make room under the budget: doubling the spacing drops the off-grid keyframes, roughly halving+	// the bytes, until the new keyframe fits or only it remains. The budget is soft in the corner+	// where a single snapshot already exceeds it.+	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 )+		{+			b2RecKeyframe* kf = &player->keyframes[i];+			if ( kf->frame % player->keyframeInterval == 0 )+			{+				player->keyframes[kept] = *kf;+				keptBytes += (size_t)kf->imageCapacity + (size_t)kf->bodyIdCount * sizeof( b2BodyId );+				kept += 1;+			}+			else+			{+				b2FreeKeyframe( kf );+			}+		}+		bool progress = kept < player->keyframeCount;+		player->keyframeCount = kept;+		player->keyframeBytes = keptBytes;+		if ( progress == false )+		{+			break;+		}+	}++	b2RecGrow( (void**)&player->keyframes, &player->keyframeCapacity, player->keyframeCount + 1, player->keyframeCount,+			   (int)sizeof( b2RecKeyframe ) );++	b2RecKeyframe* kf = &player->keyframes[player->keyframeCount];+	// Hand the serialized buffer to the keyframe rather than copying it into an exact-size block+	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 = b2Alloc( bodyBytes );+		memcpy( kf->bodyIds, player->bodyIds, (size_t)bodyBytes );+	}++	player->keyframeBytes += newBytes;+	player->keyframeCount += 1;+	player->lastKeyframeFrame = player->frame;+}++// Restore the world and player state from a keyframe, so a backward seek resumes from it instead of+// frame 0. Mirrors b2Replay_Restart but targets a mid-stream image. b2World_RestoreFromSnapshot is in place,+// so the replay world id stays stable.+static void b2ReplayRestoreKeyframe( b2Replay* player, const b2RecKeyframe* kf )+{+	if ( b2World_RestoreFromSnapshot( player->rdr.replayWorldId, kf->image, kf->imageSize ) == 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;++	b2RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, kf->bodyIdCount, 0, (int)sizeof( b2BodyId ) );+	player->bodyIdCount = kf->bodyIdCount;+	if ( kf->bodyIdCount > 0 )+	{+		memcpy( player->bodyIds, kf->bodyIds, kf->bodyIdCount * (int)sizeof( b2BodyId ) );+	}+}++bool b2Replay_StepFrame( b2Replay* player )+{+	if ( player->atEnd )+	{+		return false;+	}++	// Reset per-frame query store before dispatching new records+	player->frameQueryCount = 0;+	player->frameHitCount = 0;++	// Run this frame's Step, then consume the records that trail it (StateHash, queries, any+	// between-frame mutators) up to the next Step. The queries and hash for a frame are recorded+	// after its Step, so grouping them with that Step keeps them paired with the world state they+	// were computed against. Stopping before the next Step is what advances exactly one frame.+	bool stepped = false;+	for ( ;; )+	{+		// Peek the next opcode without consuming it. The next frame's Step ends this frame.+		if ( player->rdr.cursor >= player->rdr.size || player->rdr.ok == false )+		{+			player->atEnd = true;+			return stepped;+		}+		if ( stepped && player->rdr.data[player->rdr.cursor] == 0x80 )+		{+			// Capture a keyframe at the interval. The guard skips frames already covered, so+			// re-stepping a gap during a backward seek never re-captures.+			if ( player->frame > player->lastKeyframeFrame && player->frame % player->keyframeInterval == 0 )+			{+				b2RecCaptureKeyframe( player );+			}+			return true;+		}++		int opcode = b2RecDispatchOne( player );+		if ( opcode < 0 )+		{+			player->atEnd = true;+			return stepped;+		}+		if ( opcode == 0x80 ) // Step+		{+			player->frame += 1;+			stepped = true;+		}+		// Latch the first frame that diverged for the timeline marker+		if ( player->divergeFrame < 0 && player->rdr.diverged )+		{+			player->divergeFrame = player->frame;+		}+	}+}++void b2Replay_Restart( b2Replay* 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.+	if ( b2World_RestoreFromSnapshot( player->rdr.replayWorldId, player->frame0Image, player->frame0Size ) == false )+	{+		player->rdr.ok = false;+		return;+	}+	// Stepping resumes at the first Step, which sits right after the header and snapshot blob+	player->rdr.cursor = player->headerEnd;+	player->rdr.ok = true;+	player->rdr.diverged = false;+	player->frame = 0;+	player->divergeFrame = -1;+	player->atEnd = false;++	// Frame 0 is the pre-step snapshot, so it has no recorded queries. Clear the per-frame store so+	// the last stepped frame's queries do not linger on a load or a backward scrub to the start.+	player->frameQueryCount = 0;+	player->frameHitCount = 0;++	// Roll the outliner body list back to its frame-0 contents+	player->bodyIdCount = player->frame0BodyIdCount;+	if ( player->frame0BodyIdCount > 0 )+	{+		memcpy( player->bodyIds, player->frame0BodyIds, player->frame0BodyIdCount * (int)sizeof( b2BodyId ) );+	}+}++b2WorldId b2Replay_GetWorldId( const b2Replay* player )+{+	return player != NULL ? player->rdr.replayWorldId : b2_nullWorldId;+}++int b2Replay_GetFrame( const b2Replay* player )+{+	return player != NULL ? player->frame : 0;+}++void b2Replay_SeekFrame( b2Replay* player, int targetFrame )+{+	if ( player == NULL )+	{+		return;+	}++	if ( targetFrame < 0 )+	{+		targetFrame = 0;+	}++	// Resume from the nearest keyframe strictly below the target when it beats the current cursor.+	// A backward seek must restore since the cursor cannot rewind. A forward seek restores only when+	// a keyframe sits ahead of the cursor, capping a long forward fling at one keyframe interval of+	// replay instead of every intervening frame. Strictly below so the step loop still runs the+	// target frame and regenerates its per-frame query store, body list, and divergence latch+	// exactly as a plain forward replay would.+	const b2RecKeyframe* best = NULL;+	for ( int i = 0; i < player->keyframeCount; ++i )+	{+		const b2RecKeyframe* kf = &player->keyframes[i];+		if ( kf->frame < targetFrame && ( best == NULL || kf->frame > best->frame ) )+		{+			best = kf;+		}+	}++	if ( targetFrame < player->frame )+	{+		if ( best != NULL )+		{+			b2ReplayRestoreKeyframe( player, best );+		}+		else+		{+			b2Replay_Restart( player );+		}+	}+	else if ( best != NULL && best->frame > player->frame )+	{+		b2ReplayRestoreKeyframe( player, best );+	}++	while ( player->frame < targetFrame && b2Replay_StepFrame( player ) )+	{+	}+}++b2ReplayInfo b2Replay_GetInfo( const b2Replay* player )+{+	b2ReplayInfo 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;+}++bool b2Replay_IsAtEnd( const b2Replay* player )+{+	return player != NULL ? player->atEnd : true;+}++bool b2Replay_HasDiverged( const b2Replay* player )+{+	return player != NULL ? player->rdr.diverged : false;+}++int b2Replay_GetDivergeFrame( const b2Replay* player )+{+	return player != NULL ? player->divergeFrame : -1;+}++void b2Replay_SetKeyframePolicy( b2Replay* 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 )+	{+		b2FreeKeyframe( &player->keyframes[i] );+	}+	player->keyframeCount = 0;+	player->keyframeBytes = 0;+	player->keyframeInterval = player->keyframeMinInterval;+	player->lastKeyframeFrame = 0;+}++size_t b2Replay_GetKeyframeBudget( const b2Replay* player )+{+	return player != NULL ? player->keyframeBudget : 0;+}++int b2Replay_GetKeyframeMinInterval( const b2Replay* player )+{+	return player != NULL ? player->keyframeMinInterval : 0;+}++int b2Replay_GetKeyframeInterval( const b2Replay* player )+{+	return player != NULL ? player->keyframeInterval : 0;+}++size_t b2Replay_GetKeyframeBytes( const b2Replay* player )+{+	return player != NULL ? player->keyframeBytes : 0;+}++void b2DestroyReplay( b2Replay* player )+{+	if ( player == NULL )+	{+		return;+	}+	if ( b2World_IsValid( player->rdr.replayWorldId ) )+	{+		b2DestroyWorld( player->rdr.replayWorldId );+	}+	if ( player->data != NULL )+	{+		b2Free( player->data, player->size );+	}+	if ( player->rdr.chainPoints != NULL )+	{+		b2Free( player->rdr.chainPoints, player->rdr.chainPointCap * (int)sizeof( b2Vec2 ) );+	}+	if ( player->rdr.chainMaterials != NULL )+	{+		b2Free( player->rdr.chainMaterials, player->rdr.chainMaterialCap * (int)sizeof( b2SurfaceMaterial ) );+	}+	if ( player->rdr.hits != NULL )+	{+		b2Free( player->rdr.hits, player->rdr.hitCap * (int)sizeof( b2RecRecordedHit ) );+	}+	if ( player->frameQueries != NULL )+	{+		b2Free( player->frameQueries, player->frameQueryCap * (int)sizeof( b2RecDrawQuery ) );+	}+	if ( player->frameHits != NULL )+	{+		b2Free( player->frameHits, player->frameHitCap * (int)sizeof( b2RecRecordedHit ) );+	}+	if ( player->bodyIds != NULL )+	{+		b2Free( player->bodyIds, player->bodyIdCap * (int)sizeof( b2BodyId ) );+	}+	// frame0Image points into the owned data copy, freed above, so it is not freed here+	if ( player->frame0BodyIds != NULL )+	{+		b2Free( player->frame0BodyIds, player->frame0BodyIdCount * (int)sizeof( b2BodyId ) );+	}+	for ( int i = 0; i < player->keyframeCount; ++i )+	{+		b2FreeKeyframe( &player->keyframes[i] );+	}+	if ( player->keyframes != NULL )+	{+		b2Free( player->keyframes, (size_t)player->keyframeCapacity * sizeof( b2RecKeyframe ) );+	}++	// Restore the global length scale.+	b2SetLengthUnitsPerMeter( player->previousLengthScale );++	b2Free( player, (int)sizeof( b2Replay ) );+}++// Highlight each reported overlap shape by its AABB. Skip any destroyed since the query,+// per the b2Shape_GetBounds contract that overlap results may contain stale shapes.+static void b2RecDrawHitAABBs( const b2Replay* player, const b2RecDrawQuery* q, b2DebugDraw* draw )+{+	if ( draw->DrawPolygonFcn == NULL )+	{+		return;+	}+	for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi )+	{+		b2ShapeId id = player->frameHits[hi].id;+		if ( b2Shape_IsValid( id ) == false )+		{+			continue;+		}+		b2AABB b = b2Shape_GetBounds( id );+		b2Vec2 lower = b.lowerBound;+		b2Vec2 upper = b.upperBound;+		b2Vec2 vs[4] = { lower, { upper.x, lower.y }, upper, { lower.x, upper.y } };+		draw->DrawPolygonFcn( b2WorldTransform_identity, vs, 4, b2_colorMagenta, draw->context );+	}+}++void b2Replay_DrawFrameQueries( b2Replay* player, b2DebugDraw* draw, int queryIndex )+{+	if ( player == NULL || draw == NULL )+		return;++	// queryIndex < 0 draws all queries, otherwise just the one selected in the viewer+	for ( int qi = 0; qi < player->frameQueryCount; ++qi )+	{+		if ( queryIndex >= 0 && qi != queryIndex )+		{+			continue;+		}++		const b2RecDrawQuery* q = &player->frameQueries[qi];++		switch ( q->kind )+		{+			case B2_RECQ_CAST_RAY:+			case B2_RECQ_CAST_RAY_CLOSEST:+			{+				// Ray origin to endpoint+				b2Pos origin = q->origin;+				b2Pos end = b2OffsetPos( origin, q->translation );+				if ( draw->DrawLineFcn )+				{+					draw->DrawLineFcn( origin, end, b2_colorYellow, draw->context );+				}+				// Per-hit point + short normal+				for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi )+				{+					const b2RecRecordedHit* h = &player->frameHits[hi];+					b2Pos point = h->point;+					if ( draw->DrawPointFcn )+					{+						draw->DrawPointFcn( point, 4.0f, b2_colorYellow, draw->context );+					}+					if ( draw->DrawLineFcn )+					{+						b2Pos np = b2OffsetPos( point, b2MulSV( 0.2f, h->normal ) );+						draw->DrawLineFcn( point, np, b2_colorLightYellow, draw->context );+					}+				}+				break;+			}+			case B2_RECQ_CAST_SHAPE:+			{+				// Shape cast: draw per-hit points along the swept path+				for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi )+				{+					const b2RecRecordedHit* h = &player->frameHits[hi];+					b2Pos point = h->point;+					if ( draw->DrawPointFcn )+					{+						draw->DrawPointFcn( point, 4.0f, b2_colorSkyBlue, draw->context );+					}+					if ( draw->DrawLineFcn )+					{+						b2Pos np = b2OffsetPos( point, b2MulSV( 0.2f, h->normal ) );+						draw->DrawLineFcn( point, np, b2_colorLightSkyBlue, draw->context );+					}+				}+				break;+			}+			case B2_RECQ_CAST_MOVER:+			{+				// The mover capsule is relative to the query origin+				if ( draw->DrawSolidCapsuleFcn )+				{+					b2Pos c1 = b2OffsetPos( q->origin, q->mover.center1 );+					b2Pos c2 = b2OffsetPos( q->origin, q->mover.center2 );+					draw->DrawSolidCapsuleFcn( c1, c2, q->mover.radius, b2_colorLightSkyBlue, draw->context );+				}+				break;+			}+			case B2_RECQ_OVERLAP_AABB:+			{+				// The query box is relative to the query origin+				b2Vec2 lower = q->aabb.lowerBound;+				b2Vec2 upper = q->aabb.upperBound;+				b2Vec2 vs[4] = { lower, { upper.x, lower.y }, upper, { lower.x, upper.y } };+				if ( draw->DrawPolygonFcn )+				{+					draw->DrawPolygonFcn( (b2WorldTransform){ q->origin, b2Rot_identity }, vs, 4, b2_colorLimeGreen,+										  draw->context );+				}+				b2RecDrawHitAABBs( player, q, draw );+				break;+			}+			case B2_RECQ_OVERLAP_SHAPE:+			{+				// The proxy points are relative to the query origin+				if ( q->proxy.count == 1 )+				{+					if ( draw->DrawCircleFcn )+					{+						draw->DrawCircleFcn( b2OffsetPos( q->origin, q->proxy.points[0] ), q->proxy.radius, b2_colorLimeGreen,+											 draw->context );+					}+				}+				else if ( q->proxy.count >= 2 && draw->DrawPolygonFcn )+				{+					draw->DrawPolygonFcn( (b2WorldTransform){ q->origin, b2Rot_identity }, q->proxy.points, q->proxy.count,+										  b2_colorLimeGreen, draw->context );+				}+				b2RecDrawHitAABBs( player, q, draw );+				break;+			}+			case B2_RECQ_COLLIDE_MOVER:+			{+				// The mover capsule and the collision planes are relative to the query origin+				if ( draw->DrawSolidCapsuleFcn )+				{+					b2Pos c1 = b2OffsetPos( q->origin, q->mover.center1 );+					b2Pos c2 = b2OffsetPos( q->origin, q->mover.center2 );+					draw->DrawSolidCapsuleFcn( c1, c2, q->mover.radius, b2_colorTan, draw->context );+				}+				// Per-hit plane point and normal+				for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi )+				{+					const b2RecRecordedHit* h = &player->frameHits[hi];+					if ( h->plane.hit && draw->DrawLineFcn )+					{+						b2Pos point = b2OffsetPos( q->origin, h->plane.point );+						b2Pos np = b2OffsetPos( point, b2MulSV( 0.2f, h->plane.plane.normal ) );+						draw->DrawLineFcn( point, np, b2_colorOrange, draw->context );+					}+				}+				break;+			}+			default:+				break;+		}+	}+}++// Public query inspection. The internal b2RecQueryKind values match the public b2ReplayQueryType, so+// the kind copies across as a plain cast. Pin the first and last kinds to catch enum drift.+_Static_assert( b2_replayQueryOverlapAABB == 0 && B2_RECQ_OVERLAP_AABB == 0, "query type enum drift" );+_Static_assert( b2_replayQueryCastMover == 6 && B2_RECQ_CAST_MOVER == 6, "query type enum drift" );++int b2Replay_GetFrameQueryCount( const b2Replay* player )+{+	return player != NULL ? player->frameQueryCount : 0;+}++b2ReplayQueryInfo b2Replay_GetFrameQuery( const b2Replay* player, int index )+{+	b2ReplayQueryInfo info = { 0 };+	if ( player == NULL || index < 0 || index >= player->frameQueryCount )+	{+		return info;+	}++	const b2RecDrawQuery* q = &player->frameQueries[index];+	info.type = (b2ReplayQueryType)q->kind;+	info.filter = q->filter;+	info.aabb = q->aabb;+	info.origin = q->origin;+	info.translation = q->translation;+	info.hitCount = q->hitCount;+	return info;+}++b2ReplayQueryHit b2Replay_GetFrameQueryHit( const b2Replay* player, int queryIndex, int hitIndex )+{+	b2ReplayQueryHit hit = { 0 };+	if ( player == NULL || queryIndex < 0 || queryIndex >= player->frameQueryCount )+	{+		return hit;+	}++	const b2RecDrawQuery* q = &player->frameQueries[queryIndex];+	if ( hitIndex < 0 || hitIndex >= q->hitCount )+	{+		return hit;+	}++	const b2RecRecordedHit* h = &player->frameHits[q->hitStart + hitIndex];+	hit.shapeId = h->id;+	hit.point = h->point;+	hit.normal = h->normal;+	hit.fraction = h->fraction;+	return hit;+}++int b2Replay_GetBodyCount( const b2Replay* player )+{+	return player != NULL ? player->bodyIdCount : 0;+}++b2BodyId b2Replay_GetBodyId( const b2Replay* player, int index )+{+	if ( player == NULL || index < 0 || index >= player->bodyIdCount )+	{+		return b2_nullBodyId;+	}+	return player->bodyIds[index];+}++bool b2ValidateRecording( const void* data, int size, int workerCount )+{+	b2Replay* player = b2CreateReplay( data, size, workerCount );+	if ( player == NULL )+	{+		return false;+	}++	while ( b2Replay_StepFrame( player ) )+	{+		if ( player->rdr.diverged )+		{+			break;+		}+	}++	bool ok = player->rdr.ok && player->rdr.diverged == false;+	b2DestroyReplay( player );+	return ok;+}
+ upstream/src/replay.h view
@@ -0,0 +1,195 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "recording.h"++#include <stdbool.h>+#include <stdint.h>++typedef struct b2Replay b2Replay;++// A single recorded callback hit, used both as reader scratch and as the per-frame draw store+typedef struct b2RecRecordedHit+{+	b2ShapeId id;+	b2Pos point;+	b2Vec2 normal;+	float fraction;+	b2PlaneResult plane;+	float userReturnF;+	bool userReturnB;+} b2RecRecordedHit;++// Per-frame draw record for one query call+typedef enum b2RecQueryKind+{+	B2_RECQ_OVERLAP_AABB,+	B2_RECQ_OVERLAP_SHAPE,+	B2_RECQ_CAST_RAY,+	B2_RECQ_CAST_SHAPE,+	B2_RECQ_COLLIDE_MOVER,+	B2_RECQ_CAST_RAY_CLOSEST,+	B2_RECQ_CAST_MOVER+} b2RecQueryKind;++typedef struct b2RecDrawQuery+{+	int kind;+	b2QueryFilter filter;+	b2AABB aabb;+	b2ShapeProxy proxy;+	b2Capsule mover;+	b2Pos origin;+	b2Vec2 translation;+	float castFraction;+	int hitStart;+	int hitCount;+} b2RecDrawQuery;++// Reader state threaded through the replay loop and all dispatch functions+typedef struct b2RecReader+{+	const uint8_t* data;+	int size;+	int cursor;+	b2WorldId replayWorldId; // world created during replay; valid after CreateWorld record+	int workerCount;		 // 0 = use recorded count+	bool ok;				 // false on read overrun or id mismatch, a fatal stop+	bool diverged;			 // a StateHash failed to reproduce, non-fatal so a viewer can keep playing++	// Scratch for variable-length defs (chain points/materials), grown on demand and+	// freed with the player. Cloned by the create call so only valid during one dispatch.+	b2Vec2* chainPoints;+	int chainPointCap;+	b2SurfaceMaterial* chainMaterials;+	int chainMaterialCap;++	// Scratch for recorded query hits; grown on demand, freed with the player+	b2RecRecordedHit* hits;+	int hitCap;++	b2Replay* owner; // player that owns this reader+} b2RecReader;++// A restore point captured during forward replay, so a backward seek can re-simulate only the gap+// from the nearest keyframe instead of from frame 0. The image is a full b2SerializeWorld blob.+typedef struct b2RecKeyframe+{+	uint8_t* image; // serialized world at the end of this frame+	int imageSize;+	int imageCapacity; // allocation size of image, which over-allocates, so the free size matches+	int frame;		   // frame this restores to, a post-step boundary+	int cursor;		   // op-stream offset where the next frame resumes+	b2BodyId* bodyIds; // outliner list as of this frame+	int bodyIdCount;+	int divergeFrame; // divergence latches as of this frame, so a seek reports the linear path's state+	bool diverged;+} b2RecKeyframe;++// Incremental player. Owns a private copy of the recording bytes and drives replay one step at a time.+struct b2Replay+{+	uint8_t* data; // recording bytes, a private copy owned here+	int size;+	int headerEnd;			   // first payload offset+	float lengthScale;		   // length scale used in the recording+	float previousLengthScale; // global length scale before this player overrode it, restored on destroy+	int frame;				   // steps dispatched so far+	int frameCount;			   // total recorded steps, counted once at open+	int recordedWorkerCount;   // worker count the replay world runs at+	float recordedDt;		   // dt of the first recorded step+	int recordedSubStepCount;  // sub-steps of the first recorded step+	b2AABB bounds;			   // accumulated world bounds, resolved by the open-time scan+	int divergeFrame;		   // first step that diverged, -1 until then+	bool atEnd;				   // a StepFrame ran out of records without reaching a step+	b2RecReader rdr;		   // cursor and replay world, threaded into every dispatcher++	// Per-frame query store, reset at the top of each StepFrame+	b2RecDrawQuery* frameQueries;+	int frameQueryCount;+	int frameQueryCap;+	b2RecRecordedHit* frameHits;+	int frameHitCount;+	int frameHitCap;++	// Live bodies in creation order, tracked from create/destroy ops to drive the viewer outliner.+	// Destroyed slots hold b2_nullBodyId so ordinals stay stable. Rebuilt deterministically on replay.+	b2BodyId* bodyIds;+	int bodyIdCount;+	int bodyIdCap;++	// Frame-0 image used to restart in place, so the replay world id stays stable across a+	// restart or backward scrub. Points into the seed snapshot blob inside the owned copy.+	const uint8_t* frame0Image;+	int frame0Size;+	b2BodyId* frame0BodyIds;+	int frame0BodyIdCount;++	// Keyframe ring for fast backward seeks. Captured in increasing-frame order as the replay plays+	// forward. The spacing doubles and the off-grid keyframes are evicted once the memory budget is+	// hit, so memory stays bounded and seek cost grows only once a recording outgrows the budget.+	b2RecKeyframe* keyframes;+	int keyframeCount;+	int keyframeCapacity;+	size_t keyframeBudget;	 // memory cap in bytes for the kept snapshots+	size_t keyframeBytes;	 // running total of kept snapshot + body-list bytes+	int keyframeMinInterval; // finest spacing in frames+	int keyframeInterval;	 // current spacing, a power-of-two multiple of the min, doubles on eviction+	int lastKeyframeFrame;	 // highest frame captured, guards against re-capture while back-stepping+};++// Read primitives++uint8_t b2RecR_U8( b2RecReader* rdr );+uint16_t b2RecR_U16( b2RecReader* rdr );+uint32_t b2RecR_U24( b2RecReader* rdr );+uint32_t b2RecR_U32( b2RecReader* rdr );+uint64_t b2RecR_U64( b2RecReader* rdr );+int32_t b2RecR_I32( b2RecReader* rdr );+float b2RecR_F32( b2RecReader* rdr );+bool b2RecR_BOOL( b2RecReader* rdr );+b2Vec2 b2RecR_VEC2( b2RecReader* rdr );+b2Rot b2RecR_ROT( b2RecReader* rdr );+b2Transform b2RecR_XF( b2RecReader* rdr );+double b2RecR_F64( b2RecReader* rdr );+b2Pos b2RecR_POSITION( b2RecReader* rdr );+b2WorldTransform b2RecR_WORLDXF( b2RecReader* rdr );+b2WorldId b2RecR_WORLDID( b2RecReader* rdr );+b2BodyId b2RecR_BODYID( b2RecReader* rdr );+b2ShapeId b2RecR_SHAPEID( b2RecReader* rdr );+b2ChainId b2RecR_CHAINID( b2RecReader* rdr );+b2JointId b2RecR_JOINTID( b2RecReader* rdr );+b2Circle b2RecR_CIRCLE( b2RecReader* rdr );+b2Capsule b2RecR_CAPSULE( b2RecReader* rdr );+b2Segment b2RecR_SEGMENT( b2RecReader* rdr );+b2Polygon b2RecR_POLYGON( b2RecReader* rdr );+b2ChainSegment b2RecR_CHAINSEG( b2RecReader* rdr );+b2Filter b2RecR_FILTER( b2RecReader* rdr );+b2SurfaceMaterial b2RecR_MATERIAL( b2RecReader* rdr );+b2MassData b2RecR_MASSDATA( b2RecReader* rdr );+b2MotionLocks b2RecR_LOCKS( b2RecReader* rdr );+const char* b2RecR_STR( b2RecReader* rdr );+b2ExplosionDef b2RecR_EXPLOSIONDEF( b2RecReader* rdr );+b2BodyDef b2RecR_BODYDEF( b2RecReader* rdr );+b2ShapeDef b2RecR_SHAPEDEF( b2RecReader* rdr );+b2ChainDef b2RecR_CHAINDEF( b2RecReader* rdr );+b2DistanceJointDef b2RecR_DISTANCEJOINTDEF( b2RecReader* rdr );+b2FilterJointDef b2RecR_FILTERJOINTDEF( b2RecReader* rdr );+b2MotorJointDef b2RecR_MOTORJOINTDEF( b2RecReader* rdr );+b2MoverJointDef b2RecR_MOVERJOINTDEF( b2RecReader* rdr );+b2PogoJointDef b2RecR_POGOJOINTDEF( b2RecReader* rdr );+b2PrismaticJointDef b2RecR_PRISMATICJOINTDEF( b2RecReader* rdr );+b2RevoluteJointDef b2RecR_REVOLUTEJOINTDEF( b2RecReader* rdr );+b2WeldJointDef b2RecR_WELDJOINTDEF( b2RecReader* rdr );+b2WheelJointDef b2RecR_WHEELJOINTDEF( b2RecReader* rdr );+b2AABB b2RecR_AABB( b2RecReader* rdr );+b2QueryFilter b2RecR_QUERYFILTER( b2RecReader* rdr );+b2ShapeProxy b2RecR_SHAPEPROXY( b2RecReader* rdr );+b2RayResult b2RecR_RAYRESULT( b2RecReader* rdr );+b2PlaneResult b2RecR_PLANERESULT( b2RecReader* rdr );+b2TreeStats b2RecR_TREESTATS( b2RecReader* rdr );++// Grow the reader's hit scratch to at least n entries+void b2RecEnsureHits( b2RecReader* rdr, int n );
+ upstream/src/revolute_joint.c view
@@ -0,0 +1,594 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS )+#define _CRT_SECURE_NO_WARNINGS+#endif++#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 "box2d/box2d.h"++#include <stdio.h>++// Point-to-point constraint+// C = pB - pA+// Cdot = vB - vA+//      = vB + cross(wB, rB) - vA - cross(wA, rA)+// J = [-E -skew(rA) E skew(rB) ]++// Identity used:+// w k % (rx i + ry j) = w * (-ry i + rx j)++// Motor constraint+// Cdot = wB - wA+// J = [0 0 -1 0 0 1]+// K = invIA + invIB++void b2RevoluteJoint_EnableSpring( b2JointId jointId, bool enableSpring )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, RevoluteJointEnableSpring, jointId, enableSpring );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	if ( enableSpring != joint->revoluteJoint.enableSpring )+	{+		joint->revoluteJoint.enableSpring = enableSpring;+		joint->revoluteJoint.springImpulse = 0.0f;+	}+}++bool b2RevoluteJoint_IsSpringEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.enableSpring;+}++void b2RevoluteJoint_SetSpringHertz( b2JointId jointId, float hertz )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, RevoluteJointSetSpringHertz, jointId, hertz );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	joint->revoluteJoint.hertz = hertz;+}++float b2RevoluteJoint_GetSpringHertz( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.hertz;+}++void b2RevoluteJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )+{+	B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, RevoluteJointSetSpringDampingRatio, jointId, dampingRatio );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	joint->revoluteJoint.dampingRatio = dampingRatio;+}++float b2RevoluteJoint_GetSpringDampingRatio( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.dampingRatio;+}++void b2RevoluteJoint_SetTargetAngle( b2JointId jointId, float angle )+{+	B2_CHECK_INPUT( b2IsValidFloat( angle ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, RevoluteJointSetTargetAngle, jointId, angle );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	joint->revoluteJoint.targetAngle = angle;+}++float b2RevoluteJoint_GetTargetAngle( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.targetAngle;+}++void b2RevoluteJoint_EnableLimit( b2JointId jointId, bool enableLimit )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, RevoluteJointEnableLimit, jointId, enableLimit );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	if ( enableLimit != joint->revoluteJoint.enableLimit )+	{+		joint->revoluteJoint.enableLimit = enableLimit;+		joint->revoluteJoint.lowerImpulse = 0.0f;+		joint->revoluteJoint.upperImpulse = 0.0f;+	}+}++bool b2RevoluteJoint_IsLimitEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.enableLimit;+}++float b2RevoluteJoint_GetLowerLimit( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.lowerAngle;+}++float b2RevoluteJoint_GetUpperLimit( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.upperAngle;+}++void b2RevoluteJoint_SetLimits( b2JointId jointId, float lower, float upper )+{+	B2_CHECK_INPUT( b2IsValidFloat( lower ) );+	B2_CHECK_INPUT( b2IsValidFloat( upper ) );++	B2_ASSERT( lower <= upper );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, RevoluteJointSetLimits, jointId, lower, upper );++	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	float lowerAngle = b2MinFloat( lower, upper );+	float upperAngle = b2MaxFloat( lower, upper );+	joint->revoluteJoint.lowerAngle = b2ClampFloat( lowerAngle, -0.99f * B2_PI, 0.99f * B2_PI );+	joint->revoluteJoint.upperAngle = b2ClampFloat( upperAngle, -0.99f * B2_PI, 0.99f * B2_PI );+}++void b2RevoluteJoint_EnableMotor( b2JointId jointId, bool enableMotor )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, RevoluteJointEnableMotor, jointId, enableMotor );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	if ( enableMotor != joint->revoluteJoint.enableMotor )+	{+		joint->revoluteJoint.enableMotor = enableMotor;+		joint->revoluteJoint.motorImpulse = 0.0f;+	}+}++bool b2RevoluteJoint_IsMotorEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.enableMotor;+}++void b2RevoluteJoint_SetTargetMotorSpeed( b2JointId jointId, float motorSpeed )+{+	B2_CHECK_INPUT( b2IsValidFloat( motorSpeed ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, RevoluteJointSetTargetMotorSpeed, jointId, motorSpeed );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	joint->revoluteJoint.motorSpeed = motorSpeed;+}++float b2RevoluteJoint_GetTargetMotorSpeed( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.motorSpeed;+}++void b2RevoluteJoint_SetMaxMotorTorque( b2JointId jointId, float torque )+{+	B2_CHECK_INPUT( b2IsValidFloat( torque ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, RevoluteJointSetMaxMotorTorque, jointId, torque );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	joint->revoluteJoint.maxMotorTorque = torque;+}++float b2RevoluteJoint_GetMaxMotorTorque( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return joint->revoluteJoint.maxMotorTorque;+}++float b2RevoluteJoint_GetCurrentAngle( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	b2Rot qA = b2MulRot( b2GetBodyTransform( world, jointSim->bodyIdA ).q, jointSim->localFrameA.q );+	b2Rot qB = b2MulRot( b2GetBodyTransform( world, jointSim->bodyIdB ).q, jointSim->localFrameB.q );++	float angle = b2RelativeAngle( qA, qB );+	return angle;+}++float b2RevoluteJoint_GetCurrentSpeed( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_revoluteJoint );++	b2Body* bodyA = b2Array_Get( world->bodies, base->bodyIdA );+	b2Body* bodyB = b2Array_Get( world->bodies, base->bodyIdB );+	b2BodyState* bodyStateA = b2GetBodyState( world, bodyA );+	b2BodyState* bodyStateB = b2GetBodyState( world, bodyB );++	float wA = bodyStateA ? bodyStateA->angularVelocity : 0.0f;+	float wB = bodyStateB ? bodyStateB->angularVelocity : 0.0f;++	return wB - wA;+}++float b2RevoluteJoint_GetCurrentMotorTorque( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );+	return world->inv_h * joint->revoluteJoint.motorImpulse;+}++b2Vec2 b2GetRevoluteJointForce( b2World* world, b2JointSim* base )+{+	b2Vec2 force = b2MulSV( world->inv_h, base->revoluteJoint.linearImpulse );+	return force;+}++float b2GetRevoluteJointTorque( b2World* world, b2JointSim* base )+{+	const b2RevoluteJoint* revolute = &base->revoluteJoint;+	float torque = world->inv_h * ( revolute->motorImpulse + revolute->lowerImpulse - revolute->upperImpulse );+	return torque;+}++void b2PrepareRevoluteJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_revoluteJoint );++	// chase body id to the solver set where the body lives+	int idA = base->bodyIdA;+	int idB = base->bodyIdB;++	b2World* world = context->world;++	b2Body* bodyA = b2Array_Get( world->bodies, idA );+	b2Body* bodyB = b2Array_Get( world->bodies, idB );++	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );+	b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex );+	b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex );++	int localIndexA = bodyA->localIndex;+	int localIndexB = bodyB->localIndex;++	b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA );+	b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB );++	float mA = bodySimA->invMass;+	float iA = bodySimA->invInertia;+	float mB = bodySimB->invMass;+	float iB = bodySimB->invInertia;++	base->invMassA = mA;+	base->invMassB = mB;+	base->invIA = iA;+	base->invIB = iB;++	b2RevoluteJoint* joint = &base->revoluteJoint;++	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;++	// Compute joint anchor frames with world space rotation, relative to center of mass.+	// Avoid round-off here as much as possible.+	// b2Vec2 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 = b2MulRot( bodySimA->transform.q, base->localFrameA.q );+	joint->frameA.p = b2RotateVector( bodySimA->transform.q, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );+	joint->frameB.q = b2MulRot( bodySimB->transform.q, base->localFrameB.q );+	joint->frameB.p = b2RotateVector( bodySimB->transform.q, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );++	// Compute the initial center delta. Incremental position updates are relative to this.+	joint->deltaCenter = b2SubPos( bodySimB->center, bodySimA->center );++	float k = iA + iB;+	joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f;++	joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );++	if ( context->enableWarmStarting == false )+	{+		joint->linearImpulse = b2Vec2_zero;+		joint->springImpulse = 0.0f;+		joint->motorImpulse = 0.0f;+		joint->lowerImpulse = 0.0f;+		joint->upperImpulse = 0.0f;+	}+}++void b2WarmStartRevoluteJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_revoluteJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2RevoluteJoint* joint = &base->revoluteJoint;+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearImpulse );+		stateA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + axialImpulse );+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearImpulse );+		stateB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + axialImpulse );+	}+}++void b2SolveRevoluteJoint( b2JointSim* base, b2StepContext* context, bool useBias )+{+	B2_ASSERT( base->type == b2_revoluteJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2RevoluteJoint* joint = &base->revoluteJoint;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 vA = stateA->linearVelocity;+	float wA = stateA->angularVelocity;+	b2Vec2 vB = stateB->linearVelocity;+	float wB = stateB->angularVelocity;++	b2Rot qA = b2MulRot( stateA->deltaRotation, joint->frameA.q );+	b2Rot qB = b2MulRot( stateB->deltaRotation, joint->frameB.q );+	b2Rot relQ = b2InvMulRot( qA, qB );++	// current anchors+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	b2Vec2 linearCdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );+	float angularCdot = wB - wA;+	float axialImpulse = 0.0f;++	float k = iA + iB;+	bool fixedRotation = ( k == 0.0f );++	// Solve spring.+	if ( joint->enableSpring && fixedRotation == false )+	{+		float jointAngle = b2Rot_GetAngle( relQ );+		float jointAngleDelta = b2UnwindAngle( jointAngle - joint->targetAngle );++		float C = jointAngleDelta;+		float bias = joint->springSoftness.biasRate * C;+		float massScale = joint->springSoftness.massScale;+		float impulseScale = joint->springSoftness.impulseScale;++		float Cdot = angularCdot;+		float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse;+		joint->springImpulse += impulse;++		angularCdot += k * impulse;+		axialImpulse += impulse;+	}++	// Solve motor constraint.+	if ( joint->enableMotor && fixedRotation == false )+	{+		float Cdot = angularCdot - joint->motorSpeed;+		float oldImpulse = joint->motorImpulse;+		float maxImpulse = context->h * joint->maxMotorTorque;+		float impulse = b2ClampFloat( -joint->axialMass * Cdot, -maxImpulse - oldImpulse, maxImpulse - oldImpulse );+		joint->motorImpulse = oldImpulse + impulse;++		angularCdot += k * impulse;+		axialImpulse += impulse;+	}++	if ( joint->enableLimit && fixedRotation == false )+	{+		float jointAngle = b2Rot_GetAngle( relQ );++		// Lower limit+		{+			float C = jointAngle - 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 = angularCdot;+			float oldImpulse = joint->lowerImpulse;+			float impulse = b2MaxFloat( -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * oldImpulse, -oldImpulse );+			joint->lowerImpulse = oldImpulse + impulse;++			angularCdot += k * impulse;+			axialImpulse += impulse;+		}++		// 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.+		{+			float C = joint->upperAngle - jointAngle;+			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 = -angularCdot;+			float oldImpulse = joint->upperImpulse;+			float impulse = b2MaxFloat( -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * oldImpulse, -oldImpulse );+			joint->upperImpulse = oldImpulse + impulse;++			// sign flipped on applied impulse+			axialImpulse -= impulse;+		}+	}++	// Solve point-to-point constraint+	{+		// J = [-I -r1_skew I r2_skew]+		// r_skew = [-ry; rx]+		// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x]+		//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB]++		b2Vec2 Cdot = b2MulAdd( linearCdot, axialImpulse, b2Add( b2CrossSV( iA, rA ), b2CrossSV( iB, rB ) ) );++		b2Vec2 bias = b2Vec2_zero;+		float massScale = 1.0f;+		float impulseScale = 0.0f;+		if ( useBias )+		{+			b2Vec2 dcA = stateA->deltaPosition;+			b2Vec2 dcB = stateB->deltaPosition;++			b2Vec2 separation = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter );+			bias = b2MulSV( base->constraintSoftness.biasRate, separation );+			massScale = base->constraintSoftness.massScale;+			impulseScale = base->constraintSoftness.impulseScale;+		}++		b2Mat22 K;+		K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;+		K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;+		K.cx.y = K.cy.x;+		K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;+		b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) );++		b2Vec2 impulse;+		impulse.x = -massScale * b.x - impulseScale * joint->linearImpulse.x;+		impulse.y = -massScale * b.y - impulseScale * joint->linearImpulse.y;+		joint->linearImpulse.x += impulse.x;+		joint->linearImpulse.y += impulse.y;++		vA = b2MulSub( vA, mA, impulse );+		wA -= iA * ( b2Cross( rA, impulse ) + axialImpulse );+		vB = b2MulAdd( vB, mB, impulse );+		wB += iB * ( b2Cross( rB, impulse ) + axialImpulse );+	}++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = vA;+		stateA->angularVelocity = wA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = vB;+		stateB->angularVelocity = wB;+	}+}++void b2DrawRevoluteJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB,+						  float drawScale )+{+	B2_ASSERT( base->type == b2_revoluteJoint );++	b2RevoluteJoint* joint = &base->revoluteJoint;++	b2WorldTransform frameA = b2MulWorldTransforms( transformA, base->localFrameA );+	b2WorldTransform frameB = b2MulWorldTransforms( transformB, base->localFrameB );++	const float radius = 0.25f * drawScale;+	draw->DrawCircleFcn( frameB.p, radius, b2_colorGray, draw->context );++	b2Vec2 rx = { radius, 0.0f };+	b2Vec2 r = b2RotateVector( frameA.q, rx );+	draw->DrawLineFcn( frameA.p, b2OffsetPos( frameA.p, r ), b2_colorGray, draw->context );++	r = b2RotateVector( frameB.q, rx );+	draw->DrawLineFcn( frameB.p, b2OffsetPos( frameB.p, r ), b2_colorBlue, draw->context );++	if ( draw->drawJointExtras )+	{+		float jointAngle = b2RelativeAngle( frameA.q, frameB.q );+		char buffer[32];+		snprintf( buffer, 32, " %.1f deg", 180.0f * jointAngle / B2_PI );+		draw->DrawStringFcn( b2OffsetPos( frameA.p, r ), buffer, b2_colorWhite, draw->context );+	}++	float lowerAngle = joint->lowerAngle;+	float upperAngle = joint->upperAngle;++	if ( joint->enableLimit )+	{+		b2Rot rotLo = b2MulRot( frameA.q, b2MakeRot( lowerAngle ) );+		b2Vec2 rlo = b2RotateVector( rotLo, rx );++		b2Rot rotHi = b2MulRot( frameA.q, b2MakeRot( upperAngle ) );+		b2Vec2 rhi = b2RotateVector( rotHi, rx );++		draw->DrawLineFcn( frameB.p, b2OffsetPos( frameB.p, rlo ), b2_colorGreen, draw->context );+		draw->DrawLineFcn( frameB.p, b2OffsetPos( frameB.p, rhi ), b2_colorRed, draw->context );+	}++	if ( joint->enableSpring )+	{+		b2Rot q = b2MulRot( frameA.q, b2MakeRot( joint->targetAngle ) );+		b2Vec2 v = b2RotateVector( q, rx );+		draw->DrawLineFcn( frameB.p, b2OffsetPos( frameB.p, v ), b2_colorViolet, draw->context );+	}++	b2HexColor color = b2_colorGold;+	draw->DrawLineFcn( frameA.p, frameB.p, color, draw->context );++	if ( draw->drawJointExtras )+	{+		draw->DrawLineFcn( transformA.p, frameA.p, color, draw->context );+		draw->DrawLineFcn( transformB.p, frameB.p, color, draw->context );+	}++	// char buffer[32];+	// sprintf(buffer, "%.1f", b2Length(joint->impulse));+	// draw->DrawString(pA, buffer, draw->context);+}
+ upstream/src/scheduler.c view
@@ -0,0 +1,269 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "scheduler.h"++#include "core.h"+#include "platform.h"++#include "box2d/base.h"+#include "box2d/constants.h"++#include <stdio.h>+#include <string.h>++#define B2_SCHEDULER_SPIN_MILLISECONDS 0.05f++enum b2SchedulerTaskStatus+{+	b2_schedulerFree = 0,+	b2_schedulerPending = 1,+	b2_schedulerClaimed = 2,+	b2_schedulerComplete = 3,+};++typedef struct b2SchedulerTask+{+	b2TaskCallbackFcn* callback;+	void* taskContext;+	b2AtomicInt status;+} b2SchedulerTask;++typedef struct b2SchedulerWorkerContext+{+	struct b2Scheduler* scheduler;+	int threadIndex;+} b2SchedulerWorkerContext;++typedef struct b2Scheduler+{+	b2Thread* threads[B2_MAX_WORKERS];+	b2SchedulerWorkerContext workerContexts[B2_MAX_WORKERS];++	// total workers including main thread+	int workerCount;++	// threads created = workerCount - 1+	int threadCount;++	b2SchedulerTask tasks[B2_MAX_TASKS];+	b2AtomicInt nextSlot;++	b2Semaphore* taskSemaphore;+	b2AtomicInt sleeperCount;+	b2AtomicInt shutdown;+} b2Scheduler;++static bool b2SchedulerHasPending( b2Scheduler* scheduler )+{+	int taskCount = b2AtomicLoadInt( &scheduler->nextSlot );+	for ( int t = 0; t < taskCount; ++t )+	{+		if ( b2AtomicLoadInt( &scheduler->tasks[t].status ) == b2_schedulerPending )+		{+			return true;+		}+	}++	return false;+}++static bool b2SchedulerSpin( b2Scheduler* scheduler )+{+	uint64_t ticks = b2GetTicks();+	while ( true )+	{+		if ( b2SchedulerHasPending( scheduler ) || b2AtomicLoadInt( &scheduler->shutdown ) != 0 )+		{+			return true;+		}++		if ( b2GetMilliseconds( ticks ) >= B2_SCHEDULER_SPIN_MILLISECONDS )+		{+			return false;+		}++		for ( int i = 0; i < 8; ++i )+		{+			b2Pause();+		}+	}+}++static bool b2SchedulerTakeSleeper( b2Scheduler* scheduler )+{+	int sleeperCount = b2AtomicLoadInt( &scheduler->sleeperCount );+	while ( sleeperCount > 0 )+	{+		if ( b2AtomicCompareExchangeInt( &scheduler->sleeperCount, sleeperCount, sleeperCount - 1 ) )+		{+			return true;+		}++		sleeperCount = b2AtomicLoadInt( &scheduler->sleeperCount );+	}++	return false;+}++// Try to claim and execute one pending task.+// Returns true if work was performed, false otherwise.+static bool b2SchedulerExecuteOne( b2Scheduler* scheduler )+{+	int taskCount = b2AtomicLoadInt( &scheduler->nextSlot );+	for ( int t = 0; t < taskCount; ++t )+	{+		b2SchedulerTask* task = scheduler->tasks + t;+		if ( b2AtomicLoadInt( &task->status ) != b2_schedulerPending )+		{+			continue;+		}++		if ( b2AtomicCompareExchangeInt( &task->status, b2_schedulerPending, b2_schedulerClaimed ) == false )+		{+			continue;+		}++		task->callback( task->taskContext );++		b2AtomicStoreInt( &task->status, b2_schedulerComplete );+		return true;+	}++	return false;+}++// Background worker thread entry point.+static void b2SchedulerWorkerMain( void* context )+{+	b2SchedulerWorkerContext* workerContext = context;+	b2Scheduler* scheduler = workerContext->scheduler;++	while ( true )+	{+		// Claim and execute all available work+		while ( b2SchedulerExecuteOne( scheduler ) )+		{+		}++		if ( b2AtomicLoadInt( &scheduler->shutdown ) != 0 )+		{+			break;+		}++		if ( b2SchedulerSpin( scheduler ) )+		{+			continue;+		}++		b2AtomicFetchAddInt( &scheduler->sleeperCount, 1 );++		if ( b2SchedulerHasPending( scheduler ) || b2AtomicLoadInt( &scheduler->shutdown ) != 0 )+		{+			b2SchedulerTakeSleeper( scheduler );+			continue;+		}++		b2WaitSemaphore( scheduler->taskSemaphore );+	}+}++b2Scheduler* b2CreateScheduler( int workerCount )+{+	B2_ASSERT( 0 < workerCount && workerCount <= B2_MAX_WORKERS );++	b2Scheduler* scheduler = b2Alloc( sizeof( b2Scheduler ) );+	memset( scheduler, 0, sizeof( b2Scheduler ) );++	scheduler->workerCount = workerCount;+	int threadCount = workerCount - 1;+	scheduler->threadCount = threadCount;+	scheduler->taskSemaphore = b2CreateSemaphore( 0 );+	b2AtomicStoreInt( &scheduler->sleeperCount, 0 );+	b2AtomicStoreInt( &scheduler->shutdown, 0 );+	b2AtomicStoreInt( &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] = b2CreateThread( b2SchedulerWorkerMain, scheduler->workerContexts + i, name );+	}++	return scheduler;+}++void b2DestroyScheduler( b2Scheduler* scheduler )+{+	b2AtomicStoreInt( &scheduler->shutdown, 1 );++	// Wake all background threads so they see the shutdown flag+	for ( int i = 0; i < scheduler->threadCount; ++i )+	{+		b2SignalSemaphore( scheduler->taskSemaphore );+	}++	for ( int i = 0; i < scheduler->threadCount; ++i )+	{+		b2JoinThread( scheduler->threads[i] );+		scheduler->threads[i] = NULL;+	}++	b2DestroySemaphore( scheduler->taskSemaphore );+	b2Free( scheduler, sizeof( b2Scheduler ) );+}++void b2ResetScheduler( b2Scheduler* scheduler )+{+	b2AtomicStoreInt( &scheduler->nextSlot, 0 );+}++void* b2SchedulerEnqueueTask( b2TaskCallbackFcn* task, void* taskContext, void* userContext )+{+	b2Scheduler* scheduler = userContext;++	int slot = b2AtomicFetchAddInt( &scheduler->nextSlot, 1 );+	B2_ASSERT( slot < B2_MAX_TASKS );++	b2SchedulerTask* schedulerTask = scheduler->tasks + slot;+	schedulerTask->callback = task;+	schedulerTask->taskContext = taskContext;++	// Memory fence: status must be published after callback and context are written+	b2AtomicStoreInt( &schedulerTask->status, b2_schedulerPending );++	// One wake per enqueue is enough: at most one worker picks up each task.+	if ( b2SchedulerTakeSleeper( scheduler ) )+	{+		b2SignalSemaphore( scheduler->taskSemaphore );+	}++	return schedulerTask;+}++void b2SchedulerFinishTask( void* userTask, void* userContext )+{+	if ( userTask == NULL )+	{+		return;+	}++	b2Scheduler* scheduler = userContext;+	b2SchedulerTask* 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 ( b2AtomicLoadInt( &waitTask->status ) != b2_schedulerComplete )+	{+		if ( b2SchedulerExecuteOne( scheduler ) == false )+		{+			b2Yield();+		}+	}+}
+ upstream/src/scheduler.h view
@@ -0,0 +1,15 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++typedef void b2TaskCallbackFcn( void* taskContext );+typedef struct b2Scheduler b2Scheduler;++b2Scheduler* b2CreateScheduler( int workerCount );+void b2DestroyScheduler( b2Scheduler* scheduler );+void b2ResetScheduler( b2Scheduler* scheduler );++// See b2EnqueueTaskCallbackFcn and b2FinishTaskCallbackFcn+void* b2SchedulerEnqueueTask( b2TaskCallbackFcn* task, void* taskContext, void* userContext );+void b2SchedulerFinishTask( void* userTask, void* userContext );
+ upstream/src/sensor.c view
@@ -0,0 +1,409 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "sensor.h"++#include "body.h"+#include "contact.h"+#include "platform.h"+#include "parallel_for.h"+#include "physics_world.h"+#include "shape.h"+#include "solver_set.h"++#include "box2d/collision.h"++#include <stddef.h>+#include <stdlib.h>++struct b2SensorQueryContext+{+	b2World* world;+	b2SensorTaskContext* taskContext;+	b2Sensor* sensor;+	b2Shape* sensorShape;+	b2WorldTransform transform;+};++// 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 b2SensorQueryCallback( int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;++	struct b2SensorQueryContext* queryContext = context;+	b2Shape* sensorShape = queryContext->sensorShape;+	int sensorShapeId = sensorShape->id;++	if ( shapeId == sensorShapeId )+	{+		return true;+	}++	b2World* world = queryContext->world;+	b2Shape* otherShape = b2Array_Get( world->shapes, shapeId );++	// Are sensor events enabled on the other shape?+	if ( otherShape->enableSensorEvents == false )+	{+		return true;+	}++	// Skip shapes on the same body+	if ( otherShape->bodyId == sensorShape->bodyId )+	{+		return true;+	}++	// Check filter+	if ( b2ShouldShapesCollide( sensorShape->filter, otherShape->filter ) == false )+	{+		return true;+	}++	// Custom user filter+	if ( sensorShape->enableCustomFiltering || otherShape->enableCustomFiltering )+	{+		b2CustomFilterCallbackFcn* customFilterFcn = queryContext->world->customFilterFcn;+		if ( customFilterFcn != NULL )+		{+			b2ShapeId idA = { sensorShapeId + 1, world->worldId, sensorShape->generation };+			b2ShapeId idB = { shapeId + 1, world->worldId, otherShape->generation };+			bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext );+			if ( shouldCollide == false )+			{+				return true;+			}+		}+	}++	// The relative pose is differenced in double so sensor overlap stays exact far from the origin+	b2WorldTransform otherTransform = b2GetBodyTransform( world, otherShape->bodyId );++	b2DistanceInput input;+	input.proxyA = b2MakeShapeDistanceProxy( sensorShape );+	input.proxyB = b2MakeShapeDistanceProxy( otherShape );+	input.transform = b2InvMulWorldTransforms( queryContext->transform, otherTransform );+	input.useRadii = true;+	b2SimplexCache cache = { 0 };+	b2DistanceOutput output = b2ComputeShapeDistance( &input, &cache, NULL, 0 );++	bool overlaps = output.distance < 10.0f * FLT_EPSILON;+	if ( overlaps == false )+	{+		return true;+	}++	// Record the overlap+	b2Sensor* sensor = queryContext->sensor;+	b2Visitor* shapeRef = b2Array_Emplace( sensor->overlaps2 );+	shapeRef->shapeId = shapeId;+	shapeRef->generation = otherShape->generation;++	return true;+}++static int b2CompareVisitors( const void* a, const void* b )+{+	const b2Visitor* sa = a;+	const b2Visitor* sb = b;++	if ( sa->shapeId < sb->shapeId )+	{+		return -1;+	}++	return 1;+}++static void b2SensorTask( int startIndex, int endIndex, int threadIndex, void* context )+{+	b2TracyCZoneNC( sensor_task, "Overlap", b2_colorBrown, true );++	b2World* world = context;+	b2SensorTaskContext* taskContext = world->sensorTaskContexts.data + threadIndex;++	B2_ASSERT( startIndex < endIndex );++	b2DynamicTree* trees = world->broadPhase.trees;+	for ( int sensorIndex = startIndex; sensorIndex < endIndex; ++sensorIndex )+	{+		b2Sensor* sensor = b2Array_Get( world->sensors, sensorIndex );+		b2Shape* sensorShape = b2Array_Get( world->shapes, sensor->shapeId );++		// Swap overlap arrays+		b2Array( b2Visitor ) temp = sensor->overlaps1;+		sensor->overlaps1 = sensor->overlaps2;+		sensor->overlaps2 = temp;+		b2Array_Clear( sensor->overlaps2 );++		// Append sensor hits+		int hitCount = sensor->hits.count;+		for ( int i = 0; i < hitCount; ++i )+		{+			b2Array_Push( sensor->overlaps2, sensor->hits.data[i] );+		}++		// Clear the hits+		b2Array_Clear( sensor->hits );++		b2Body* body = b2Array_Get( world->bodies, sensorShape->bodyId );+		if ( body->setIndex == b2_disabledSet || sensorShape->enableSensorEvents == false )+		{+			if ( sensor->overlaps1.count != 0 )+			{+				// This sensor is dropping all overlaps because it has been disabled.+				b2SetBit( &taskContext->eventBits, sensorIndex );+			}+			continue;+		}++		b2WorldTransform transform = b2GetBodyTransformQuick( world, body );++		struct b2SensorQueryContext queryContext = {+			.world = world,+			.taskContext = taskContext,+			.sensor = sensor,+			.sensorShape = sensorShape,+			.transform = transform,+		};++		B2_ASSERT( sensorShape->sensorIndex == sensorIndex );+		b2AABB queryBounds = sensorShape->aabb;++		// Query all trees+		b2DynamicTree_OverlapAABB( trees + 0, queryBounds, sensorShape->filter.maskBits, b2SensorQueryCallback, &queryContext );+		b2DynamicTree_OverlapAABB( trees + 1, queryBounds, sensorShape->filter.maskBits, b2SensorQueryCallback, &queryContext );+		b2DynamicTree_OverlapAABB( trees + 2, queryBounds, sensorShape->filter.maskBits, b2SensorQueryCallback, &queryContext );++		// Sort the overlaps to enable finding begin and end events.+		qsort( sensor->overlaps2.data, sensor->overlaps2.count, sizeof( b2Visitor ), b2CompareVisitors );++		// Remove duplicates from overlaps2 (sorted). Duplicates are possible due to the hit events appended earlier.+		int uniqueCount = 0;+		int overlapCount = sensor->overlaps2.count;+		b2Visitor* 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+			b2SetBit( &taskContext->eventBits, sensorIndex );+		}+		else+		{+			for ( int i = 0; i < count1; ++i )+			{+				b2Visitor* s1 = sensor->overlaps1.data + i;+				b2Visitor* s2 = sensor->overlaps2.data + i;++				if ( s1->shapeId != s2->shapeId || s1->generation != s2->generation )+				{+					// something changed+					b2SetBit( &taskContext->eventBits, sensorIndex );+					break;+				}+			}+		}+	}++	b2TracyCZoneEnd( sensor_task );+}++void b2OverlapSensors( b2World* world )+{+	int sensorCount = world->sensors.count;+	if ( sensorCount == 0 )+	{+		return;+	}++	B2_ASSERT( world->workerCount > 0 );++	b2TracyCZoneNC( overlap_sensors, "Sensors", b2_colorMediumPurple, true );++	for ( int i = 0; i < world->workerCount; ++i )+	{+		b2SetBitCountAndClear( &world->sensorTaskContexts.data[i].eventBits, sensorCount );+	}++	// Parallel-for sensors overlaps+	int minRange = 16;+	b2ParallelFor( world, &b2SensorTask, sensorCount, minRange, world );++	b2TracyCZoneNC( sensor_state, "Events", b2_colorLightSlateGray, true );++	b2BitSet* bitSet = &world->sensorTaskContexts.data[0].eventBits;+	for ( int i = 1; i < world->workerCount; ++i )+	{+		b2InPlaceUnion( 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 = b2CTZ64( word );+			int sensorIndex = (int)( 64 * k + ctz );++			b2Sensor* sensor = b2Array_Get( world->sensors, sensorIndex );+			b2Shape* sensorShape = b2Array_Get( world->shapes, sensor->shapeId );+			b2ShapeId sensorId = { sensor->shapeId + 1, world->worldId, sensorShape->generation };++			int count1 = sensor->overlaps1.count;+			int count2 = sensor->overlaps2.count;+			const b2Visitor* refs1 = sensor->overlaps1.data;+			const b2Visitor* 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 b2Visitor* r1 = refs1 + index1;+				const b2Visitor* r2 = refs2 + index2;+				if ( r1->shapeId == r2->shapeId )+				{+					if ( r1->generation < r2->generation )+					{+						// end+						b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };+						b2SensorEndTouchEvent event = {+							.sensorShapeId = sensorId,+							.visitorShapeId = visitorId,+						};+						b2Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );+						index1 += 1;+					}+					else if ( r1->generation > r2->generation )+					{+						// begin+						b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };+						b2SensorBeginTouchEvent event = { sensorId, visitorId };+						b2Array_Push( world->sensorBeginEvents, event );+						index2 += 1;+					}+					else+					{+						// persisted+						index1 += 1;+						index2 += 1;+					}+				}+				else if ( r1->shapeId < r2->shapeId )+				{+					// end+					b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };+					b2SensorEndTouchEvent event = { sensorId, visitorId };+					b2Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );+					index1 += 1;+				}+				else+				{+					// begin+					b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };+					b2SensorBeginTouchEvent event = { sensorId, visitorId };+					b2Array_Push( world->sensorBeginEvents, event );+					index2 += 1;+				}+			}++			while ( index1 < count1 )+			{+				// end+				const b2Visitor* r1 = refs1 + index1;+				b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };+				b2SensorEndTouchEvent event = { sensorId, visitorId };+				b2Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );+				index1 += 1;+			}++			while ( index2 < count2 )+			{+				// begin+				const b2Visitor* r2 = refs2 + index2;+				b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };+				b2SensorBeginTouchEvent event = { sensorId, visitorId };+				b2Array_Push( world->sensorBeginEvents, event );+				index2 += 1;+			}++			// Clear the smallest set bit+			word = word & ( word - 1 );+		}+	}++	b2TracyCZoneEnd( sensor_state );+	b2TracyCZoneEnd( overlap_sensors );+}++void b2DestroySensor( b2World* world, b2Shape* sensorShape )+{+	b2Sensor* sensor = b2Array_Get( world->sensors, sensorShape->sensorIndex );+	for ( int i = 0; i < sensor->overlaps2.count; ++i )+	{+		b2Visitor* ref = sensor->overlaps2.data + i;+		b2SensorEndTouchEvent event = {+			.sensorShapeId =+				{+					.index1 = sensorShape->id + 1,+					.world0 = world->worldId,+					.generation = sensorShape->generation,+				},+			.visitorShapeId =+				{+					.index1 = ref->shapeId + 1,+					.world0 = world->worldId,+					.generation = ref->generation,+				},+		};++		b2Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );+	}++	// Destroy sensor+	b2Array_Destroy( sensor->hits );+	b2Array_Destroy( sensor->overlaps1 );+	b2Array_Destroy( sensor->overlaps2 );++	int movedIndex = b2Array_RemoveSwap( world->sensors, sensorShape->sensorIndex );+	if ( movedIndex != B2_NULL_INDEX )+	{+		// Fixup moved sensor+		b2Sensor* movedSensor = b2Array_Get( world->sensors, sensorShape->sensorIndex );+		b2Shape* otherSensorShape = b2Array_Get( world->shapes, movedSensor->shapeId );+		otherSensorShape->sensorIndex = sensorShape->sensorIndex;+	}+}
+ upstream/src/sensor.h view
@@ -0,0 +1,49 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "bitset.h"+#include "container.h"++typedef struct b2Shape b2Shape;+typedef struct b2World b2World;++// Used to track shapes that hit sensors using time of impact+typedef struct b2SensorHit+{+	int sensorId;+	int visitorId;+} b2SensorHit;++b2DeclareArray( b2SensorHit );++typedef struct b2Visitor+{+	int shapeId;+	uint16_t generation;+} b2Visitor;++b2DeclareArray( b2Visitor );++typedef struct b2Sensor+{+	// todo find a way to pool these+	b2Array( b2Visitor ) hits;+	b2Array( b2Visitor ) overlaps1;+	b2Array( b2Visitor ) overlaps2;+	int shapeId;+} b2Sensor;++b2DeclareArray( b2Sensor );++typedef struct b2SensorTaskContext+{+	b2BitSet eventBits;+} b2SensorTaskContext;++b2DeclareArray( b2SensorTaskContext );++void b2OverlapSensors( b2World* world );++void b2DestroySensor( b2World* world, b2Shape* sensorShape );
+ upstream/src/shape.c view
@@ -0,0 +1,2080 @@+// SPDX-FileCopyrightText: 2023 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 "solver_set.h"++#include "box2d/box2d.h"++#include <stddef.h>++static b2Shape* b2GetShape( b2World* world, b2ShapeId shapeId )+{+	int id = shapeId.index1 - 1;+	b2Shape* shape = b2Array_Get( world->shapes, id );+	B2_ASSERT( shape->id == id && shape->generation == shapeId.generation );+	return shape;+}++static b2ChainShape* b2GetChainShape( b2World* world, b2ChainId chainId )+{+	int id = chainId.index1 - 1;+	b2ChainShape* chain = b2Array_Get( world->chainShapes, id );+	B2_ASSERT( chain->id == id && chain->generation == chainId.generation );+	return chain;+}++static float b2ComputeShapeMargin( b2Shape* shape )+{+	float margin = 0.0f;++	switch ( shape->type )+	{+		case b2_capsuleShape:+		{+			margin = 0.5f * b2Distance( shape->capsule.center2, shape->capsule.center1 ) + shape->capsule.radius;+		}+		break;++		case b2_circleShape:+		{+			margin = shape->circle.radius;+		}+		break;++		case b2_polygonShape:+		{+			const b2Polygon* poly = &shape->polygon;+			float maxExtentSqr = 0.0f;+			int count = poly->count;+			for ( int i = 0; i < count; ++i )+			{+				float distanceSqr = b2DistanceSquared( poly->vertices[i], poly->centroid );+				maxExtentSqr = b2MaxFloat( maxExtentSqr, distanceSqr );+			}++			margin = sqrtf( maxExtentSqr );+		}+		break;++		case b2_segmentShape:+		{+			margin = 0.5f * b2Distance( shape->segment.point1, shape->segment.point2 );+		}+		break;++		case b2_chainSegmentShape:+		{+			margin = 0.5f * b2Distance( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2 );+		}+		break;++		default:+			B2_VALIDATE( false );+			return B2_MAX_AABB_MARGIN;+	}++	return b2MinFloat( B2_MAX_AABB_MARGIN, B2_AABB_MARGIN_FRACTION * margin );+}++static void b2UpdateShapeAABBs( b2Shape* shape, b2AABB* fatAABB, b2WorldTransform transform, b2BodyType proxyType )+{+	// Compute a bounding box with a speculative margin+	const float speculativeDistance = B2_SPECULATIVE_DISTANCE;+	const float aabbMargin = shape->aabbMargin;++	b2AABB aabb = b2ComputeFatShapeAABB( shape, transform, speculativeDistance );+	shape->aabb = aabb;++	// Smaller margin for static bodies. Cannot be zero due to TOI tolerance.+	float margin = proxyType == b2_staticBody ? speculativeDistance : aabbMargin;+	fatAABB->lowerBound.x = aabb.lowerBound.x - margin;+	fatAABB->lowerBound.y = aabb.lowerBound.y - margin;+	fatAABB->upperBound.x = aabb.upperBound.x + margin;+	fatAABB->upperBound.y = aabb.upperBound.y + margin;+}++static b2Shape* b2CreateShapeInternal( b2World* world, b2Body* body, b2WorldTransform transform, const b2ShapeDef* def,+									   const void* geometry, b2ShapeType shapeType )+{+	int shapeId = b2AllocId( &world->shapeIdPool );++	if ( shapeId == world->shapes.count )+	{+		b2Array_Push( world->shapes, (b2Shape){ 0 } );+		b2Array_Push( world->fatAABBs, (b2AABB){ 0 } );+	}+	else+	{+		B2_ASSERT( world->shapes.data[shapeId].id == B2_NULL_INDEX );+	}++	B2_ASSERT( world->fatAABBs.count == world->shapes.count );++	b2Shape* shape = b2Array_Get( world->shapes, shapeId );++	switch ( shapeType )+	{+		case b2_capsuleShape:+			shape->capsule = *(const b2Capsule*)geometry;+			break;++		case b2_circleShape:+			shape->circle = *(const b2Circle*)geometry;+			break;++		case b2_polygonShape:+			shape->polygon = *(const b2Polygon*)geometry;+			break;++		case b2_segmentShape:+			shape->segment = *(const b2Segment*)geometry;+			break;++		case b2_chainSegmentShape:+			shape->chainSegment = *(const b2ChainSegment*)geometry;+			break;++		default:+			B2_ASSERT( false );+			break;+	}++	shape->id = shapeId;+	shape->bodyId = body->id;+	shape->type = shapeType;+	shape->density = def->density;+	shape->material = def->material;+	shape->filter = def->filter;+	shape->userData = def->userData;+	shape->enableSensorEvents = def->enableSensorEvents;+	shape->enableContactEvents = def->enableContactEvents;+	shape->enableCustomFiltering = def->enableCustomFiltering;+	shape->enableHitEvents = def->enableHitEvents;+	shape->enablePreSolveEvents = def->enablePreSolveEvents;+	shape->proxyKey = B2_NULL_INDEX;+	shape->localCentroid = b2GetShapeCentroid( shape );+	shape->aabbMargin = b2ComputeShapeMargin( shape );+	shape->aabb = (b2AABB){ b2Vec2_zero, b2Vec2_zero };+	world->fatAABBs.data[shapeId] = (b2AABB){ b2Vec2_zero, b2Vec2_zero };+	shape->generation += 1;++	if ( body->setIndex != b2_disabledSet )+	{+		b2BodyType proxyType = body->type;+		b2CreateShapeProxy( world, shape, proxyType, transform, def->invokeContactCreation || def->isSensor );+	}++	// Add to shape doubly linked list+	if ( body->headShapeId != B2_NULL_INDEX )+	{+		b2Shape* headShape = b2Array_Get( world->shapes, body->headShapeId );+		headShape->prevShapeId = shapeId;+	}++	shape->prevShapeId = B2_NULL_INDEX;+	shape->nextShapeId = body->headShapeId;+	body->headShapeId = shapeId;+	body->shapeCount += 1;++	if ( def->isSensor )+	{+		shape->sensorIndex = world->sensors.count;+		b2Sensor sensor = { 0 };+		sensor.shapeId = shapeId;+		b2Array_CreateN( sensor.hits, 4 );+		b2Array_CreateN( sensor.overlaps1, 16 );+		b2Array_CreateN( sensor.overlaps2, 16 );+		b2Array_Push( world->sensors, sensor );+	}+	else+	{+		shape->sensorIndex = B2_NULL_INDEX;+	}++	b2ValidateSolverSets( world );++	return shape;+}++static bool b2IsValidMaterial( const b2SurfaceMaterial* material )+{+	return b2IsValidFloat( material->friction ) && material->friction >= 0.0f && b2IsValidFloat( material->restitution ) &&+		   material->restitution >= 0.0f && b2IsValidFloat( material->rollingResistance ) &&+		   material->rollingResistance >= 0.0f && b2IsValidFloat( material->tangentSpeed );+}++static bool b2IsValidCircle( const b2Circle* circle )+{+	return b2IsValidVec2( circle->center ) && b2IsValidFloat( circle->radius ) && circle->radius >= 0.0f;+}++static bool b2IsValidCapsule( const b2Capsule* capsule )+{+	return b2IsValidVec2( capsule->center1 ) && b2IsValidVec2( capsule->center2 ) && b2IsValidFloat( capsule->radius ) &&+		   capsule->radius >= 0.0f;+}++static bool b2IsValidSegment( const b2Segment* segment )+{+	return b2IsValidVec2( segment->point1 ) && b2IsValidVec2( segment->point2 );+}++static bool b2IsValidChainSegment( const b2ChainSegment* chainSegment )+{+	return b2IsValidVec2( chainSegment->ghost1 ) && b2IsValidSegment( &chainSegment->segment ) &&+		   b2IsValidVec2( chainSegment->ghost2 );+}++static bool b2IsValidPolygon( const b2Polygon* polygon )+{+	if ( polygon->count < 1 || polygon->count > B2_MAX_POLYGON_VERTICES )+	{+		return false;+	}++	if ( b2IsValidFloat( polygon->radius ) == false || polygon->radius < 0.0f )+	{+		return false;+	}++	if ( b2IsValidVec2( polygon->centroid ) == false )+	{+		return false;+	}++	for ( int i = 0; i < polygon->count; ++i )+	{+		if ( b2IsValidVec2( polygon->vertices[i] ) == false || b2IsValidVec2( polygon->normals[i] ) == false )+		{+			return false;+		}+	}++	return true;+}++static b2ShapeId b2CreateShape( b2BodyId bodyId, const b2ShapeDef* def, const void* geometry, b2ShapeType shapeType )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( b2IsValidFloat( def->density ) && def->density >= 0.0f, (b2ShapeId){ 0 } );+	B2_CHECK_INPUT_RETURN( b2IsValidMaterial( &def->material ), (b2ShapeId){ 0 } );++	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return (b2ShapeId){ 0 };+	}++	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );++	b2Shape* shape = b2CreateShapeInternal( world, body, transform, def, geometry, shapeType );++	if ( def->updateBodyMass == true )+	{+		b2UpdateBodyMassData( world, body );+	}+	else if ( ( body->flags & b2_dirtyMass ) == 0 )+	{+		body->flags |= b2_dirtyMass;+		b2SyncBodyFlags( world, body );+	}++	b2ValidateSolverSets( world );++	b2ShapeId id = { shape->id + 1, bodyId.world0, shape->generation };+	return id;+}++b2ShapeId b2CreateCircleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle )+{+	B2_CHECK_INPUT_RETURN( b2IsValidCircle( circle ), b2_nullShapeId );++	b2ShapeId id = b2CreateShape( bodyId, def, circle, b2_circleShape );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC_CREATE( world, CreateCircleShape, id, bodyId, *def, *circle );++	return id;+}++b2ShapeId b2CreateCapsuleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule )+{+	B2_CHECK_INPUT_RETURN( b2IsValidCapsule( capsule ), b2_nullShapeId );++	float lengthSqr = b2DistanceSquared( capsule->center1, capsule->center2 );+	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )+	{+		return b2_nullShapeId;+	}++	b2ShapeId id = b2CreateShape( bodyId, def, capsule, b2_capsuleShape );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC_CREATE( world, CreateCapsuleShape, id, bodyId, *def, *capsule );++	return id;+}++b2ShapeId b2CreatePolygonShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon )+{+	B2_CHECK_INPUT_RETURN( b2IsValidPolygon( polygon ), b2_nullShapeId );++	b2ShapeId id = b2CreateShape( bodyId, def, polygon, b2_polygonShape );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC_CREATE( world, CreatePolygonShape, id, bodyId, *def, *polygon );++	return id;+}++b2ShapeId b2CreateSegmentShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment )+{+	B2_CHECK_INPUT_RETURN( b2IsValidSegment( segment ), b2_nullShapeId );++	float lengthSqr = b2DistanceSquared( segment->point1, segment->point2 );+	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )+	{+		B2_ASSERT( false );+		return b2_nullShapeId;+	}++	b2ShapeId id = b2CreateShape( bodyId, def, segment, b2_segmentShape );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC_CREATE( world, CreateSegmentShape, id, bodyId, *def, *segment );++	return id;+}++b2ShapeId b2CreateChainSegmentShape( b2BodyId bodyId, const b2ShapeDef* def, const b2ChainSegment* chainSegment )+{+	B2_CHECK_INPUT_RETURN( b2IsValidChainSegment( chainSegment ), b2_nullShapeId );++	float lengthSqr = b2DistanceSquared( chainSegment->segment.point1, chainSegment->segment.point2 );+	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )+	{+		B2_ASSERT( false );+		return b2_nullShapeId;+	}++	// No parent chain shape+	b2ChainSegment local = *chainSegment;+	local.chainId = B2_NULL_INDEX;++	b2ShapeId id = b2CreateShape( bodyId, def, &local, b2_chainSegmentShape );++	b2World* world = b2GetWorld( bodyId.world0 );+	B2_REC_CREATE( world, CreateChainSegmentShape, id, bodyId, *def, local );++	return id;+}++// Destroy a shape on a body. This doesn't need to be called when destroying a body.+static void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body )+{+	int shapeId = shape->id;++	// Remove the shape from the body's doubly linked list.+	if ( shape->prevShapeId != B2_NULL_INDEX )+	{+		b2Shape* prevShape = b2Array_Get( world->shapes, shape->prevShapeId );+		prevShape->nextShapeId = shape->nextShapeId;+	}++	if ( shape->nextShapeId != B2_NULL_INDEX )+	{+		b2Shape* nextShape = b2Array_Get( world->shapes, shape->nextShapeId );+		nextShape->prevShapeId = shape->prevShapeId;+	}++	if ( shapeId == body->headShapeId )+	{+		body->headShapeId = shape->nextShapeId;+	}++	body->shapeCount -= 1;++	// Remove from broad-phase.+	b2DestroyShapeProxy( shape, &world->broadPhase );++	// Destroy any contacts associated with the shape.+	int contactKey = body->headContactKey;+	while ( contactKey != B2_NULL_INDEX )+	{+		int contactId = contactKey >> 1;+		int edgeIndex = contactKey & 1;++		b2Contact* contact = b2Array_Get( world->contacts, contactId );+		contactKey = contact->edges[edgeIndex].nextKey;++		if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId )+		{+			b2DestroyContact( world, contact );+		}+	}++	if ( shape->sensorIndex != B2_NULL_INDEX )+	{+		b2Sensor* sensor = b2Array_Get( world->sensors, shape->sensorIndex );+		for ( int i = 0; i < sensor->overlaps2.count; ++i )+		{+			b2Visitor* ref = sensor->overlaps2.data + i;+			b2SensorEndTouchEvent event = {+				.sensorShapeId =+					{+						.index1 = shapeId + 1,+						.world0 = world->worldId,+						.generation = shape->generation,+					},+				.visitorShapeId =+					{+						.index1 = ref->shapeId + 1,+						.world0 = world->worldId,+						.generation = ref->generation,+					},+			};++			b2Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );+		}++		// Destroy sensor+		b2Array_Destroy( sensor->hits );+		b2Array_Destroy( sensor->overlaps1 );+		b2Array_Destroy( sensor->overlaps2 );++		int movedIndex = b2Array_RemoveSwap( world->sensors, shape->sensorIndex );+		if ( movedIndex != B2_NULL_INDEX )+		{+			// Fixup moved sensor+			b2Sensor* movedSensor = b2Array_Get( world->sensors, shape->sensorIndex );+			b2Shape* otherSensorShape = b2Array_Get( world->shapes, movedSensor->shapeId );+			otherSensorShape->sensorIndex = shape->sensorIndex;+		}+	}++	// Return shape to free list.+	b2FreeId( &world->shapeIdPool, shapeId );+	shape->id = B2_NULL_INDEX;++	b2ValidateSolverSets( world );+}++void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	b2Shape* shape = b2GetShape( world, shapeId );++	// Cannot destroy a chain segment that has a parent chain shape+	if ( shape->type == b2_chainSegmentShape && shape->chainSegment.chainId != B2_NULL_INDEX )+	{+		B2_ASSERT( false );+		return;+	}++	B2_REC( world, DestroyShape, shapeId, updateBodyMass );++	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	b2DestroyShapeInternal( world, shape, body );++	if ( updateBodyMass == true )+	{+		b2UpdateBodyMassData( world, body );+	}+}++b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def )+{+	B2_CHECK_DEF( def );+	B2_CHECK_INPUT_RETURN( def->isLoop ? def->pointCount >= 3 : def->pointCount >= 2, (b2ChainId){ 0 } );++	int segmentCount = def->isLoop ? def->pointCount : def->pointCount - 1;++	B2_CHECK_INPUT_RETURN( def->materialCount == 1 || def->materialCount == segmentCount, (b2ChainId){ 0 } );++	for ( int i = 0; i < def->pointCount; ++i )+	{+		B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->points[i] ), (b2ChainId){ 0 } );+	}++	for ( int i = 0; i < def->materialCount; ++i )+	{+		B2_CHECK_INPUT_RETURN( b2IsValidMaterial( def->materials + i ), (b2ChainId){ 0 } );+	}++	if ( def->isLoop == false )+	{+		// These are set to INFINITY in the default def as a sentinel to ensure they are set.+		B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->ghost1 ), (b2ChainId){ 0 } );+		B2_CHECK_INPUT_RETURN( b2IsValidVec2( def->ghost2 ), (b2ChainId){ 0 } );+	}++	b2World* world = b2GetWorldLocked( bodyId.world0 );+	if ( world == NULL )+	{+		return (b2ChainId){ 0 };+	}++	b2Body* body = b2GetBodyFullId( world, bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );++	int chainId = b2AllocId( &world->chainIdPool );++	if ( chainId == world->chainShapes.count )+	{+		b2Array_Push( world->chainShapes, (b2ChainShape){ 0 } );+	}+	else+	{+		B2_ASSERT( world->chainShapes.data[chainId].id == B2_NULL_INDEX );+	}++	b2ChainShape* chainShape = b2Array_Get( world->chainShapes, chainId );++	chainShape->id = chainId;+	chainShape->bodyId = body->id;+	chainShape->nextChainId = body->headChainId;+	chainShape->segmentCount = segmentCount;+	chainShape->generation += 1;++	int materialCount = def->materialCount;++	body->headChainId = chainId;++	b2ShapeDef shapeDef = b2DefaultShapeDef();+	shapeDef.userData = def->userData;+	shapeDef.filter = def->filter;+	shapeDef.enableSensorEvents = def->enableSensorEvents;+	shapeDef.enableContactEvents = false;+	shapeDef.enableHitEvents = false;++	const b2Vec2* points = def->points;+	int n = segmentCount;+	chainShape->shapeIndices = b2Alloc( n * sizeof( int ) );++	float tolSqr = B2_LINEAR_SLOP * B2_LINEAR_SLOP;+	B2_UNUSED( tolSqr );++	if ( def->isLoop )+	{+		b2ChainSegment chainSegment;++		int prevIndex = n - 1;+		for ( int i = 0; i < n; ++i )+		{+			chainSegment.ghost1 = points[prevIndex];+			chainSegment.segment.point1 = points[i];+			chainSegment.segment.point2 = points[( i + 1 ) % n];+			chainSegment.ghost2 = points[( i + 2 ) % n];++			B2_ASSERT( b2DistanceSquared( chainSegment.ghost1, chainSegment.segment.point1 ) > tolSqr );+			B2_ASSERT( b2DistanceSquared( chainSegment.segment.point1, chainSegment.segment.point2 ) > tolSqr );++			chainSegment.chainId = chainId;+			prevIndex = i;++			int materialIndex = materialCount == 1 ? 0 : i;+			shapeDef.material = def->materials[materialIndex];++			b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape );+			chainShape->shapeIndices[i] = shape->id;+		}+	}+	else+	{+		b2ChainSegment chainSegment;++		for ( int i = 0; i < n; ++i )+		{+			B2_ASSERT( i + 1 < def->pointCount );+			chainSegment.ghost1 = i == 0 ? def->ghost1 : points[i - 1];+			chainSegment.segment.point1 = points[i + 0];+			chainSegment.segment.point2 = points[i + 1];+			chainSegment.ghost2 = i == n - 1 ? def->ghost2 : points[i + 2];++			B2_ASSERT( b2DistanceSquared( chainSegment.ghost1, chainSegment.segment.point1 ) > tolSqr );+			B2_ASSERT( b2DistanceSquared( chainSegment.segment.point1, chainSegment.segment.point2 ) > tolSqr );+			B2_ASSERT( b2DistanceSquared( chainSegment.segment.point2, chainSegment.ghost2 ) > tolSqr );++			chainSegment.chainId = chainId;++			int materialIndex = materialCount == 1 ? 0 : i;+			shapeDef.material = def->materials[materialIndex];++			b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape );+			chainShape->shapeIndices[i] = shape->id;+		}+	}++	b2ChainId id = { chainId + 1, world->worldId, chainShape->generation };++	B2_REC_CREATE( world, CreateChain, id, bodyId, *def );++	return id;+}++void b2FreeChainData( b2ChainShape* chain )+{+	b2Free( chain->shapeIndices, chain->segmentCount * sizeof( int ) );+	chain->shapeIndices = NULL;+}++void b2DestroyChain( b2ChainId chainId )+{+	b2World* world = b2GetWorldLocked( chainId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, DestroyChain, chainId );++	b2ChainShape* chain = b2GetChainShape( world, chainId );++	b2Body* body = b2Array_Get( world->bodies, chain->bodyId );++	// Remove the chain from the body's singly linked list.+	int* chainIdPtr = &body->headChainId;+	bool found = false;+	while ( *chainIdPtr != B2_NULL_INDEX )+	{+		if ( *chainIdPtr == chain->id )+		{+			*chainIdPtr = chain->nextChainId;+			found = true;+			break;+		}++		chainIdPtr = &( world->chainShapes.data[*chainIdPtr].nextChainId );+	}++	B2_ASSERT( found == true );+	if ( found == false )+	{+		return;+	}++	int count = chain->segmentCount;+	for ( int i = 0; i < count; ++i )+	{+		int shapeId = chain->shapeIndices[i];+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		b2DestroyShapeInternal( world, shape, body );+	}++	b2FreeChainData( chain );++	// Return chain to free list.+	b2FreeId( &world->chainIdPool, chain->id );+	chain->id = B2_NULL_INDEX;++	b2ValidateSolverSets( world );+}++b2WorldId b2Chain_GetWorld( b2ChainId chainId )+{+	b2World* world = b2GetWorld( chainId.world0 );+	return (b2WorldId){ chainId.world0 + 1, world->generation };+}++int b2Chain_GetSegmentCount( b2ChainId chainId )+{+	b2World* world = b2GetWorldLocked( chainId.world0 );+	if ( world == NULL )+	{+		return 0;+	}++	b2ChainShape* chain = b2GetChainShape( world, chainId );+	return chain->segmentCount;+}++int b2Chain_GetSegments( b2ChainId chainId, b2ShapeId* segmentArray, int capacity )+{+	b2World* world = b2GetWorldLocked( chainId.world0 );+	if ( world == NULL )+	{+		return 0;+	}++	b2ChainShape* chain = b2GetChainShape( world, chainId );++	int count = b2MinInt( chain->segmentCount, capacity );+	for ( int i = 0; i < count; ++i )+	{+		int shapeId = chain->shapeIndices[i];+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		segmentArray[i] = (b2ShapeId){ shapeId + 1, chainId.world0, shape->generation };+	}++	return count;+}++void b2Chain_SetSurfaceMaterial( b2ChainId chainId, const b2SurfaceMaterial* material, int segmentIndex )+{+	B2_CHECK_INPUT( b2IsValidMaterial( material ) );++	b2World* world = b2GetWorldLocked( chainId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ChainSetSurfaceMaterial, chainId, *material, segmentIndex );++	b2ChainShape* chainShape = b2GetChainShape( world, chainId );+	B2_ASSERT( 0 <= segmentIndex && segmentIndex < chainShape->segmentCount );+	int shapeId = chainShape->shapeIndices[segmentIndex];+	b2Shape* shape = b2Array_Get( world->shapes, shapeId );+	shape->material = *material;+}++void b2Chain_SetAllSurfaceMaterials( b2ChainId chainId, const b2SurfaceMaterial* material )+{+	B2_CHECK_INPUT( b2IsValidMaterial( material ) );++	b2World* world = b2GetWorldLocked( chainId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ChainSetAllSurfaceMaterials, chainId, *material );++	b2ChainShape* chainShape = b2GetChainShape( world, chainId );++	for ( int i = 0; i < chainShape->segmentCount; ++i )+	{+		int shapeId = chainShape->shapeIndices[i];+		b2Shape* shape = b2Array_Get( world->shapes, shapeId );+		shape->material = *material;+	}+}++b2SurfaceMaterial b2Chain_GetSurfaceMaterial( b2ChainId chainId, int segmentIndex )+{+	b2World* world = b2GetWorld( chainId.world0 );+	b2ChainShape* chainShape = b2GetChainShape( world, chainId );+	B2_ASSERT( 0 <= segmentIndex && segmentIndex < chainShape->segmentCount );+	int shapeId = chainShape->shapeIndices[segmentIndex];+	b2Shape* shape = b2Array_Get( world->shapes, shapeId );+	return shape->material;+}++b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2WorldTransform xf )+{+	switch ( shape->type )+	{+		case b2_capsuleShape:+			return b2ComputeCapsuleBounds( &shape->capsule, xf );+		case b2_circleShape:+			return b2ComputeCircleBounds( &shape->circle, xf );+		case b2_polygonShape:+			return b2ComputePolygonBounds( &shape->polygon, xf );+		case b2_segmentShape:+			return b2ComputeSegmentBounds( &shape->segment, xf );+		case b2_chainSegmentShape:+			return b2ComputeSegmentBounds( &shape->chainSegment.segment, xf );+		default:+		{+			B2_ASSERT( false );+			b2AABB empty = { { 0.0f, 0.0f }, { 0.0f, 0.0f } };+			return empty;+		}+	}+}++b2Vec2 b2GetShapeCentroid( const b2Shape* shape )+{+	switch ( shape->type )+	{+		case b2_capsuleShape:+			return b2Lerp( shape->capsule.center1, shape->capsule.center2, 0.5f );+		case b2_circleShape:+			return shape->circle.center;+		case b2_polygonShape:+			return shape->polygon.centroid;+		case b2_segmentShape:+			return b2Lerp( shape->segment.point1, shape->segment.point2, 0.5f );+		case b2_chainSegmentShape:+			return b2Lerp( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2, 0.5f );+		default:+			return b2Vec2_zero;+	}+}++// todo_erin maybe compute this on shape creation+float b2GetShapePerimeter( const b2Shape* shape )+{+	switch ( shape->type )+	{+		case b2_capsuleShape:+			return 2.0f * b2Length( b2Sub( shape->capsule.center1, shape->capsule.center2 ) ) ++				   2.0f * B2_PI * shape->capsule.radius;+		case b2_circleShape:+			return 2.0f * B2_PI * shape->circle.radius;+		case b2_polygonShape:+		{+			const b2Vec2* points = shape->polygon.vertices;+			int count = shape->polygon.count;+			float perimeter = 2.0f * B2_PI * shape->polygon.radius;+			B2_ASSERT( count > 0 );+			b2Vec2 prev = points[count - 1];+			for ( int i = 0; i < count; ++i )+			{+				b2Vec2 next = points[i];+				perimeter += b2Length( b2Sub( next, prev ) );+				prev = next;+			}++			return perimeter;+		}+		case b2_segmentShape:+			return 2.0f * b2Length( b2Sub( shape->segment.point1, shape->segment.point2 ) );+		case b2_chainSegmentShape:+			return 2.0f * b2Length( b2Sub( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2 ) );+		default:+			return 0.0f;+	}+}++// This projects the shape perimeter onto an infinite line+float b2GetShapeProjectedPerimeter( const b2Shape* shape, b2Vec2 line )+{+	switch ( shape->type )+	{+		case b2_capsuleShape:+		{+			b2Vec2 axis = b2Sub( shape->capsule.center2, shape->capsule.center1 );+			float projectedLength = b2AbsFloat( b2Dot( axis, line ) );+			return projectedLength + 2.0f * shape->capsule.radius;+		}++		case b2_circleShape:+			return 2.0f * shape->circle.radius;++		case b2_polygonShape:+		{+			const b2Vec2* points = shape->polygon.vertices;+			int count = shape->polygon.count;+			B2_ASSERT( count > 0 );+			float value = b2Dot( points[0], line );+			float lower = value;+			float upper = value;+			for ( int i = 1; i < count; ++i )+			{+				value = b2Dot( points[i], line );+				lower = b2MinFloat( lower, value );+				upper = b2MaxFloat( upper, value );+			}++			return ( upper - lower ) + 2.0f * shape->polygon.radius;+		}++		case b2_segmentShape:+		{+			float value1 = b2Dot( shape->segment.point1, line );+			float value2 = b2Dot( shape->segment.point2, line );+			return b2AbsFloat( value2 - value1 );+		}++		case b2_chainSegmentShape:+		{+			float value1 = b2Dot( shape->chainSegment.segment.point1, line );+			float value2 = b2Dot( shape->chainSegment.segment.point2, line );+			return b2AbsFloat( value2 - value1 );+		}++		default:+			return 0.0f;+	}+}++b2MassData b2ComputeShapeMass( const b2Shape* shape )+{+	switch ( shape->type )+	{+		case b2_capsuleShape:+			return b2ComputeCapsuleMass( &shape->capsule, shape->density );+		case b2_circleShape:+			return b2ComputeCircleMass( &shape->circle, shape->density );+		case b2_polygonShape:+			return b2ComputePolygonMass( &shape->polygon, shape->density );+		default:+			return (b2MassData){ 0 };+	}+}++b2ShapeExtent b2ComputeShapeExtent( const b2Shape* shape, b2Vec2 localCenter )+{+	b2ShapeExtent extent = { 0 };++	switch ( shape->type )+	{+		case b2_capsuleShape:+		{+			float radius = shape->capsule.radius;+			extent.minExtent = radius;+			b2Vec2 c1 = b2Sub( shape->capsule.center1, localCenter );+			b2Vec2 c2 = b2Sub( shape->capsule.center2, localCenter );+			extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) ) + radius;+		}+		break;++		case b2_circleShape:+		{+			float radius = shape->circle.radius;+			extent.minExtent = radius;+			extent.maxExtent = b2Length( b2Sub( shape->circle.center, localCenter ) ) + radius;+		}+		break;++		case b2_polygonShape:+		{+			const b2Polygon* poly = &shape->polygon;+			float minExtent = B2_HUGE;+			float maxExtentSqr = 0.0f;+			int count = poly->count;+			for ( int i = 0; i < count; ++i )+			{+				b2Vec2 v = poly->vertices[i];+				float planeOffset = b2Dot( poly->normals[i], b2Sub( v, poly->centroid ) );+				minExtent = b2MinFloat( minExtent, planeOffset );++				float distanceSqr = b2LengthSquared( b2Sub( v, localCenter ) );+				maxExtentSqr = b2MaxFloat( maxExtentSqr, distanceSqr );+			}++			extent.minExtent = minExtent + poly->radius;+			extent.maxExtent = sqrtf( maxExtentSqr ) + poly->radius;+		}+		break;++		case b2_segmentShape:+		{+			extent.minExtent = 0.0f;+			b2Vec2 c1 = b2Sub( shape->segment.point1, localCenter );+			b2Vec2 c2 = b2Sub( shape->segment.point2, localCenter );+			extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) );+		}+		break;++		case b2_chainSegmentShape:+		{+			extent.minExtent = 0.0f;+			b2Vec2 c1 = b2Sub( shape->chainSegment.segment.point1, localCenter );+			b2Vec2 c2 = b2Sub( shape->chainSegment.segment.point2, localCenter );+			extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) );+		}+		break;++		default:+			break;+	}++	return extent;+}++b2CastOutput b2RayCastShape( const b2RayCastInput* input, const b2Shape* shape, b2Transform transform )+{+	b2RayCastInput localInput = *input;+	localInput.origin = b2InvTransformPoint( transform, input->origin );+	localInput.translation = b2InvRotateVector( transform.q, input->translation );++	b2CastOutput output = { 0 };+	switch ( shape->type )+	{+		case b2_capsuleShape:+			output = b2CastRayCapsule( &shape->capsule, &localInput );+			break;+		case b2_circleShape:+			output = b2CastRayCircle( &shape->circle, &localInput );+			break;+		case b2_polygonShape:+			output = b2CastRayPolygon( &shape->polygon, &localInput );+			break;+		case b2_segmentShape:+			output = b2CastRaySegment( &shape->segment, &localInput, false );+			break;+		case b2_chainSegmentShape:+			output = b2CastRaySegment( &shape->chainSegment.segment, &localInput, true );+			break;+		default:+			return output;+	}++	// The output point stays in the frame of the input transform, a caller chosen frame that is+	// typically re-centered near the origin.+	output.point = b2TransformPoint( transform, output.point );+	output.normal = b2RotateVector( transform.q, output.normal );+	return output;+}++b2CastOutput b2ShapeCastShape( const b2ShapeCastInput* input, const b2Shape* shape, b2Transform transform )+{+	b2CastOutput output = { 0 };++	if ( input->proxy.count == 0 )+	{+		return output;+	}++	b2ShapeCastInput localInput = *input;++	for ( int i = 0; i < localInput.proxy.count; ++i )+	{+		localInput.proxy.points[i] = b2InvTransformPoint( transform, input->proxy.points[i] );+	}++	localInput.translation = b2InvRotateVector( transform.q, input->translation );++	switch ( shape->type )+	{+		case b2_capsuleShape:+			output = b2CastShapeCapsule( &shape->capsule, &localInput );+			break;+		case b2_circleShape:+			output = b2CastShapeCircle( &shape->circle, &localInput );+			break;+		case b2_polygonShape:+			output = b2CastShapePolygon( &shape->polygon, &localInput );+			break;+		case b2_segmentShape:+			output = b2CastShapeSegment( &shape->segment, &localInput );+			break;+		case b2_chainSegmentShape:+		{+			// Check for back side collision+			b2Vec2 approximateCentroid = localInput.proxy.points[0];+			for ( int i = 1; i < localInput.proxy.count; ++i )+			{+				approximateCentroid = b2Add( approximateCentroid, localInput.proxy.points[i] );+			}++			approximateCentroid = b2MulSV( 1.0f / localInput.proxy.count, approximateCentroid );++			b2Vec2 edge = b2Sub( shape->chainSegment.segment.point2, shape->chainSegment.segment.point1 );+			b2Vec2 r = b2Sub( approximateCentroid, shape->chainSegment.segment.point1 );++			if ( b2Cross( r, edge ) < 0.0f )+			{+				// Shape cast starts behind+				return output;+			}++			output = b2CastShapeSegment( &shape->chainSegment.segment, &localInput );+		}+		break;+		default:+			return output;+	}++	// Same frame contract as b2RayCastShape, the point stays in the input transform frame+	output.point = b2TransformPoint( transform, output.point );+	output.normal = b2RotateVector( transform.q, output.normal );+	return output;+}++b2PlaneResult b2CollideMover( const b2Capsule* mover, const b2Shape* shape, b2Transform transform )+{+	b2Capsule localMover;+	localMover.center1 = b2InvTransformPoint( transform, mover->center1 );+	localMover.center2 = b2InvTransformPoint( transform, mover->center2 );+	localMover.radius = mover->radius;++	b2PlaneResult result = { 0 };+	switch ( shape->type )+	{+		case b2_capsuleShape:+			result = b2CollideMoverAndCapsule( &localMover, &shape->capsule );+			break;+		case b2_circleShape:+			result = b2CollideMoverAndCircle( &localMover, &shape->circle );+			break;+		case b2_polygonShape:+			result = b2CollideMoverAndPolygon( &localMover, &shape->polygon );+			break;+		case b2_segmentShape:+			result = b2CollideMoverAndSegment( &localMover, &shape->segment );+			break;+		case b2_chainSegmentShape:+			result = b2CollideMoverAndSegment( &localMover, &shape->chainSegment.segment );+			break;+		default:+			return result;+	}++	if ( result.hit == false )+	{+		return result;+	}++	result.plane.normal = b2RotateVector( transform.q, result.plane.normal );+	return result;+}++void b2CreateShapeProxy( b2World* world, b2Shape* shape, b2BodyType type, b2WorldTransform transform, bool forcePairCreation )+{+	B2_ASSERT( shape->proxyKey == B2_NULL_INDEX );++	b2AABB* fatAABB = world->fatAABBs.data + shape->id;+	b2UpdateShapeAABBs( shape, fatAABB, transform, type );++	// Create proxies in the broad-phase.+	shape->proxyKey = b2BroadPhase_CreateProxy( &world->broadPhase, type, *fatAABB, shape->filter.categoryBits, shape->id,+												forcePairCreation );+	B2_ASSERT( B2_PROXY_TYPE( shape->proxyKey ) < b2_bodyTypeCount );+}++void b2DestroyShapeProxy( b2Shape* shape, b2BroadPhase* bp )+{+	if ( shape->proxyKey != B2_NULL_INDEX )+	{+		b2BroadPhase_DestroyProxy( bp, shape->proxyKey );+		shape->proxyKey = B2_NULL_INDEX;+	}+}++b2ShapeProxy b2MakeShapeDistanceProxy( const b2Shape* shape )+{+	switch ( shape->type )+	{+		case b2_capsuleShape:+			return b2MakeProxy( &shape->capsule.center1, 2, shape->capsule.radius );+		case b2_circleShape:+			return b2MakeProxy( &shape->circle.center, 1, shape->circle.radius );+		case b2_polygonShape:+			return b2MakeProxy( shape->polygon.vertices, shape->polygon.count, shape->polygon.radius );+		case b2_segmentShape:+			return b2MakeProxy( &shape->segment.point1, 2, 0.0f );+		case b2_chainSegmentShape:+			return b2MakeProxy( &shape->chainSegment.segment.point1, 2, 0.0f );+		default:+		{+			B2_ASSERT( false );+			b2ShapeProxy empty = { 0 };+			return empty;+		}+	}+}++b2BodyId b2Shape_GetBody( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return b2MakeBodyId( world, shape->bodyId );+}++b2WorldId b2Shape_GetWorld( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	return (b2WorldId){ shapeId.world0 + 1, world->generation };+}++void b2Shape_SetUserData( b2ShapeId shapeId, void* userData )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	shape->userData = userData;+}++void* b2Shape_GetUserData( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->userData;+}++bool b2Shape_IsSensor( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->sensorIndex != B2_NULL_INDEX;+}++bool b2Shape_TestPoint( b2ShapeId shapeId, b2Pos point )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );++	b2WorldTransform transform = b2GetBodyTransform( world, shape->bodyId );+	b2Vec2 localPoint = b2InvTransformWorldPoint( transform, point );++	bool result;+	switch ( shape->type )+	{+		case b2_capsuleShape:+			result = b2PointInCapsule( &shape->capsule, localPoint );+			break;++		case b2_circleShape:+			result = b2PointInCircle( &shape->circle, localPoint );+			break;++		case b2_polygonShape:+			result = b2PointInPolygon( &shape->polygon, localPoint );+			break;++		default:+			result = false;+			break;+	}++	return result;+}++b2WorldCastOutput b2Shape_CastRay( b2ShapeId shapeId, b2Pos origin, b2Vec2 translation )+{+	B2_ASSERT( b2IsValidPos( origin ) );+	B2_ASSERT( b2IsValidVec2( translation ) );++	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );++	// Re-center on the origin so the cast runs in float precision+	b2Transform transform = b2ToRelativeTransform( b2GetBodyTransform( world, shape->bodyId ), origin );++	// The ray starts at the origin, so its origin in the re-centered frame is zero+	b2RayCastInput input = { b2Vec2_zero, translation, 1.0f };++	// Lift the re-centered float result back to a world position+	b2CastOutput local = b2RayCastShape( &input, shape, transform );+	b2WorldCastOutput output;+	output.normal = local.normal;+	output.point = b2OffsetPos( origin, local.point );+	output.fraction = local.fraction;+	output.iterations = local.iterations;+	output.hit = local.hit;++	return output;+}++void b2Shape_SetDensity( b2ShapeId shapeId, float density, bool updateBodyMass )+{+	B2_CHECK_INPUT( b2IsValidFloat( density ) && density >= 0.0f );++	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeSetDensity, shapeId, density, updateBodyMass );++	b2Shape* shape = b2GetShape( world, shapeId );+	if ( density == shape->density )+	{+		// early return to avoid expensive function+		return;+	}++	shape->density = density;++	if ( updateBodyMass == true )+	{+		b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+		b2UpdateBodyMassData( world, body );+	}+}++float b2Shape_GetDensity( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->density;+}++void b2Shape_SetFriction( b2ShapeId shapeId, float friction )+{+	B2_CHECK_INPUT( b2IsValidFloat( friction ) && friction >= 0.0f );++	b2World* world = b2GetWorld( shapeId.world0 );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, ShapeSetFriction, shapeId, friction );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->material.friction = friction;+}++float b2Shape_GetFriction( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->material.friction;+}++void b2Shape_SetRestitution( b2ShapeId shapeId, float restitution )+{+	B2_CHECK_INPUT( b2IsValidFloat( restitution ) && restitution >= 0.0f );++	b2World* world = b2GetWorld( shapeId.world0 );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, ShapeSetRestitution, shapeId, restitution );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->material.restitution = restitution;+}++float b2Shape_GetRestitution( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->material.restitution;+}++void b2Shape_SetUserMaterial( b2ShapeId shapeId, uint64_t material )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return;+	}++	B2_REC( world, ShapeSetUserMaterial, shapeId, material );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->material.userMaterialId = material;+}++uint64_t b2Shape_GetUserMaterial( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->material.userMaterialId;+}++b2SurfaceMaterial b2Shape_GetSurfaceMaterial( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->material;+}++void b2Shape_SetSurfaceMaterial( b2ShapeId shapeId, const b2SurfaceMaterial* surfaceMaterial )+{+	B2_CHECK_INPUT( b2IsValidMaterial( surfaceMaterial ) );++	b2World* world = b2GetWorld( shapeId.world0 );+	B2_REC( world, ShapeSetSurfaceMaterial, shapeId, *surfaceMaterial );+	b2Shape* shape = b2GetShape( world, shapeId );+	shape->material = *surfaceMaterial;+}++b2Filter b2Shape_GetFilter( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->filter;+}++static void b2ResetProxy( b2World* world, b2Shape* shape, bool destroyProxy )+{+	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );++	int shapeId = shape->id;++	// destroy all contacts associated with this shape+	int contactKey = body->headContactKey;+	while ( contactKey != B2_NULL_INDEX )+	{+		int contactId = contactKey >> 1;+		int edgeIndex = contactKey & 1;++		b2Contact* contact = b2Array_Get( world->contacts, contactId );+		contactKey = contact->edges[edgeIndex].nextKey;++		if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId )+		{+			b2DestroyContact( world, contact );+		}+	}++	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );+	if ( shape->proxyKey != B2_NULL_INDEX )+	{+		b2BodyType proxyType = B2_PROXY_TYPE( shape->proxyKey );+		b2AABB* fatAABB = world->fatAABBs.data + shapeId;+		b2UpdateShapeAABBs( shape, fatAABB, transform, proxyType );++		if ( destroyProxy )+		{+			b2BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey );++			bool forcePairCreation = true;+			shape->proxyKey = b2BroadPhase_CreateProxy( &world->broadPhase, proxyType, *fatAABB, shape->filter.categoryBits,+														shapeId, forcePairCreation );+		}+		else+		{+			b2BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, *fatAABB );+		}+	}+	else+	{+		b2BodyType proxyType = body->type;+		b2UpdateShapeAABBs( shape, world->fatAABBs.data + shapeId, transform, proxyType );+	}++	b2ValidateSolverSets( world );+}++void b2Shape_SetFilter( b2ShapeId shapeId, b2Filter filter )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeSetFilter, shapeId, filter );++	b2Shape* shape = b2GetShape( world, shapeId );+	if ( filter.maskBits == shape->filter.maskBits && filter.categoryBits == shape->filter.categoryBits &&+		 filter.groupIndex == shape->filter.groupIndex )+	{+		return;+	}++	// If the category bits change, I need to destroy the proxy because it affects the tree sorting.+	bool destroyProxy = filter.categoryBits != shape->filter.categoryBits;++	shape->filter = filter;++	b2ResetProxy( world, shape, destroyProxy );++	// note: this does not immediately update sensor overlaps. Instead sensor+	// overlaps are updated the next time step+}++void b2Shape_EnableSensorEvents( b2ShapeId shapeId, bool flag )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeEnableSensorEvents, shapeId, flag );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->enableSensorEvents = flag;+}++bool b2Shape_AreSensorEventsEnabled( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->enableSensorEvents;+}++void b2Shape_EnableContactEvents( b2ShapeId shapeId, bool flag )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeEnableContactEvents, shapeId, flag );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->enableContactEvents = flag;+}++bool b2Shape_AreContactEventsEnabled( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->enableContactEvents;+}++void b2Shape_EnablePreSolveEvents( b2ShapeId shapeId, bool flag )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeEnablePreSolveEvents, shapeId, flag );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->enablePreSolveEvents = flag;+}++bool b2Shape_ArePreSolveEventsEnabled( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->enablePreSolveEvents;+}++void b2Shape_EnableHitEvents( b2ShapeId shapeId, bool flag )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeEnableHitEvents, shapeId, flag );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->enableHitEvents = flag;+}++bool b2Shape_AreHitEventsEnabled( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->enableHitEvents;+}++b2ShapeType b2Shape_GetType( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->type;+}++b2Circle b2Shape_GetCircle( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	B2_ASSERT( shape->type == b2_circleShape );+	return shape->circle;+}++b2Segment b2Shape_GetSegment( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	B2_ASSERT( shape->type == b2_segmentShape );+	return shape->segment;+}++b2ChainSegment b2Shape_GetChainSegment( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	B2_ASSERT( shape->type == b2_chainSegmentShape );+	return shape->chainSegment;+}++b2Capsule b2Shape_GetCapsule( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	B2_ASSERT( shape->type == b2_capsuleShape );+	return shape->capsule;+}++b2Polygon b2Shape_GetPolygon( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	B2_ASSERT( shape->type == b2_polygonShape );+	return shape->polygon;+}++void b2Shape_SetCircle( b2ShapeId shapeId, const b2Circle* circle )+{+	B2_CHECK_INPUT( b2IsValidCircle( circle ) );++	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeSetCircle, shapeId, *circle );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->circle = *circle;+	shape->type = b2_circleShape;+	shape->aabbMargin = b2ComputeShapeMargin( shape );++	bool destroyProxy = true;+	b2ResetProxy( world, shape, destroyProxy );+}++void b2Shape_SetCapsule( b2ShapeId shapeId, const b2Capsule* capsule )+{+	B2_CHECK_INPUT( b2IsValidCapsule( capsule ) );++	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	float lengthSqr = b2DistanceSquared( capsule->center1, capsule->center2 );+	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )+	{+		return;+	}++	B2_REC( world, ShapeSetCapsule, shapeId, *capsule );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->capsule = *capsule;+	shape->type = b2_capsuleShape;+	shape->aabbMargin = b2ComputeShapeMargin( shape );++	bool destroyProxy = true;+	b2ResetProxy( world, shape, destroyProxy );+}++void b2Shape_SetSegment( b2ShapeId shapeId, const b2Segment* segment )+{+	B2_CHECK_INPUT( b2IsValidSegment( segment ) );++	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeSetSegment, shapeId, *segment );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->segment = *segment;+	shape->type = b2_segmentShape;+	shape->aabbMargin = b2ComputeShapeMargin( shape );++	bool destroyProxy = true;+	b2ResetProxy( world, shape, destroyProxy );+}++void b2Shape_SetPolygon( b2ShapeId shapeId, const b2Polygon* polygon )+{+	B2_CHECK_INPUT( b2IsValidPolygon( polygon ) );++	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeSetPolygon, shapeId, *polygon );++	b2Shape* shape = b2GetShape( world, shapeId );+	shape->polygon = *polygon;+	shape->type = b2_polygonShape;+	shape->aabbMargin = b2ComputeShapeMargin( shape );++	bool destroyProxy = true;+	b2ResetProxy( world, shape, destroyProxy );+}++void b2Shape_SetChainSegment( b2ShapeId shapeId, const b2ChainSegment* chainSegment )+{+	B2_CHECK_INPUT( b2IsValidChainSegment( chainSegment ) );++	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	b2Shape* shape = b2GetShape( world, shapeId );++	// Cannot modify a chain segment that has a parent chain shape+	if ( shape->type == b2_chainSegmentShape && shape->chainSegment.chainId != B2_NULL_INDEX )+	{+		B2_ASSERT( false );+		return;+	}++	float lengthSqr = b2DistanceSquared( chainSegment->segment.point1, chainSegment->segment.point2 );+	if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP )+	{+		return;+	}++	B2_REC( world, ShapeSetChainSegment, shapeId, *chainSegment );++	shape->chainSegment = *chainSegment;+	shape->chainSegment.chainId = B2_NULL_INDEX;+	shape->type = b2_chainSegmentShape;+	shape->aabbMargin = b2ComputeShapeMargin( shape );++	bool destroyProxy = true;+	b2ResetProxy( world, shape, destroyProxy );+}++b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	b2Shape* shape = b2GetShape( world, shapeId );+	if ( shape->type == b2_chainSegmentShape )+	{+		int chainId = shape->chainSegment.chainId;+		if ( chainId != B2_NULL_INDEX )+		{+			b2ChainShape* chain = b2Array_Get( world->chainShapes, chainId );+			b2ChainId id = { chainId + 1, shapeId.world0, chain->generation };+			return id;+		}+	}++	return (b2ChainId){ 0 };+}++int b2Shape_GetContactCapacity( b2ShapeId shapeId )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return 0;+	}++	b2Shape* shape = b2GetShape( world, shapeId );+	if ( shape->sensorIndex != B2_NULL_INDEX )+	{+		return 0;+	}++	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );++	// Conservative and fast+	return body->contactCount;+}++int b2Shape_GetContactData( b2ShapeId shapeId, b2ContactData* contactData, int capacity )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return 0;+	}++	b2Shape* shape = b2GetShape( world, shapeId );+	if ( shape->sensorIndex != B2_NULL_INDEX )+	{+		return 0;+	}++	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	int contactKey = body->headContactKey;+	int index = 0;+	while ( contactKey != B2_NULL_INDEX && index < capacity )+	{+		int contactId = contactKey >> 1;+		int edgeIndex = contactKey & 1;++		b2Contact* contact = b2Array_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 & b2_contactTouchingFlag ) != 0 )+		{+			b2Shape* shapeA = world->shapes.data + contact->shapeIdA;+			b2Shape* shapeB = world->shapes.data + contact->shapeIdB;++			contactData[index].contactId = (b2ContactId){ contact->contactId + 1, shapeId.world0, 0, contact->generation };+			contactData[index].shapeIdA = (b2ShapeId){ shapeA->id + 1, shapeId.world0, shapeA->generation };+			contactData[index].shapeIdB = (b2ShapeId){ shapeB->id + 1, shapeId.world0, shapeB->generation };++			b2ContactSim* contactSim = b2GetContactSim( world, contact );+			contactData[index].manifold = contactSim->manifold;+			index += 1;+		}++		contactKey = contact->edges[edgeIndex].nextKey;+	}++	B2_ASSERT( index <= capacity );++	return index;+}++int b2Shape_GetSensorCapacity( b2ShapeId shapeId )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return 0;+	}++	b2Shape* shape = b2GetShape( world, shapeId );+	if ( shape->sensorIndex == B2_NULL_INDEX )+	{+		return 0;+	}++	b2Sensor* sensor = b2Array_Get( world->sensors, shape->sensorIndex );+	return sensor->overlaps2.count;+}++int b2Shape_GetSensorData( b2ShapeId shapeId, b2ShapeId* visitorIds, int capacity )+{+	b2World* world = b2GetWorldLocked( shapeId.world0 );+	if ( world == NULL )+	{+		return 0;+	}++	b2Shape* shape = b2GetShape( world, shapeId );+	if ( shape->sensorIndex == B2_NULL_INDEX )+	{+		return 0;+	}++	b2Sensor* sensor = b2Array_Get( world->sensors, shape->sensorIndex );++	int count = b2MinInt( sensor->overlaps2.count, capacity );+	b2Visitor* refs = sensor->overlaps2.data;+	for ( int i = 0; i < count; ++i )+	{+		b2ShapeId visitorId = {+			.index1 = refs[i].shapeId + 1,+			.world0 = shapeId.world0,+			.generation = refs[i].generation,+		};++		visitorIds[i] = visitorId;+	}++	return count;+}++b2AABB b2Shape_GetBounds( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	if ( world == NULL )+	{+		return (b2AABB){ 0 };+	}++	b2Shape* shape = b2GetShape( world, shapeId );+	return shape->aabb;+}++b2MassData b2Shape_ComputeMassData( b2ShapeId shapeId )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	if ( world == NULL )+	{+		return (b2MassData){ 0 };+	}++	b2Shape* shape = b2GetShape( world, shapeId );+	return b2ComputeShapeMass( shape );+}++b2Pos b2Shape_GetClosestPoint( b2ShapeId shapeId, b2Pos target )+{+	b2World* world = b2GetWorld( shapeId.world0 );+	if ( world == NULL )+	{+		return b2Pos_zero;+	}++	b2Shape* shape = b2GetShape( world, shapeId );+	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );+	b2WorldTransform transform = b2GetBodyTransformQuick( world, body );++	// The target rides in as the frame of proxy B, so the relative pose is differenced in double+	// and the result stays exact far from the origin+	b2Vec2 zero = b2Vec2_zero;+	b2WorldTransform targetTransform = { target, b2Rot_identity };++	b2DistanceInput input;+	input.proxyA = b2MakeShapeDistanceProxy( shape );+	input.proxyB = b2MakeProxy( &zero, 1, 0.0f );+	input.transform = b2InvMulWorldTransforms( transform, targetTransform );+	input.useRadii = true;++	b2SimplexCache cache = { 0 };+	b2DistanceOutput output = b2ComputeShapeDistance( &input, &cache, NULL, 0 );++	return b2TransformWorldPoint( transform, output.pointA );+}++// 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 b2Shape_ApplyWind( b2ShapeId shapeId, b2Vec2 wind, float drag, float lift, bool wake )+{+	B2_CHECK_INPUT( b2IsValidVec2( wind ) );+	B2_CHECK_INPUT( b2IsValidFloat( drag ) );+	B2_CHECK_INPUT( b2IsValidFloat( lift ) );++	b2World* world = b2GetWorld( shapeId.world0 );+	if ( world == NULL )+	{+		return;+	}++	B2_REC( world, ShapeApplyWind, shapeId, wind, drag, lift, wake );++	b2Shape* shape = b2GetShape( world, shapeId );++	b2ShapeType shapeType = shape->type;+	if ( shapeType != b2_circleShape && shapeType != b2_capsuleShape && shapeType != b2_polygonShape )+	{+		return;+	}++	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );++	if ( body->type != b2_dynamicBody )+	{+		return;+	}++	if ( body->setIndex >= b2_firstSleepingSet && wake == false )+	{+		return;+	}++	b2BodySim* sim = b2GetBodySim( world, body );++	if ( body->setIndex != b2_awakeSet )+	{+		// Must wake for state to exist+		b2WakeBody( world, body );+	}++	B2_ASSERT( body->setIndex == b2_awakeSet );++	b2BodyState* state = b2GetBodyState( world, body );+	b2WorldTransform transform = sim->transform;++	float lengthUnits = b2GetLengthUnitsPerMeter();+	float volumeUnits = lengthUnits * lengthUnits * lengthUnits;++	// In 2D I'm assuming unit depth+	float airDensity = 1.2250f / ( volumeUnits );++	b2Vec2 force = { 0 };+	float torque = 0.0f;++	switch ( shape->type )+	{+		case b2_circleShape:+		{+			float radius = shape->circle.radius;+			b2Vec2 centroid = shape->localCentroid;+			b2Vec2 lever = b2RotateVector( transform.q, b2Sub( centroid, sim->localCenter ) );+			b2Vec2 shapeVelocity = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, lever ) );+			b2Vec2 relativeVelocity = b2MulSub( wind, drag, shapeVelocity );+			float speed;+			b2Vec2 direction = b2GetLengthAndNormalize( &speed, relativeVelocity );+			float projectedArea = 2.0f * radius;+			force = b2MulSV( 0.5f * airDensity * projectedArea * speed * speed, direction );+			torque = b2Cross( lever, force );+		}+		break;++		case b2_capsuleShape:+		{+			b2Vec2 centroid = shape->localCentroid;+			b2Vec2 lever = b2RotateVector( transform.q, b2Sub( centroid, sim->localCenter ) );+			b2Vec2 shapeVelocity = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, lever ) );+			b2Vec2 relativeVelocity = b2MulSub( wind, drag, shapeVelocity );+			float speed;+			b2Vec2 direction = b2GetLengthAndNormalize( &speed, relativeVelocity );++			b2Vec2 d = b2Sub( shape->capsule.center2, shape->capsule.center1 );+			d = b2RotateVector( transform.q, d );++			float radius = shape->capsule.radius;+			float projectedArea = 2.0f * radius + b2AbsFloat( b2Cross( d, direction ) );++			// Normal that opposes the wind+			b2Vec2 normal = b2LeftPerp( b2Normalize( d ) );+			normal = b2Dot( normal, direction ) > 0.0f ? b2Neg( normal ) : normal;++			// portion of wind that is perpendicular to surface+			b2Vec2 liftDirection = b2CrossSV( b2Cross( normal, direction ), direction );++			float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed;+			force = b2MulSV( forceMagnitude, b2MulAdd( direction, lift, liftDirection ) );++			b2Vec2 edgeLever = b2MulAdd( lever, radius, normal );+			torque = b2Cross( edgeLever, force );+		}+		break;++		case b2_polygonShape:+		{+			b2Vec2 centroid = shape->localCentroid;+			b2Vec2 lever = b2RotateVector( transform.q, b2Sub( centroid, sim->localCenter ) );+			b2Vec2 shapeVelocity = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, lever ) );+			b2Vec2 relativeVelocity = b2MulSub( wind, drag, shapeVelocity );+			float speed;+			b2Vec2 direction = b2GetLengthAndNormalize( &speed, relativeVelocity );++			// polygon radius is ignored for simplicity+			int count = shape->polygon.count;+			b2Vec2* vertices = shape->polygon.vertices;++			b2Vec2 v1 = vertices[count - 1];+			for ( int i = 0; i < count; ++i )+			{+				b2Vec2 v2 = vertices[i];+				b2Vec2 d = b2Sub( v2, v1 );+				b2Vec2 edgeCenter = b2Lerp( v1, v2, 0.5f );+				v1 = v2;++				d = b2RotateVector( transform.q, d );++				float projectedArea = b2Cross( d, direction );+				if ( projectedArea <= 0.0f )+				{+					// back facing+					continue;+				}++				b2Vec2 normal = b2RightPerp( b2Normalize( d ) );++				// portion of wind that is perpendicular to surface+				b2Vec2 liftDirection = b2CrossSV( b2Cross( normal, direction ), direction );++				float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed;+				b2Vec2 f = b2MulSV( forceMagnitude, b2MulAdd( direction, lift, liftDirection ) );++				b2Vec2 edgeLever = b2RotateVector( transform.q, b2Sub( edgeCenter, sim->localCenter ) );++				force = b2Add( force, f );+				torque += b2Cross( edgeLever, f );+			}+		}+		break;++		default:+			break;+	}++	sim->force = b2Add( sim->force, force );+	sim->torque += torque;+}
+ upstream/src/shape.h view
@@ -0,0 +1,134 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "container.h"++#include "box2d/types.h"++typedef struct b2BroadPhase b2BroadPhase;+typedef struct b2World b2World;++typedef struct b2Shape+{+	int id;+	int bodyId;+	int prevShapeId;+	int nextShapeId;+	int sensorIndex;+	b2ShapeType type;+	b2SurfaceMaterial material;+	float density;+	float aabbMargin;+	b2AABB aabb;+	b2Vec2 localCentroid;+	int proxyKey;++	b2Filter filter;+	void* userData;++	union+	{+		b2Capsule capsule;+		b2Circle circle;+		b2Polygon polygon;+		b2Segment segment;+		b2ChainSegment chainSegment;+	};++	uint16_t generation;+	bool enableSensorEvents;+	bool enableContactEvents;+	bool enableCustomFiltering;+	bool enableHitEvents;+	bool enablePreSolveEvents;+} b2Shape;++typedef struct b2ChainShape+{+	int id;+	int bodyId;+	int nextChainId;+	int segmentCount;+	uint16_t generation;+	int* shapeIndices;+} b2ChainShape;++typedef struct b2ShapeExtent+{+	float minExtent;+	float maxExtent;+} b2ShapeExtent;++// Sensors are shapes that live in the broad-phase but never have contacts.+// At the end of the time step all sensors are queried for overlap with any other shapes.+// Sensors ignore body type and sleeping.+// Sensors generate events when there is a new overlap or and overlap disappears.+// The sensor overlaps don't get cleared until the next time step regardless of the overlapped+// shapes being destroyed.+// When a sensor is destroyed.+typedef struct+{+	b2Array( int ) overlaps;+} b2SensorOverlaps;++void b2CreateShapeProxy( b2World* world, b2Shape* shape, b2BodyType type, b2WorldTransform transform, bool forcePairCreation );+void b2DestroyShapeProxy( b2Shape* shape, b2BroadPhase* bp );++void b2FreeChainData( b2ChainShape* chain );++b2MassData b2ComputeShapeMass( const b2Shape* shape );+b2ShapeExtent b2ComputeShapeExtent( const b2Shape* shape, b2Vec2 localCenter );+b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2WorldTransform transform );++// Conservative world AABB for a shape, inflated by extra margin. In large world mode this is+// computed in double and rounded outward so the inflation is not lost far from the origin.+b2AABB b2ComputeFatShapeAABB( const b2Shape* shape, b2WorldTransform transform, float extra );+b2Vec2 b2GetShapeCentroid( const b2Shape* shape );+float b2GetShapePerimeter( const b2Shape* shape );+float b2GetShapeProjectedPerimeter( const b2Shape* shape, b2Vec2 line );++b2ShapeProxy b2MakeShapeDistanceProxy( const b2Shape* shape );++b2CastOutput b2RayCastShape( const b2RayCastInput* input, const b2Shape* shape, b2Transform transform );+b2CastOutput b2ShapeCastShape( const b2ShapeCastInput* input, const b2Shape* shape, b2Transform transform );++b2PlaneResult b2CollideMoverAndCircle( const b2Capsule* mover, const b2Circle* shape );+b2PlaneResult b2CollideMoverAndCapsule( const b2Capsule* mover, const b2Capsule* shape );+b2PlaneResult b2CollideMoverAndPolygon( const b2Capsule* mover, const b2Polygon* shape );+b2PlaneResult b2CollideMoverAndSegment( const b2Capsule* mover, const b2Segment* shape );+b2PlaneResult b2CollideMover( const b2Capsule* mover, const b2Shape* shape, b2Transform transform );++static inline float b2GetShapeRadius( const b2Shape* shape )+{+	switch ( shape->type )+	{+		case b2_capsuleShape:+			return shape->capsule.radius;+		case b2_circleShape:+			return shape->circle.radius;+		case b2_polygonShape:+			return shape->polygon.radius;+		default:+			return 0.0f;+	}+}++static inline bool b2ShouldShapesCollide( b2Filter filterA, b2Filter 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 b2ShouldQueryCollide( b2Filter shapeFilter, b2QueryFilter queryFilter )+{+	return ( shapeFilter.categoryBits & queryFilter.maskBits ) != 0 && ( shapeFilter.maskBits & queryFilter.categoryBits ) != 0;+}++b2DeclareArray( b2Shape );+b2DeclareArray( b2ChainShape );
+ upstream/src/simd.c view
@@ -0,0 +1,112 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "simd.h"++#include "platform.h"++#include <stdbool.h>+#include <stdint.h>++static b2AtomicInt b2_simdWidth;++#if defined( B2_SIMD_HAS_WIDTH_8 )++#if defined( _MSC_VER )+#include <intrin.h>+#endif++// This code enables runtime dispatch to AVX2.+// Inspired by https://github.com/simdjson/simdjson/blob/master/src/internal/isadetection.h++static void b2CpuId( unsigned int leaf, unsigned int subLeaf, unsigned int registers[4] )+{+#if defined( _MSC_VER )+	int info[4];+	__cpuidex( info, (int)leaf, (int)subLeaf );+	registers[0] = (unsigned int)info[0];+	registers[1] = (unsigned int)info[1];+	registers[2] = (unsigned int)info[2];+	registers[3] = (unsigned int)info[3];+#else+	// clang cpuid.h can give the rbx swap register the same register as the leaf input, clobbering rbx+	__asm__ volatile( "cpuid"+					  : "=a"( registers[0] ), "=b"( registers[1] ), "=c"( registers[2] ), "=d"( registers[3] )+					  : "a"( leaf ), "c"( subLeaf ) );+#endif+}++static uint64_t b2GetExtendedControlRegister( void )+{+#if defined( _MSC_VER ) && !defined( __clang__ )+	return _xgetbv( 0 );+#else+	uint32_t low, high;+	__asm__ volatile( "xgetbv" : "=a"( low ), "=d"( high ) : "c"( 0 ) );+	return ( (uint64_t)high << 32 ) | low;+#endif+}++static bool b2HasAVX2( void )+{+	unsigned int registers[4];+	b2CpuId( 0, 0, registers );+	if ( registers[0] < 7 )+	{+		return false;+	}++	b2CpuId( 1, 0, registers );+	unsigned int osxsaveAndAvx = ( 1u << 27 ) | ( 1u << 28 );+	if ( ( registers[2] & osxsaveAndAvx ) != osxsaveAndAvx )+	{+		return false;+	}++	if ( ( b2GetExtendedControlRegister() & 6 ) != 6 )+	{+		return false;+	}++	b2CpuId( 7, 0, registers );+	return ( registers[1] & ( 1u << 5 ) ) != 0;+}++#endif++static int b2DetectSIMDWidth( void )+{+#if defined( B2_SIMD_HAS_WIDTH_8 )+	return b2HasAVX2() ? 8 : 4;+#else+	return 4;+#endif+}++int b2GetSIMDWidth( void )+{+	int width = b2AtomicLoadInt( &b2_simdWidth );+	if ( width == 0 )+	{+		width = b2DetectSIMDWidth();+		b2AtomicStoreInt( &b2_simdWidth, width );+	}++	return width;+}++bool b2IsAVX2Available( void )+{+	return b2DetectSIMDWidth() == 8;+}++void b2SetSIMDWidth( int width )+{+	B2_ASSERT( width == 0 || width == 4 || width == 8 );+	if ( width == 8 && b2DetectSIMDWidth() != 8 )+	{+		width = 4;+	}++	b2AtomicStoreInt( &b2_simdWidth, width );+}
+ upstream/src/simd.h view
@@ -0,0 +1,297 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "core.h"+#include "dynamic_tree.h"++#include "box2d/collision.h"+#include "box2d/math_functions.h"++#if defined( B2_SIMD_NEON )++#include <arm_neon.h>++typedef float32x4_t b2AABBV;++B2_FORCE_INLINE b2AABBV b2LoadAABBV( const b2AABB* aabb )+{+	return vld1q_f32( &aabb->lowerBound.x );+}++B2_FORCE_INLINE bool b2OverlapNode( b2AABBV av, const b2TreeNode* node )+{+	// [lower.x lower.y upper.x upper.y]+	float32x4_t bv = vld1q_f32( &node->aabb.lowerBound.x );++	// [alx aly blx bly]+	float32x4_t t1 = vcombine_f32( vget_low_f32( av ), vget_low_f32( bv ) );++	// [bux buy aux auy]+	float32x4_t t2 = vcombine_f32( vget_high_f32( bv ), vget_high_f32( av ) );++	return vminvq_u32( vcleq_f32( t1, t2 ) ) != 0;+}++B2_FORCE_INLINE bool b2OverlapV( const b2AABB* a, const b2AABB* b )+{+	// [lower.x lower.y upper.x upper.y]+	float32x4_t av = vld1q_f32( &a->lowerBound.x );+	float32x4_t bv = vld1q_f32( &b->lowerBound.x );++	// [alx aly blx bly]+	float32x4_t t1 = vcombine_f32( vget_low_f32( av ), vget_low_f32( bv ) );++	// [bux buy aux auy]+	float32x4_t t2 = vcombine_f32( vget_high_f32( bv ), vget_high_f32( av ) );++	return vminvq_u32( vcleq_f32( t1, t2 ) ) != 0;+}++B2_FORCE_INLINE b2AABB b2UnionV( b2AABB a, b2AABB b )+{+	float32x4_t b1 = vld1q_f32( &a.lowerBound.x );+	float32x4_t b2 = vld1q_f32( &b.lowerBound.x );+	float32x4_t lower = vminq_f32( b1, b2 );+	float32x4_t upper = vmaxq_f32( b1, b2 );+	b2AABB result;+	vst1q_f32( &result.lowerBound.x, vcombine_f32( vget_low_f32( lower ), vget_high_f32( upper ) ) );+	return result;+}++B2_FORCE_INLINE b2AABBV b2UnionPairV( const b2TreeNode* pair )+{+	float32x4_t b1 = vld1q_f32( &pair[0].aabb.lowerBound.x );+	float32x4_t b2 = vld1q_f32( &pair[1].aabb.lowerBound.x );+	return vcombine_f32( vget_low_f32( vminq_f32( b1, b2 ) ), vget_high_f32( vmaxq_f32( b1, b2 ) ) );+}++B2_FORCE_INLINE void b2StoreAABBV( b2AABB* aabb, b2AABBV value, bool condition )+{+	uint32x4_t mask = vdupq_n_u32( condition ? 0xFFFFFFFFu : 0u );+	float32x4_t old = vld1q_f32( &aabb->lowerBound.x );+	vst1q_f32( &aabb->lowerBound.x, vbslq_f32( mask, value, old ) );+}++B2_FORCE_INLINE int b2LaneMaskV( uint32x4_t condition )+{+	static const uint32_t laneBits[4] = { 1, 2, 4, 8 };+	return (int)vaddvq_u32( vandq_u32( condition, vld1q_u32( laneBits ) ) );+}++B2_FORCE_INLINE int b2OverlapPairMask( const b2TreeNode* pairA, const b2TreeNode* pairB )+{+	float32x4_t a0 = vld1q_f32( &pairA[0].aabb.lowerBound.x );+	float32x4_t a1 = vld1q_f32( &pairA[1].aabb.lowerBound.x );+	float32x4_t b0 = vld1q_f32( &pairB[0].aabb.lowerBound.x );+	float32x4_t b1 = vld1q_f32( &pairB[1].aabb.lowerBound.x );++	float32x4_t bLower = vzip1q_f32( b0, b1 );+	float32x4_t bUpper = vzip2q_f32( b0, b1 );+	float32x4_t bLx = vcombine_f32( vget_low_f32( bLower ), vget_low_f32( bLower ) );+	float32x4_t bLy = vcombine_f32( vget_high_f32( bLower ), vget_high_f32( bLower ) );+	float32x4_t bUx = vcombine_f32( vget_low_f32( bUpper ), vget_low_f32( bUpper ) );+	float32x4_t bUy = vcombine_f32( vget_high_f32( bUpper ), vget_high_f32( bUpper ) );++	float32x4_t aLx = vcombine_f32( vdup_laneq_f32( a0, 0 ), vdup_laneq_f32( a1, 0 ) );+	float32x4_t aLy = vcombine_f32( vdup_laneq_f32( a0, 1 ), vdup_laneq_f32( a1, 1 ) );+	float32x4_t aUx = vcombine_f32( vdup_laneq_f32( a0, 2 ), vdup_laneq_f32( a1, 2 ) );+	float32x4_t aUy = vcombine_f32( vdup_laneq_f32( a0, 3 ), vdup_laneq_f32( a1, 3 ) );++	uint32x4_t x = vandq_u32( vcleq_f32( aLx, bUx ), vcleq_f32( bLx, aUx ) );+	uint32x4_t y = vandq_u32( vcleq_f32( aLy, bUy ), vcleq_f32( bLy, aUy ) );+	return b2LaneMaskV( vandq_u32( x, y ) );+}++B2_FORCE_INLINE int b2OverlapNodePairMask( b2AABBV av, const b2TreeNode* pair )+{+	float32x4_t b0 = vld1q_f32( &pair[0].aabb.lowerBound.x );+	float32x4_t b1 = vld1q_f32( &pair[1].aabb.lowerBound.x );++	float32x4_t bLower = vzip1q_f32( b0, b1 );+	float32x4_t bUpper = vzip2q_f32( b0, b1 );+	float32x4_t aLower = vzip1q_f32( av, av );+	float32x4_t aUpper = vzip2q_f32( av, av );++	int mask = b2LaneMaskV( vandq_u32( vcleq_f32( aLower, bUpper ), vcleq_f32( bLower, aUpper ) ) );+	return mask & ( mask >> 2 ) & 3;+}++#elif defined( B2_SIMD_SSE2 ) || defined( B2_SIMD_AVX2 )++#include <emmintrin.h>++typedef __m128 b2AABBV;++B2_FORCE_INLINE b2AABBV b2LoadAABBV( const b2AABB* aabb )+{+	return _mm_loadu_ps( &aabb->lowerBound.x );+}++// Passing the tree node rather than the AABB avoids a stack copy.+// todo confirm assembly+B2_FORCE_INLINE bool b2OverlapNode( b2AABBV av, const b2TreeNode* node )+{+	// Unaligned load+	// [lower.x lower.y upper.x upper.y]+	__m128 bv = _mm_loadu_ps( &node->aabb.lowerBound.x );++	// [alx aly blx bly]+	__m128 t1 = _mm_movelh_ps( av, bv );++	// [bux buy aux auy]+	__m128 t2 = _mm_movehl_ps( av, bv );++	return _mm_movemask_ps( _mm_cmple_ps( t1, t2 ) ) == 0xF;+}++B2_FORCE_INLINE bool b2OverlapV( const b2AABB* a, const b2AABB* b )+{+	// Unaligned load+	// [lower.x lower.y upper.x upper.y]+	__m128 av = _mm_loadu_ps( &a->lowerBound.x );+	__m128 bv = _mm_loadu_ps( &b->lowerBound.x );++	// [alx aly blx bly]+	__m128 t1 = _mm_movelh_ps( av, bv );++	// [bux buy aux auy]+	__m128 t2 = _mm_movehl_ps( av, bv );++	return _mm_movemask_ps( _mm_cmple_ps( t1, t2 ) ) == 0xF;+}++B2_FORCE_INLINE b2AABB b2UnionV( b2AABB a, b2AABB b )+{+	// Unaligned load+	// [lower.x lower.y upper.x upper.y]+	__m128 b1 = _mm_loadu_ps( &a.lowerBound.x );+	__m128 b2 = _mm_loadu_ps( &b.lowerBound.x );+	__m128 lower = _mm_min_ps( b1, b2 );+	__m128 upper = _mm_max_ps( b1, b2 );+	__m128 c = _mm_shuffle_ps( lower, upper, _MM_SHUFFLE( 3, 2, 1, 0 ) );+	b2AABB result = { 0 };+	_mm_storeu_ps( &result.lowerBound.x, c );+	return result;+}++B2_FORCE_INLINE b2AABBV b2UnionPairV( const b2TreeNode* pair )+{+	__m128 b1 = _mm_loadu_ps( &pair[0].aabb.lowerBound.x );+	__m128 b2 = _mm_loadu_ps( &pair[1].aabb.lowerBound.x );+	__m128 lower = _mm_min_ps( b1, b2 );+	__m128 upper = _mm_max_ps( b1, b2 );+	return _mm_shuffle_ps( lower, upper, _MM_SHUFFLE( 3, 2, 1, 0 ) );+}++// Conditionally store the value. This is optimized for tree refitting.+B2_FORCE_INLINE void b2StoreAABBV( b2AABB* aabb, b2AABBV value, bool condition )+{+	__m128 mask = _mm_castsi128_ps( _mm_set1_epi32( condition ? -1 : 0 ) );+	__m128 old = _mm_loadu_ps( &aabb->lowerBound.x );++	// blend+	_mm_storeu_ps( &aabb->lowerBound.x, _mm_or_ps( _mm_and_ps( mask, value ), _mm_andnot_ps( mask, old ) ) );+}++B2_FORCE_INLINE int b2OverlapPairMask( const b2TreeNode* pairA, const b2TreeNode* pairB )+{+	__m128 a0 = _mm_loadu_ps( &pairA[0].aabb.lowerBound.x );+	__m128 a1 = _mm_loadu_ps( &pairA[1].aabb.lowerBound.x );+	__m128 b0 = _mm_loadu_ps( &pairB[0].aabb.lowerBound.x );+	__m128 b1 = _mm_loadu_ps( &pairB[1].aabb.lowerBound.x );++	__m128 bLower = _mm_unpacklo_ps( b0, b1 );+	__m128 bUpper = _mm_unpackhi_ps( b0, b1 );+	__m128 bLx = _mm_movelh_ps( bLower, bLower );+	__m128 bLy = _mm_movehl_ps( bLower, bLower );+	__m128 bUx = _mm_movelh_ps( bUpper, bUpper );+	__m128 bUy = _mm_movehl_ps( bUpper, bUpper );++	__m128 aLx = _mm_shuffle_ps( a0, a1, _MM_SHUFFLE( 0, 0, 0, 0 ) );+	__m128 aLy = _mm_shuffle_ps( a0, a1, _MM_SHUFFLE( 1, 1, 1, 1 ) );+	__m128 aUx = _mm_shuffle_ps( a0, a1, _MM_SHUFFLE( 2, 2, 2, 2 ) );+	__m128 aUy = _mm_shuffle_ps( a0, a1, _MM_SHUFFLE( 3, 3, 3, 3 ) );++	__m128 x = _mm_and_ps( _mm_cmple_ps( aLx, bUx ), _mm_cmple_ps( bLx, aUx ) );+	__m128 y = _mm_and_ps( _mm_cmple_ps( aLy, bUy ), _mm_cmple_ps( bLy, aUy ) );+	return _mm_movemask_ps( _mm_and_ps( x, y ) );+}++B2_FORCE_INLINE int b2OverlapNodePairMask( b2AABBV av, const b2TreeNode* pair )+{+	__m128 b0 = _mm_loadu_ps( &pair[0].aabb.lowerBound.x );+	__m128 b1 = _mm_loadu_ps( &pair[1].aabb.lowerBound.x );++	__m128 bLower = _mm_unpacklo_ps( b0, b1 );+	__m128 bUpper = _mm_unpackhi_ps( b0, b1 );+	__m128 aLower = _mm_unpacklo_ps( av, av );+	__m128 aUpper = _mm_unpackhi_ps( av, av );++	int mask = _mm_movemask_ps( _mm_and_ps( _mm_cmple_ps( aLower, bUpper ), _mm_cmple_ps( bLower, aUpper ) ) );+	return mask & ( mask >> 2 ) & 3;+}++#else++typedef b2AABB b2AABBV;++B2_FORCE_INLINE b2AABBV b2LoadAABBV( const b2AABB* aabb )+{+	return *aabb;+}++B2_FORCE_INLINE bool b2OverlapNode( b2AABBV av, const b2TreeNode* node )+{+	const b2AABB* bv = &node->aabb;+	return av.lowerBound.x <= bv->upperBound.x && av.lowerBound.y <= bv->upperBound.y && bv->lowerBound.x <= av.upperBound.x &&+		   bv->lowerBound.y <= av.upperBound.y;+}++B2_FORCE_INLINE bool b2OverlapV( const b2AABB* a, const b2AABB* b )+{+	return a->lowerBound.x <= b->upperBound.x && a->lowerBound.y <= b->upperBound.y && b->lowerBound.x <= a->upperBound.x &&+		   b->lowerBound.y <= a->upperBound.y;+}++B2_FORCE_INLINE b2AABB b2UnionV( b2AABB a, b2AABB b )+{+	return b2AABB_Union( a, b );+}++B2_FORCE_INLINE b2AABBV b2UnionPairV( const b2TreeNode* pair )+{+	return b2AABB_Union( pair[0].aabb, pair[1].aabb );+}++B2_FORCE_INLINE void b2StoreAABBV( b2AABB* aabb, b2AABBV value, bool condition )+{+	if ( condition )+	{+		*aabb = value;+	}+}++B2_FORCE_INLINE int b2OverlapPairMask( const b2TreeNode* pairA, const b2TreeNode* pairB )+{+	int mask = 0;+	mask |= b2OverlapV( &pairA[0].aabb, &pairB[0].aabb ) ? 1 : 0;+	mask |= b2OverlapV( &pairA[0].aabb, &pairB[1].aabb ) ? 2 : 0;+	mask |= b2OverlapV( &pairA[1].aabb, &pairB[0].aabb ) ? 4 : 0;+	mask |= b2OverlapV( &pairA[1].aabb, &pairB[1].aabb ) ? 8 : 0;+	return mask;+}++B2_FORCE_INLINE int b2OverlapNodePairMask( b2AABBV av, const b2TreeNode* pair )+{+	int mask = 0;+	mask |= b2OverlapNode( av, pair + 0 ) ? 1 : 0;+	mask |= b2OverlapNode( av, pair + 1 ) ? 2 : 0;+	return mask;+}++#endif++int b2GetSIMDWidth( void );+void b2SetSIMDWidth( int width );
+ upstream/src/snapshot.c view
@@ -0,0 +1,1203 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#include "snapshot.h"++#include "bitset.h"+#include "body.h"+#include "broad_phase.h"+#include "constraint_graph.h"+#include "contact.h"+#include "container.h"+#include "core.h"+#include "dynamic_tree.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 "box2d/box2d.h"+#include "box2d/collision.h"+#include "box2d/types.h"++#include <string.h>++// Snapshot image magic and version+#define B2_SNAP_MAGIC 0x32534E42u // 'BNS2'++// Bump this if any of the data structures below get modified. The layout hash only catches+// size changes, a same-size reinterpretation like the contact cache reshape needs this bump.+#define B2_SNAP_VERSION 13u // extend b2Capacity++// Header flag bits+#define B2_SNAP_FLAG_VALIDATION 0x1u // image was built with validation, only used for diagnostics+#define B2_SNAP_FLAG_DOUBLE_PRECISION 0x2u // image was built with double precision world positions++// Layout hash seeds from all structs we memcpy, plus key constants.+// Changing any struct or constant updates the hash. Re-purposing or swapping+// fields might not change the hash+static uint32_t b2ComputeLayoutHash( void )+{+	// FNV-1a hash+	uint32_t h = 2166136261u;+#define MIX( x )                                                                                   \+	h ^= (uint32_t)( x );                                                                          \+	h *= 16777619u;+	MIX( sizeof( b2Body ) )+	MIX( sizeof( b2BodySim ) )+	MIX( sizeof( b2BodyState ) )+	MIX( sizeof( b2Shape ) )+	MIX( sizeof( b2ChainShape ) )+	MIX( sizeof( b2Contact ) )+	MIX( sizeof( b2ContactSim ) )+	MIX( sizeof( b2Joint ) )+	MIX( sizeof( b2JointSim ) )+	MIX( sizeof( b2Island ) )+	MIX( sizeof( b2IslandSim ) )+	MIX( sizeof( b2ContactLink ) )+	MIX( sizeof( b2JointLink ) )+	MIX( sizeof( b2Sensor ) )+	MIX( sizeof( b2Visitor ) )+	MIX( sizeof( b2SolverSet ) )+	MIX( sizeof( b2GraphColor ) )+	MIX( sizeof( b2DynamicTree ) )+	MIX( sizeof( b2TreeNode ) )+	MIX( sizeof( b2TreeProxy ) )+	MIX( sizeof( b2SetItem ) )+	MIX( sizeof( b2IdPool ) )+	MIX( sizeof( b2SurfaceMaterial ) )+	MIX( B2_GRAPH_COLOR_COUNT )+	MIX( b2_bodyTypeCount )+	MIX( sizeof( void* ) )+#undef MIX+	return h;+}++// Snapshot image header, written at offset 0+typedef struct b2SnapHeader+{+	uint32_t magic;+	uint32_t version;+	uint32_t layoutHash;+	uint32_t flags; // B2_SNAP_FLAG_*, keeps 16-byte alignment+} b2SnapHeader;++// Compile-time tripwires for the structs serialized field-by-field. If one fires, a field was added+// or reordered: resync the matching code in b2SerializeWorld / b2DeserializeIntoShell and bump+// B2_SNAP_VERSION.+#if INTPTR_MAX == INT64_MAX+_Static_assert( sizeof( b2ChainShape ) == 32,+				"b2ChainShape layout changed; resync snapshot chain serialization" );+_Static_assert( sizeof( b2Sensor ) == 56,+				"b2Sensor layout changed; resync snapshot sensor serialization" );+_Static_assert( sizeof( b2Island ) == 64,+				"b2Island layout changed; resync snapshot island serialization" );+#endif++// Bounds-checked read cursor, mirrors b2RecReader discipline+typedef struct b2SnapReader+{+	const uint8_t* data;+	int cursor;+	int size;+	bool ok;+} b2SnapReader;++static void b2SnapRCheck( b2SnapReader* r, int need )+{+	if ( need < 0 || (int64_t)r->cursor + (int64_t)need > (int64_t)r->size )+	{+		r->ok = false;+	}+}++static void b2SnapR_Bytes( b2SnapReader* r, void* dst, int n )+{+	b2SnapRCheck( r, n );+	if ( !r->ok )+	{+		return;+	}+	memcpy( dst, r->data + r->cursor, n );+	r->cursor += n;+}++static int b2SnapR_I32( b2SnapReader* r )+{+	int32_t v = 0;+	b2SnapR_Bytes( r, &v, 4 );+	return (int)v;+}++static uint32_t b2SnapR_U32( b2SnapReader* r )+{+	uint32_t v = 0;+	b2SnapR_Bytes( r, &v, 4 );+	return v;+}++static void b2SnapW_I32( b2RecBuffer* buf, int v )+{+	int32_t w = (int32_t)v;+	b2RecBufAppend( buf, &w, 4 );+}++static void b2SnapW_U32( b2RecBuffer* buf, uint32_t v )+{+	b2RecBufAppend( buf, &v, 4 );+}++static void b2SnapW_Bytes( b2RecBuffer* buf, const void* src, int n )+{+	b2RecBufAppend( buf, src, n );+}++// Reject a count read from the image before it reaches an allocation or memset. count must be non+// negative, its in-memory footprint must fit in int, and the stream must hold at least+// minStreamBytes per element. A corrupt or truncated image then fails.+static bool b2SnapCheckCount( const b2SnapReader* r, int count, int memSize, int minStreamBytes )+{+	if ( count < 0 || memSize < 0 || minStreamBytes < 0 )+	{+		return false;+	}+	if ( memSize > 0 && count > INT32_MAX / memSize )+	{+		return false;+	}+	int64_t remaining = (int64_t)r->size - (int64_t)r->cursor;+	return (int64_t)count * (int64_t)minStreamBytes <= remaining;+}++// Serialize a POD array: count then raw bytes+#define b2SerPodArray( buf, arr )                                                                  \+	do                                                                                             \+	{                                                                                              \+		b2SnapW_I32( buf, ( arr ).count );                                                         \+		if ( ( arr ).count > 0 )                                                                   \+		{                                                                                          \+			b2SnapW_Bytes( buf, ( arr ).data, ( arr ).count * (int)sizeof( *( arr ).data ) );      \+		}                                                                                          \+	}                                                                                              \+	while ( 0 )++// Serialize a sparse struct array whose element carries a host userData pointer. userData is host+// wiring, not simulation state, so write it as NULL: images stay reproducible and never persist+// host addresses across a save. On restore the field reads back NULL.+#define b2SerSimArray( buf, arr, type )                                                            \+	do                                                                                             \+	{                                                                                              \+		b2SnapW_I32( buf, ( arr ).count );                                                         \+		for ( int slot = 0; slot < ( arr ).count; ++slot )                                         \+		{                                                                                          \+			type elem = ( arr ).data[slot];                                                        \+			elem.userData = NULL;                                                                  \+			b2SnapW_Bytes( buf, &elem, (int)sizeof( type ) );                                      \+		}                                                                                          \+	}                                                                                              \+	while ( 0 )++// Deserialize a POD array: validate count, resize, then memcpy+#define b2DesPodArray( r, arr )                                                                    \+	do                                                                                             \+	{                                                                                              \+		int cnt = b2SnapR_I32( r );                                                                \+		int elemSize = (int)sizeof( *( arr ).data );                                               \+		if ( ( r )->ok && b2SnapCheckCount( r, cnt, elemSize, elemSize ) == false )                \+		{                                                                                          \+			( r )->ok = false;                                                                     \+		}                                                                                          \+		if ( ( r )->ok && cnt > 0 )                                                                \+		{                                                                                          \+			b2Array_Resize( arr, cnt );                                                            \+			b2SnapR_Bytes( r, ( arr ).data, cnt * elemSize );                                      \+		}                                                                                          \+		else if ( ( r )->ok )                                                                      \+		{                                                                                          \+			( arr ).count = 0;                                                                     \+		}                                                                                          \+	}                                                                                              \+	while ( 0 )++// Id pool: nextIndex + freeArray+static void b2SerIdPool( b2RecBuffer* buf, const b2IdPool* pool )+{+	b2SnapW_I32( buf, pool->nextIndex );+	b2SerPodArray( buf, pool->freeArray );+}++static void b2DesIdPool( b2SnapReader* r, b2IdPool* pool )+{+	pool->nextIndex = b2SnapR_I32( r );+	b2DesPodArray( r, pool->freeArray );+}++// BitSet: blockCount + raw uint64_t words+static void b2SerBitSet( b2RecBuffer* buf, const b2BitSet* bs )+{+	b2SnapW_U32( buf, bs->blockCount );+	if ( bs->blockCount > 0 )+	{+		b2SnapW_Bytes( buf, bs->bits, (int)( bs->blockCount * sizeof( uint64_t ) ) );+	}+}++// Restore a bitset, leak-clean: destroy the existing one, alloc fresh at exact blockCount capacity.+// Always keep bits non-NULL, matching b2CreateBitSet, so a later b2GrowBitSet on a count-0 bitset+// has a buffer to grow from. An empty color bitset serializes as 0 blocks but must restore usable.+static void b2DesBitSet( b2SnapReader* r, b2BitSet* bs )+{+	uint32_t blockCount = b2SnapR_U32( r );+	if ( r->ok && b2SnapCheckCount( r, (int)blockCount, (int)sizeof( uint64_t ),+									(int)sizeof( uint64_t ) ) == false )+	{+		r->ok = false;+	}+	b2DestroyBitSet( bs );+	if ( !r->ok )+	{+		return;+	}+	uint32_t blockCapacity = blockCount > 0 ? blockCount : 1;+	bs->bits = b2Alloc( blockCapacity * sizeof( uint64_t ) );+	memset( bs->bits, 0, blockCapacity * sizeof( uint64_t ) );+	bs->blockCapacity = blockCapacity;+	bs->blockCount = blockCount;+	if ( blockCount > 0 )+	{+		b2SnapR_Bytes( r, bs->bits, (int)( blockCount * sizeof( uint64_t ) ) );+	}+}++// HashSet (pairSet): raw items at full capacity, probe order depends on it+static void b2SerHashSet( b2RecBuffer* buf, const b2HashSet* hs )+{+	b2SnapW_U32( buf, hs->capacity );+	b2SnapW_U32( buf, hs->count );+	if ( hs->capacity > 0 )+	{+		b2SnapW_Bytes( buf, hs->items, (int)( hs->capacity * sizeof( b2SetItem ) ) );+	}+}++static void b2DesHashSet( b2SnapReader* r, b2HashSet* hs )+{+	uint32_t cap = b2SnapR_U32( r );+	uint32_t cnt = b2SnapR_U32( r );+	// Probing masks with capacity-1, so capacity must be a power of two and count can't exceed it.+	// (cap & (cap-1)) == 0 also accepts 0, which the empty branch handles.+	bool valid =+		b2SnapCheckCount( r, (int)cap, (int)sizeof( b2SetItem ), (int)sizeof( b2SetItem ) ) &&+		( cap & ( cap - 1 ) ) == 0 && cnt <= cap;+	if ( r->ok && valid == false )+	{+		r->ok = false;+	}+	// Destroy the fresh empty set the shell gave us+	b2DestroySet( hs );+	if ( !r->ok )+	{+		return;+	}+	if ( cap > 0 )+	{+		hs->items = b2Alloc( cap * sizeof( b2SetItem ) );+		hs->capacity = cap;+		hs->count = cnt;+		b2SnapR_Bytes( r, hs->items, (int)( cap * sizeof( b2SetItem ) ) );+	}+	else+	{+		hs->items = NULL;+		hs->capacity = 0;+		hs->count = 0;+	}+}++static void b2SerTree( b2RecBuffer* buf, const b2DynamicTree* tree )+{+	b2SnapW_I32( buf, tree->nodeEnd );+	b2SnapW_I32( buf, tree->nodeCapacity );+	b2SnapW_I32( buf, tree->pairFreeList );+	b2SnapW_I32( buf, tree->proxyCount );+	b2SnapW_I32( buf, tree->proxyCapacity );+	b2SnapW_I32( buf, tree->proxyFreeList );+	b2SnapW_I32( buf, tree->dfsOrdered ? 1 : 0 );++	b2SnapW_Bytes( buf, tree->nodes, tree->nodeEnd * (int)sizeof( b2TreeNode ) );+	b2SnapW_Bytes( buf, tree->parents, tree->nodeEnd * (int)sizeof( int32_t ) );+	b2SnapW_Bytes( buf, tree->proxies, tree->proxyCapacity * (int)sizeof( b2TreeProxy ) );+}++static void b2DesTree( b2SnapReader* r, b2DynamicTree* tree )+{+	int nodeEnd = b2SnapR_I32( r );+	int nodeCapacity = b2SnapR_I32( r );+	int pairFreeList = b2SnapR_I32( r );+	int proxyCount = b2SnapR_I32( r );+	int proxyCapacity = b2SnapR_I32( r );+	int proxyFreeList = b2SnapR_I32( r );+	int ordered = b2SnapR_I32( r );++	if ( r->ok && ( nodeEnd < 2 || ( nodeEnd & 1 ) != 0 || nodeCapacity < nodeEnd ||+					proxyCount < 0 || proxyCapacity < 1 || proxyCapacity < proxyCount ) )+	{+		r->ok = false;+	}++	// Stream bounds against what is actually written+	if ( r->ok && b2SnapCheckCount( r, nodeEnd, (int)sizeof( b2TreeNode ),+									(int)sizeof( b2TreeNode ) + (int)sizeof( int32_t ) ) == false )+	{+		r->ok = false;+	}++	if ( r->ok && b2SnapCheckCount( r, proxyCapacity, (int)sizeof( b2TreeProxy ),+									(int)sizeof( b2TreeProxy ) ) == false )+	{+		r->ok = false;+	}++	// Free what the shell or a live world holds, rebuild scratch included. Destroy zeroes the+	// struct, which covers every field the image does not carry.+	b2DestroyDynamicTreeInternal( tree );++	if ( !r->ok )+	{+		// Leave a valid empty tree so the shell can still be destroyed+		b2CreateDynamicTreeInternal( tree, 0 );+		return;+	}++	// This is just an allocation hint and the exact value doesn't matter.+	nodeCapacity = b2MinInt( nodeCapacity, 2 * nodeEnd );++	tree->nodeEnd = nodeEnd;+	tree->nodeCapacity = nodeCapacity;+	tree->pairFreeList = pairFreeList;+	tree->proxyCount = proxyCount;+	tree->proxyCapacity = proxyCapacity;+	tree->proxyFreeList = proxyFreeList;+	tree->dfsOrdered = ordered != 0;++	tree->nodes = b2AllocZero( nodeCapacity * sizeof( b2TreeNode ) );+	b2SnapR_Bytes( r, tree->nodes, nodeEnd * (int)sizeof( b2TreeNode ) );++	tree->parents = b2AllocZero( nodeCapacity * sizeof( int32_t ) );+	b2SnapR_Bytes( r, tree->parents, nodeEnd * (int)sizeof( int32_t ) );++	tree->proxies = b2AllocZero( proxyCapacity * sizeof( b2TreeProxy ) );+	b2SnapR_Bytes( r, tree->proxies, proxyCapacity * (int)sizeof( b2TreeProxy ) );+}++// Solver set: setIndex + 5 POD arrays+static void b2SerSolverSet( b2RecBuffer* buf, const b2SolverSet* set )+{+	b2SnapW_I32( buf, set->setIndex );+	b2SerPodArray( buf, set->bodySims );+	b2SerPodArray( buf, set->bodyStates );+	b2SerPodArray( buf, set->jointSims );+	b2SerPodArray( buf, set->contactSims );+	b2SerPodArray( buf, set->islandSims );+}++static void b2DesSolverSet( b2SnapReader* r, b2SolverSet* set )+{+	set->setIndex = b2SnapR_I32( r );+	b2DesPodArray( r, set->bodySims );+	b2DesPodArray( r, set->bodyStates );+	b2DesPodArray( r, set->jointSims );+	b2DesPodArray( r, set->contactSims );+	b2DesPodArray( r, set->islandSims );+}++// Graph color: bodySet + contactSims + jointSims (overflow color has no bodySet)+static void b2SerGraphColor( b2RecBuffer* buf, const b2GraphColor* color, bool isOverflow )+{+	if ( !isOverflow )+	{+		b2SerBitSet( buf, &color->bodySet );+	}+	b2SerPodArray( buf, color->contactSims );+	b2SerPodArray( buf, color->jointSims );+}++static void b2DesGraphColor( b2SnapReader* r, b2GraphColor* color, bool isOverflow )+{+	if ( !isOverflow )+	{+		b2DesBitSet( r, &color->bodySet );+	}+	b2DesPodArray( r, color->contactSims );+	b2DesPodArray( r, color->jointSims );+	// Transient wideConstraints/overflowConstraints stay NULL, wideConstraintCount 0+}++// World scalar config (simulation settings only, no runtime/shell fields)+// Only simulation scalars belong here, never host or worker state (workerCount,+// scheduler, callbacks, user data). b2World_RestoreFromSnapshot relies on that so an in-place+// restore preserves the live world's wiring.+static void b2SerWorldConfig( b2RecBuffer* buf, const b2World* world )+{+	b2SnapW_Bytes( buf, &world->gravity, sizeof( b2Vec2 ) );+	b2SnapW_Bytes( buf, &world->hitEventThreshold, sizeof( float ) );+	b2SnapW_Bytes( buf, &world->restitutionThreshold, sizeof( float ) );+	b2SnapW_I32( buf, world->restitutionIterations );+	b2SnapW_Bytes( buf, &world->maxLinearSpeed, sizeof( float ) );+	b2SnapW_Bytes( buf, &world->contactSpeed, sizeof( float ) );+	b2SnapW_Bytes( buf, &world->contactHertz, sizeof( float ) );+	b2SnapW_Bytes( buf, &world->contactDampingRatio, sizeof( float ) );+	b2SnapW_Bytes( buf, &world->contactRecycleDistance, sizeof( float ) );+	b2SnapW_Bytes( buf, &world->stepIndex, sizeof( uint64_t ) );+	b2SnapW_I32( buf, world->splitIslandId );+	// Step scaling cached for the force/torque reporting getters, which run between steps+	b2SnapW_Bytes( buf, &world->inv_h, sizeof( float ) );+	b2SnapW_Bytes( buf, &world->inv_dt, sizeof( float ) );+	// End-event double-buffer parity, so the first post-restore event query reads the right half+	b2SnapW_I32( buf, world->endEventArrayIndex );+	// maxCapacity (b2Capacity struct)+	b2SnapW_Bytes( buf, &world->maxCapacity, sizeof( b2Capacity ) );+	// bool flags packed as individual bytes for layout stability+	uint8_t flags = 0;+	flags |= world->enableSleep ? 0x01u : 0u;+	flags |= world->enableWarmStarting ? 0x02u : 0u;+	flags |= world->enableContinuous ? 0x08u : 0u;+	flags |= world->enableRestitutionPropagation ? 0x10u : 0u;+	b2RecBufAppend( buf, &flags, 1 );+}++static void b2DesWorldConfig( b2SnapReader* r, b2World* world )+{+	b2SnapR_Bytes( r, &world->gravity, sizeof( b2Vec2 ) );+	b2SnapR_Bytes( r, &world->hitEventThreshold, sizeof( float ) );+	b2SnapR_Bytes( r, &world->restitutionThreshold, sizeof( float ) );+	world->restitutionIterations = b2SnapR_I32( r );+	b2SnapR_Bytes( r, &world->maxLinearSpeed, sizeof( float ) );+	b2SnapR_Bytes( r, &world->contactSpeed, sizeof( float ) );+	b2SnapR_Bytes( r, &world->contactHertz, sizeof( float ) );+	b2SnapR_Bytes( r, &world->contactDampingRatio, sizeof( float ) );+	b2SnapR_Bytes( r, &world->contactRecycleDistance, sizeof( float ) );+	b2SnapR_Bytes( r, &world->stepIndex, sizeof( uint64_t ) );+	world->splitIslandId = b2SnapR_I32( r );+	b2SnapR_Bytes( r, &world->inv_h, sizeof( float ) );+	b2SnapR_Bytes( r, &world->inv_dt, sizeof( float ) );+	world->endEventArrayIndex = b2SnapR_I32( r );+	b2SnapR_Bytes( r, &world->maxCapacity, sizeof( b2Capacity ) );+	uint8_t flags = 0;+	b2SnapR_Bytes( r, &flags, 1 );+	world->enableSleep = ( flags & 0x01u ) != 0;+	world->enableWarmStarting = ( flags & 0x02u ) != 0;+	world->enableContinuous = ( flags & 0x08u ) != 0;+	world->enableRestitutionPropagation = ( flags & 0x10u ) != 0;+}++void b2SerializeWorld( b2World* world, b2RecBuffer* buf )+{+	// Image header+	b2SnapHeader hdr;+	hdr.magic = B2_SNAP_MAGIC;+	hdr.version = B2_SNAP_VERSION;+	hdr.layoutHash = b2ComputeLayoutHash();+	hdr.flags = B2_ENABLE_VALIDATION ? B2_SNAP_FLAG_VALIDATION : 0u;+	if ( b2IsDoublePrecision() )+	{+		hdr.flags |= B2_SNAP_FLAG_DOUBLE_PRECISION;+	}+	b2SnapW_Bytes( buf, &hdr, (int)sizeof( hdr ) );++	// World config+	b2SerWorldConfig( buf, world );++	// 7 id pools+	b2SerIdPool( buf, &world->bodyIdPool );+	b2SerIdPool( buf, &world->shapeIdPool );+	b2SerIdPool( buf, &world->chainIdPool );+	b2SerIdPool( buf, &world->contactIdPool );+	b2SerIdPool( buf, &world->jointIdPool );+	b2SerIdPool( buf, &world->islandIdPool );+	b2SerIdPool( buf, &world->solverSetIdPool );++	// Solver sets+	int setCount = world->solverSets.count;+	b2SnapW_I32( buf, setCount );+	for ( int i = 0; i < setCount; ++i )+	{+		b2SerSolverSet( buf, world->solverSets.data + i );+	}++	// Sparse arrays. Bodies, shapes and joints carry a host userData pointer scrubbed to NULL on+	// write. Contacts have no userData, so they go out as raw POD.+	b2SerSimArray( buf, world->bodies, b2Body );+	b2SerSimArray( buf, world->shapes, b2Shape );+	b2SerPodArray( buf, world->fatAABBs );+	b2SerPodArray( buf, world->contacts );+	b2SerSimArray( buf, world->joints, b2Joint );++	// Chain shapes: POD scalars then per-live-slot heap arrays+	int chainCount = world->chainShapes.count;+	b2SnapW_I32( buf, chainCount );+	for ( int i = 0; i < chainCount; ++i )+	{+		b2ChainShape* chain = world->chainShapes.data + i;+		// Write POD scalars+		b2SnapW_I32( buf, chain->id );+		b2SnapW_I32( buf, chain->bodyId );+		b2SnapW_I32( buf, chain->nextChainId );+		b2SnapW_I32( buf, chain->segmentCount );+		b2SnapW_Bytes( buf, &chain->generation, sizeof( uint16_t ) );+		if ( chain->id != B2_NULL_INDEX )+		{+			// Live slot: write the two heap arrays+			b2SnapW_Bytes( buf, chain->shapeIndices, chain->segmentCount * (int)sizeof( int ) );+		}+	}++	// Sensors: shapeId + 3 visitor arrays per slot+	int sensorCount = world->sensors.count;+	b2SnapW_I32( buf, sensorCount );+	for ( int i = 0; i < sensorCount; ++i )+	{+		b2Sensor* s = world->sensors.data + i;+		b2SnapW_I32( buf, s->shapeId );+		b2SerPodArray( buf, s->hits );+		b2SerPodArray( buf, s->overlaps1 );+		b2SerPodArray( buf, s->overlaps2 );+	}++	// Islands: POD scalars + 3 inner arrays per slot+	int islandCount = world->islands.count;+	b2SnapW_I32( buf, islandCount );+	for ( int i = 0; i < islandCount; ++i )+	{+		b2Island* island = world->islands.data + i;+		b2SnapW_I32( buf, island->setIndex );+		b2SnapW_I32( buf, island->localIndex );+		b2SnapW_I32( buf, island->islandId );+		b2SnapW_I32( buf, island->constraintRemoveCount );+		b2SerPodArray( buf, island->bodies );+		b2SerPodArray( buf, island->contacts );+		b2SerPodArray( buf, island->joints );+	}++	// Broad phase+	b2BroadPhase* bp = &world->broadPhase;+	for ( int t = 0; t < b2_bodyTypeCount; ++t )+	{+		b2SerTree( buf, &bp->trees[t] );+	}+	b2SerHashSet( buf, &bp->pairSet );++	// Constraint graph: B2_GRAPH_COLOR_COUNT colors+	b2ConstraintGraph* graph = &world->constraintGraph;+	for ( int c = 0; c < B2_GRAPH_COLOR_COUNT; ++c )+	{+		b2SerGraphColor( buf, &graph->colors[c], c == B2_OVERFLOW_INDEX );+	}+}++// Free per-object heap the overwrite steps below don't reach, so restoring over a+// populated world doesn't leak. Outer arrays stay alive for the steps to reuse.+// Mirrors the per-object teardown in b2DestroyWorld.+static void b2FreeLiveSimElements( b2World* world )+{+	for ( int i = 0; i < world->chainShapes.count; ++i )+	{+		b2ChainShape* chain = world->chainShapes.data + i;+		if ( chain->id != B2_NULL_INDEX )+		{+			b2FreeChainData( chain );+		}+	}++	for ( int i = 0; i < world->sensors.count; ++i )+	{+		b2Sensor* sensor = world->sensors.data + i;+		b2Array_Destroy( sensor->hits );+		b2Array_Destroy( sensor->overlaps1 );+		b2Array_Destroy( sensor->overlaps2 );+	}++	for ( int i = 0; i < world->islands.count; ++i )+	{+		b2Island* island = world->islands.data + i;+		b2Array_Destroy( island->bodies );+		b2Array_Destroy( island->contacts );+		b2Array_Destroy( island->joints );+	}+}++// Overwrite a world with the simulation state from the reader. Mirrors the write+// order in b2SerializeWorld. The world must be a clean shell from b2CreateWorld, or+// a live world whose per-object heap was first freed by b2FreeLiveSimElements. Host+// wiring (scheduler, callbacks, user data) is never touched. Returns false on a+// corrupt image.+static bool b2DeserializeIntoShell( b2SnapReader* r, b2World* world )+{+	// Step 1: world scalars+	b2DesWorldConfig( r, world );++	// Step 2: 7 id pools (overwrite entirely, including solverSetIdPool from the 3 pre-created+	// sets)+	b2DesIdPool( r, &world->bodyIdPool );+	b2DesIdPool( r, &world->shapeIdPool );+	b2DesIdPool( r, &world->chainIdPool );+	b2DesIdPool( r, &world->contactIdPool );+	b2DesIdPool( r, &world->jointIdPool );+	b2DesIdPool( r, &world->islandIdPool );+	b2DesIdPool( r, &world->solverSetIdPool );++	// Step 3: solver sets+	// (a) Destroy the 5 inner arrays of the 3 pre-created sets WITHOUT freeing the set id+	//     (b2DestroySolverSet would do that, but we already authored the id pool above)+	for ( int i = 0; i < world->solverSets.count; ++i )+	{+		b2SolverSet* set = world->solverSets.data + i;+		b2Array_Destroy( set->bodySims );+		b2Array_Destroy( set->bodyStates );+		b2Array_Destroy( set->contactSims );+		b2Array_Destroy( set->jointSims );+		b2Array_Destroy( set->islandSims );+	}++	// Each set writes at least setIndex plus 5 array counts+	int setCount = b2SnapR_I32( r );+	if ( r->ok && b2SnapCheckCount( r, setCount, (int)sizeof( b2SolverSet ),+									6 * (int)sizeof( int ) ) == false )+	{+		r->ok = false;+	}++	if ( r->ok )+	{+		// (b) Resize outer array to hold all sets, zeroing new slots so their headers are clean+		b2Array_ResizeAndSetZero( world->solverSets, setCount );++		// (c) Deserialize each set+		for ( int i = 0; i < setCount; ++i )+		{+			b2DesSolverSet( r, world->solverSets.data + i );+		}+	}++	// Step 4: sparse arrays. userData was written as NULL by the serializer, so a plain copy+	// restores it cleanly with no host pointers to scrub here.+	b2DesPodArray( r, world->bodies );+	b2DesPodArray( r, world->shapes );+	b2DesPodArray( r, world->fatAABBs );+	b2DesPodArray( r, world->contacts );+	b2DesPodArray( r, world->joints );++	// Step 5: chain shapes+	{+		// Destroy the shell's chainShapes array (empty, but has a backing allocation)+		b2Array_Destroy( world->chainShapes );+		b2Array_Create( world->chainShapes );++		// Each chain writes 4 ints plus a uint16 generation+		int chainCount = b2SnapR_I32( r );+		if ( r->ok &&+			 b2SnapCheckCount( r, chainCount, (int)sizeof( b2ChainShape ),+							   4 * (int)sizeof( int ) + (int)sizeof( uint16_t ) ) == false )+		{+			r->ok = false;+		}+		if ( r->ok )+		{+			b2Array_Resize( world->chainShapes, chainCount );+			// Zero the whole array so free slots have NULL pointers+			memset( world->chainShapes.data, 0, chainCount * sizeof( b2ChainShape ) );+		}++		for ( int i = 0; i < chainCount && r->ok; ++i )+		{+			b2ChainShape* chain = world->chainShapes.data + i;+			chain->id = b2SnapR_I32( r );+			chain->bodyId = b2SnapR_I32( r );+			chain->nextChainId = b2SnapR_I32( r );+			chain->segmentCount = b2SnapR_I32( r );+			b2SnapR_Bytes( r, &chain->generation, sizeof( uint16_t ) );+			// A partial read leaves id as 0, which is a valid slot value, so gate the live branch+			// on r->ok+			if ( r->ok && chain->id != B2_NULL_INDEX )+			{+				if ( b2SnapCheckCount( r, chain->segmentCount, (int)sizeof( int ),+									   (int)sizeof( int ) ) == false )+				{+					r->ok = false;+					break;+				}+				// Live slot: allocate and copy heap arrays+				chain->shapeIndices = b2Alloc( chain->segmentCount * (int)sizeof( int ) );+				b2SnapR_Bytes( r, chain->shapeIndices, chain->segmentCount * (int)sizeof( int ) );+			}+			else+			{+				// Free slot must have NULL pointers; zero init above handles this+				chain->shapeIndices = NULL;+			}+		}+	}++	// Step 6: sensors+	{+		// Destroy the shell's sensors array+		b2Array_Destroy( world->sensors );+		b2Array_Create( world->sensors );++		// Each sensor writes shapeId plus 3 array counts+		int sensorCount = b2SnapR_I32( r );+		if ( r->ok && b2SnapCheckCount( r, sensorCount, (int)sizeof( b2Sensor ),+										4 * (int)sizeof( int ) ) == false )+		{+			r->ok = false;+		}+		if ( r->ok )+		{+			b2Array_Resize( world->sensors, sensorCount );+			// Zero so inner array headers start clean+			memset( world->sensors.data, 0, sensorCount * sizeof( b2Sensor ) );+		}++		for ( int i = 0; i < sensorCount && r->ok; ++i )+		{+			b2Sensor* s = world->sensors.data + i;+			s->shapeId = b2SnapR_I32( r );+			// Re-init inner arrays then fill them+			b2Array_Create( s->hits );+			b2Array_Create( s->overlaps1 );+			b2Array_Create( s->overlaps2 );+			b2DesPodArray( r, s->hits );+			b2DesPodArray( r, s->overlaps1 );+			b2DesPodArray( r, s->overlaps2 );+		}+	}++	// Step 7: islands+	{+		// Destroy the shell's islands array+		b2Array_Destroy( world->islands );+		b2Array_Create( world->islands );++		// Each island writes 4 ints plus 3 array counts+		int islandCount = b2SnapR_I32( r );+		if ( r->ok && b2SnapCheckCount( r, islandCount, (int)sizeof( b2Island ),+										7 * (int)sizeof( int ) ) == false )+		{+			r->ok = false;+		}+		if ( r->ok )+		{+			b2Array_Resize( world->islands, islandCount );+			memset( world->islands.data, 0, islandCount * sizeof( b2Island ) );+		}++		for ( int i = 0; i < islandCount && r->ok; ++i )+		{+			b2Island* island = world->islands.data + i;+			island->setIndex = b2SnapR_I32( r );+			island->localIndex = b2SnapR_I32( r );+			island->islandId = b2SnapR_I32( r );+			island->constraintRemoveCount = b2SnapR_I32( r );+			b2Array_Create( island->bodies );+			b2Array_Create( island->contacts );+			b2Array_Create( island->joints );+			b2DesPodArray( r, island->bodies );+			b2DesPodArray( r, island->contacts );+			b2DesPodArray( r, island->joints );+		}+	}++	// Step 8: broad phase+	{+		b2BroadPhase* bp = &world->broadPhase;++		// Trees: the shell already allocated tree nodes; free them and replace+		for ( int t = 0; t < b2_bodyTypeCount; ++t )+		{+			b2DesTree( r, &bp->trees[t] );+		}++		// pairSet+		b2DesHashSet( r, &bp->pairSet );+	}++	// Step 9: constraint graph+	{+		b2ConstraintGraph* graph = &world->constraintGraph;+		for ( int c = 0; c < B2_GRAPH_COLOR_COUNT; ++c )+		{+			b2DesGraphColor( r, &graph->colors[c], c == B2_OVERFLOW_INDEX );+		}+	}++	// Event buffers are transient and never serialized. An in-place restore reuses the live+	// world's arrays, so end events queued by a between-step mutator (b2Body_Disable and friends)+	// would survive the restore and surface after the first resimmed step. Reset to match a fresh+	// world from snapshot. The double-buffer parity is restored, but the contents must start empty.+	b2Array_Clear( world->bodyMoveEvents );+	b2Array_Clear( world->sensorBeginEvents );+	b2Array_Clear( world->sensorEndEvents[0] );+	b2Array_Clear( world->sensorEndEvents[1] );+	b2Array_Clear( world->contactBeginEvents );+	b2Array_Clear( world->contactEndEvents[0] );+	b2Array_Clear( world->contactEndEvents[1] );+	b2Array_Clear( world->contactHitEvents );+	b2Array_Clear( world->jointEvents );++	return r->ok;+}++// Validate a snapshot image header and arm the reader just past it. Returns false on a rejected+// image (too small, bad magic, wrong version, layout mismatch), leaving the caller's world+// untouched. A layout mismatch caused purely by differing validation builds is called out, since+// the validation gated fields change struct sizes and such images can never share a layout.+static bool b2OpenSnapshotImage( const uint8_t* image, int size, b2SnapReader* r )+{+	if ( image == NULL || size < (int)sizeof( b2SnapHeader ) )+	{+		return false;+	}++	b2SnapHeader hdr;+	memcpy( &hdr, image, sizeof( hdr ) );+	if ( hdr.magic != B2_SNAP_MAGIC || hdr.version != B2_SNAP_VERSION )+	{+		return false;+	}++	// World positions are stored at the build precision, so the layout differs irreconcilably+	// across modes. Called out before the layout hash so the cause is clear rather than a generic+	// mismatch.+	bool imageDouble = ( hdr.flags & B2_SNAP_FLAG_DOUBLE_PRECISION ) != 0;+	bool buildDouble = b2IsDoublePrecision();+	if ( imageDouble != buildDouble )+	{+		b2Log( "snapshot precision mismatch: image %s, this build %s\n",+			   imageDouble ? "double" : "float", buildDouble ? "double" : "float" );+		return false;+	}++	if ( hdr.layoutHash != b2ComputeLayoutHash() )+	{+		bool imageValidation = ( hdr.flags & B2_SNAP_FLAG_VALIDATION ) != 0;+		if ( imageValidation != (bool)B2_ENABLE_VALIDATION )+		{+			b2Log( "snapshot layout mismatch: image built with validation %s, this build %s\n",+				   imageValidation ? "on" : "off", B2_ENABLE_VALIDATION ? "on" : "off" );+		}+		else+		{+			b2Log( "snapshot layout mismatch\n" );+		}+		return false;+	}++	r->data = image;+	r->cursor = (int)sizeof( hdr );+	r->size = size;+	r->ok = true;+	return true;+}++b2WorldId b2CreateWorldFromSnapshot( const uint8_t* image, int size, int workerCount )+{+	b2WorldId nullId = b2_nullWorldId;++	b2SnapReader readerStorage;+	b2SnapReader* r = &readerStorage;+	if ( b2OpenSnapshotImage( image, size, r ) == false )+	{+		return nullId;+	}++	// Build a minimal valid def so b2CreateWorld produces a fully valid shell+	b2WorldDef def = b2DefaultWorldDef();+	def.workerCount = workerCount;+	def.enqueueTask = NULL;+	def.finishTask = NULL;+	def.userTaskContext = NULL;++	// Capacity is only a sizing hint. Every container is resized from the image below+	b2WorldId id = b2CreateWorld( &def );+	if ( !b2World_IsValid( id ) )+	{+		return nullId;+	}++	b2World* world = b2GetWorldFromId( id );++	if ( b2DeserializeIntoShell( r, world ) == false )+	{+		// Image was corrupt; clean up by destroying the world+		b2DestroyWorld( id );+		return nullId;+	}++	// A world loaded from scratch carries no host pointers+	world->preSolveFcn = NULL;+	world->preSolveContext = NULL;+	world->customFilterFcn = NULL;+	world->customFilterContext = NULL;+	world->userData = NULL;++	return id;+}++bool b2World_RestoreFromSnapshot( b2WorldId worldId, const uint8_t* image, int size )+{+	// Validate the image fully before touching the world so a bad image leaves it intact+	b2SnapReader readerStorage;+	b2SnapReader* r = &readerStorage;+	if ( b2OpenSnapshotImage( image, size, r ) == false )+	{+		return false;+	}++	b2World* world = b2GetWorldFromId( worldId );++	// Restoring mid step would corrupt an in-flight solve+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return false;+	}++	if ( world->recording != NULL )+	{+		b2Log( "b2World_RestoreFromSnapshot: not supported while recording\n" );+		return false;+	}++	// Point of no return. The slot, generation, and host wiring are kept, so held ids+	// resolve into the rebuilt world. A truncated payload past here leaves the world+	// unusable and the caller must destroy it.+	b2FreeLiveSimElements( world );++	return b2DeserializeIntoShell( r, world );+}++int b2World_GetSnapshot( b2WorldId worldId, uint8_t* image, int capacity )+{+	b2World* world = b2GetWorldFromId( worldId );++	// Serializing mid step would capture an inconsistent, in-flight world+	B2_ASSERT( world->locked == false );+	if ( world->locked )+	{+		return 0;+	}++	// Size query: count the bytes without allocating or copying the whole image+	if ( image == NULL )+	{+		b2RecBuffer counter = { 0 };+		counter.countOnly = true;+		b2SerializeWorld( world, &counter );+		return counter.size;+	}++	b2RecBuffer buf = { 0 };+	b2SerializeWorld( world, &buf );+	int size = buf.size;++	if ( size <= capacity )+	{+		memcpy( image, buf.data, size );+	}++	b2RecBufFree( &buf );+	return size;+}++static uint64_t b2FnvMixBytes( uint64_t hash, const void* data, int n )+{+	const uint8_t* p = (const uint8_t*)data;+	for ( int i = 0; i < n; ++i )+	{+		hash = ( hash ^ (uint64_t)p[i] ) * B2_SNAP_FNV_PRIME;+	}+	return hash;+}++static uint64_t b2FnvMixFloat( uint64_t hash, float f )+{+	uint32_t bits;+	memcpy( &bits, &f, 4 );+	return ( hash ^ (uint64_t)bits ) * B2_SNAP_FNV_PRIME;+}++static uint64_t b2FnvMixInt( uint64_t hash, int v )+{+	return ( hash ^ (uint64_t)(uint32_t)v ) * B2_SNAP_FNV_PRIME;+}++uint64_t b2HashWorldStateDeep( b2World* world )+{+	uint64_t hash = B2_SNAP_FNV_INIT;++	// Bodies: same iteration order as b2HashWorldState (sparse array, skip free slots)+	int bodyCount = world->bodies.count;+	for ( int i = 0; i < bodyCount; ++i )+	{+		b2Body* body = world->bodies.data + i;+		if ( body->id != i )+		{+			continue;+		}++		b2BodySim* sim = b2GetBodySim( world, body );++		hash = b2FnvMixPosition( hash, sim->transform.p );+		hash = b2FnvMixFloat( hash, sim->transform.q.c );+		hash = b2FnvMixFloat( hash, sim->transform.q.s );++		b2BodyState* state = b2GetBodyState( world, body );+		if ( state != NULL )+		{+			hash = b2FnvMixFloat( hash, state->linearVelocity.x );+			hash = b2FnvMixFloat( hash, state->linearVelocity.y );+			hash = b2FnvMixFloat( hash, state->angularVelocity );+		}++		// Index bookkeeping+		hash = b2FnvMixInt( hash, body->setIndex );+		hash = b2FnvMixInt( hash, body->localIndex );+	}++	// Contacts: sparse array, skip free slots (colorIndex == B2_NULL_INDEX and setIndex ==+	// B2_NULL_INDEX for free)+	int contactCount = world->contacts.count;+	for ( int i = 0; i < contactCount; ++i )+	{+		b2Contact* contact = world->contacts.data + i;+		if ( contact->contactId != i )+		{+			continue;+		}++		hash = b2FnvMixInt( hash, contact->setIndex );+		hash = b2FnvMixInt( hash, contact->colorIndex );+		hash = b2FnvMixInt( hash, contact->localIndex );++		// Manifold points + impulses from the contact sim+		// Exclude B2_ENABLE_VALIDATION-gated bodyIdA/bodyIdB for build-config stability+		b2ContactSim* sim = b2GetContactSim( world, contact );+		if ( sim != NULL )+		{+			b2Manifold* m = &sim->manifold;+			hash = b2FnvMixInt( hash, m->pointCount );+			for ( int p = 0; p < m->pointCount; ++p )+			{+				hash = b2FnvMixFloat( hash, m->points[p].normalImpulse );+				hash = b2FnvMixFloat( hash, m->points[p].tangentImpulse );+				hash = b2FnvMixFloat( hash, m->points[p].totalNormalImpulse );+			}+		}+	}++	// Joints: sparse array, skip free slots+	int jointCount = world->joints.count;+	for ( int i = 0; i < jointCount; ++i )+	{+		b2Joint* joint = world->joints.data + i;+		if ( joint->jointId != i )+		{+			continue;+		}++		hash = b2FnvMixInt( hash, joint->setIndex );+		hash = b2FnvMixInt( hash, joint->colorIndex );+		hash = b2FnvMixInt( hash, joint->localIndex );++		b2JointSim* sim = b2GetJointSim( world, joint );+		if ( sim != NULL )+		{+			// Hash accumulated impulses per joint type+			switch ( sim->type )+			{+				case b2_distanceJoint:+					hash = b2FnvMixFloat( hash, sim->distanceJoint.impulse );+					hash = b2FnvMixFloat( hash, sim->distanceJoint.lowerImpulse );+					hash = b2FnvMixFloat( hash, sim->distanceJoint.upperImpulse );+					hash = b2FnvMixFloat( hash, sim->distanceJoint.motorImpulse );+					break;+				case b2_moverJoint:+					hash = b2FnvMixFloat( hash, sim->moverJoint.linearVelocityImpulse.x );+					hash = b2FnvMixFloat( hash, sim->moverJoint.linearVelocityImpulse.y );+					break;+				case b2_motorJoint:+					hash = b2FnvMixFloat( hash, sim->motorJoint.linearVelocityImpulse.x );+					hash = b2FnvMixFloat( hash, sim->motorJoint.linearVelocityImpulse.y );+					hash = b2FnvMixFloat( hash, sim->motorJoint.angularVelocityImpulse );+					hash = b2FnvMixFloat( hash, sim->motorJoint.linearSpringImpulse.x );+					hash = b2FnvMixFloat( hash, sim->motorJoint.linearSpringImpulse.y );+					hash = b2FnvMixFloat( hash, sim->motorJoint.angularSpringImpulse );+					break;+				case b2_pogoJoint:+					hash = b2FnvMixFloat( hash, sim->pogoJoint.impulse );+					hash = b2FnvMixFloat( hash, sim->pogoJoint.velocity );+					break;+				case b2_prismaticJoint:+					hash = b2FnvMixBytes( hash, &sim->prismaticJoint.impulse, sizeof( b2Vec2 ) );+					hash = b2FnvMixFloat( hash, sim->prismaticJoint.springImpulse );+					hash = b2FnvMixFloat( hash, sim->prismaticJoint.motorImpulse );+					hash = b2FnvMixFloat( hash, sim->prismaticJoint.lowerImpulse );+					hash = b2FnvMixFloat( hash, sim->prismaticJoint.upperImpulse );+					break;+				case b2_revoluteJoint:+					hash =+						b2FnvMixBytes( hash, &sim->revoluteJoint.linearImpulse, sizeof( b2Vec2 ) );+					hash = b2FnvMixFloat( hash, sim->revoluteJoint.springImpulse );+					hash = b2FnvMixFloat( hash, sim->revoluteJoint.motorImpulse );+					hash = b2FnvMixFloat( hash, sim->revoluteJoint.lowerImpulse );+					hash = b2FnvMixFloat( hash, sim->revoluteJoint.upperImpulse );+					break;+				case b2_weldJoint:+					hash = b2FnvMixBytes( hash, &sim->weldJoint.linearImpulse, sizeof( b2Vec2 ) );+					hash = b2FnvMixFloat( hash, sim->weldJoint.angularImpulse );+					break;+				case b2_wheelJoint:+					hash = b2FnvMixFloat( hash, sim->wheelJoint.perpImpulse );+					hash = b2FnvMixFloat( hash, sim->wheelJoint.motorImpulse );+					hash = b2FnvMixFloat( hash, sim->wheelJoint.springImpulse );+					hash = b2FnvMixFloat( hash, sim->wheelJoint.lowerImpulse );+					hash = b2FnvMixFloat( hash, sim->wheelJoint.upperImpulse );+					break;+				default:+					break;+			}+		}+	}++	// 7 id pools: nextIndex + count+	hash = b2FnvMixInt( hash, world->bodyIdPool.nextIndex );+	hash = b2FnvMixInt( hash, b2GetIdCount( &world->bodyIdPool ) );+	hash = b2FnvMixInt( hash, world->shapeIdPool.nextIndex );+	hash = b2FnvMixInt( hash, b2GetIdCount( &world->shapeIdPool ) );+	hash = b2FnvMixInt( hash, world->chainIdPool.nextIndex );+	hash = b2FnvMixInt( hash, b2GetIdCount( &world->chainIdPool ) );+	hash = b2FnvMixInt( hash, world->contactIdPool.nextIndex );+	hash = b2FnvMixInt( hash, b2GetIdCount( &world->contactIdPool ) );+	hash = b2FnvMixInt( hash, world->jointIdPool.nextIndex );+	hash = b2FnvMixInt( hash, b2GetIdCount( &world->jointIdPool ) );+	hash = b2FnvMixInt( hash, world->islandIdPool.nextIndex );+	hash = b2FnvMixInt( hash, b2GetIdCount( &world->islandIdPool ) );+	hash = b2FnvMixInt( hash, world->solverSetIdPool.nextIndex );+	hash = b2FnvMixInt( hash, b2GetIdCount( &world->solverSetIdPool ) );++	// Solver set count+	hash = b2FnvMixInt( hash, world->solverSets.count );++	return hash;+}++uint64_t b2World_GetStateHash( b2WorldId worldId )+{+	b2World* world = b2GetWorldFromId( worldId );+	return b2HashWorldStateDeep( world );+}
+ upstream/src/snapshot.h view
@@ -0,0 +1,18 @@+// SPDX-FileCopyrightText: 2026 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "recording.h"++#include <stdint.h>++typedef struct b2World b2World;++// Serialize the complete simulation state of world into buf. Backs the public+// b2World_GetSnapshot. Must be called at a step boundary (between b2World_Step calls).+// Reuses b2RecBuffer/b2RecBufAppend for output.+void b2SerializeWorld( b2World* world, b2RecBuffer* buf );++// Extensive hash for testing+uint64_t b2HashWorldStateDeep( b2World* world );
+ upstream/src/solver.c view
@@ -0,0 +1,2023 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "solver.h"++#include "aabb.h"+#include "arena_allocator.h"+#include "bitset.h"+#include "body.h"+#include "contact.h"+#include "contact_solver.h"+#include "core.h"+#include "dynamic_tree.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 <stdbool.h>+#include <stddef.h>+#include <string.h>++// these are useful for solver testing+#define ITERATIONS 1+#define RELAX_ITERATIONS 1++typedef struct b2WorkerContext+{+	b2StepContext* context;+	int workerIndex;+	void* userTask;+} b2WorkerContext;++// Integrate velocities and apply damping+static void b2IntegrateVelocitiesTask( b2SolverBlock block, b2StepContext* context )+{+	b2TracyCZoneNC( integrate_velocity, "IntVel", b2_colorDeepPink, true );++	b2BodyState* states = context->states;+	b2BodySim* sims = context->sims;++	B2_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b2_awakeSet].bodyStates.count );++	b2Vec2 gravity = context->world->gravity;+	float h = context->h;++	for ( int i = block.startIndex; i < block.startIndex + block.count; ++i )+	{+		b2BodySim* sim = sims + i;+		b2BodyState* state = states + i;++		b2Vec2 v = state->linearVelocity;+		float w = state->angularVelocity;++		// Apply forces, torque, gravity, and damping+		// Apply damping.+		// 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;++		// lvd = h * im * f + h * g+		b2Vec2 linearVelocityDelta = b2Add( b2MulSV( h * sim->invMass, sim->force ), b2MulSV( h * gravityScale, gravity ) );+		float angularVelocityDelta = h * sim->invInertia * sim->torque;++		v = b2MulAdd( linearVelocityDelta, linearDamping, v );+		w = angularVelocityDelta + angularDamping * w;++		state->linearVelocity = v;+		state->angularVelocity = w;+	}++	b2TracyCZoneEnd( integrate_velocity );+}++static void b2IntegratePositionsTask( b2SolverBlock block, b2StepContext* context )+{+	b2TracyCZoneNC( integrate_positions, "IntPos", b2_colorDarkSeaGreen, true );++	B2_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b2_awakeSet].bodyStates.count );++	b2BodyState* states = context->states;+	float h = context->h;+	float maxLinearSpeed = context->maxLinearVelocity;+	float maxAngularSpeed = B2_MAX_ROTATION * context->inv_dt;+	float maxLinearSpeedSquared = maxLinearSpeed * maxLinearSpeed;+	float maxAngularSpeedSquared = maxAngularSpeed * maxAngularSpeed;++	for ( int i = block.startIndex; i < block.startIndex + block.count; ++i )+	{+		b2BodyState* state = states + i;++		b2Vec2 v = state->linearVelocity;+		float w = state->angularVelocity;++		// Motion locks - these can be viewed as a constraint that comes last+		v.x = ( state->flags & b2_lockLinearX ) ? 0.0f : v.x;+		v.y = ( state->flags & b2_lockLinearY ) ? 0.0f : v.y;+		w = ( state->flags & b2_lockAngularZ ) ? 0.0f : w;++		// Clamp to max linear speed+		if ( b2Dot( v, v ) > maxLinearSpeedSquared )+		{+			float ratio = maxLinearSpeed / b2Length( v );+			v = b2MulSV( ratio, v );+			state->flags |= b2_isSpeedCapped;+		}++		// Clamp to max angular speed+		if ( w * w > maxAngularSpeedSquared && ( state->flags & b2_allowFastRotation ) == 0 )+		{+			float ratio = maxAngularSpeed / b2AbsFloat( w );+			w *= ratio;+			state->flags |= b2_isSpeedCapped;+		}++		state->linearVelocity = v;+		state->angularVelocity = w;+		state->deltaPosition = b2MulAdd( state->deltaPosition, h, state->linearVelocity );+		state->deltaRotation = b2IntegrateRotation( state->deltaRotation, h * state->angularVelocity );+	}++	b2TracyCZoneEnd( integrate_positions );+}++#define B2_MAX_CONTINUOUS_SENSOR_HITS 8++struct b2ContinuousContext+{+	b2World* world;+	b2BodySim* fastBodySim;+	b2Shape* fastShape;+	b2Vec2 centroid1, centroid2;+	b2Sweep sweep;+	b2Pos base;+	float fraction;+	b2SensorHit sensorHits[B2_MAX_CONTINUOUS_SENSOR_HITS];+	float sensorFractions[B2_MAX_CONTINUOUS_SENSOR_HITS];+	int sensorCount;+};++#define B2_CORE_FRACTION 0.25f++// This is called from b2DynamicTree_OverlapAABB for continuous collision+static bool b2ContinuousQueryCallback( int proxyId, uint64_t userData, void* context )+{+	B2_UNUSED( proxyId );++	int shapeId = (int)userData;+	struct b2ContinuousContext* continuousContext = context;+	b2Shape* fastShape = continuousContext->fastShape;+	b2BodySim* fastBodySim = continuousContext->fastBodySim;++	B2_ASSERT( fastShape->sensorIndex == B2_NULL_INDEX );++	// Skip same shape+	if ( shapeId == fastShape->id )+	{+		return true;+	}++	b2World* world = continuousContext->world;+	b2Shape* shape = b2Array_Get( world->shapes, shapeId );++	// Skip same body+	if ( shape->bodyId == fastShape->bodyId )+	{+		return true;+	}++	// Skip sensors unless the shapes want sensor events+	bool isSensor = shape->sensorIndex != B2_NULL_INDEX;+	if ( isSensor && ( shape->enableSensorEvents == false || fastShape->enableSensorEvents == false ) )+	{+		return true;+	}++	// Skip filtered shapes+	bool canCollide = b2ShouldShapesCollide( fastShape->filter, shape->filter );+	if ( canCollide == false )+	{+		return true;+	}++	b2Body* body = b2Array_Get( world->bodies, shape->bodyId );++	b2BodySim* bodySim = b2GetBodySim( world, body );+	B2_ASSERT( body->type == b2_staticBody || ( fastBodySim->flags & b2_isBullet ) );++	// Skip bullets+	// Warning: it is only safe to read flags from other bodies, not body sims because there are+	// body sim flag writes in the continuous solver.+	if ( body->flags & b2_isBullet )+	{+		return true;+	}++	// Skip filtered bodies+	b2Body* fastBody = b2Array_Get( world->bodies, fastBodySim->bodyId );+	canCollide = b2ShouldBodiesCollide( world, fastBody, body );+	if ( canCollide == false )+	{+		return true;+	}++	// Custom user filtering+	if ( shape->enableCustomFiltering || fastShape->enableCustomFiltering )+	{+		b2CustomFilterCallbackFcn* customFilterFcn = world->customFilterFcn;+		if ( customFilterFcn != NULL )+		{+			b2ShapeId idA = { shape->id + 1, world->worldId, shape->generation };+			b2ShapeId idB = { fastShape->id + 1, world->worldId, fastShape->generation };+			canCollide = customFilterFcn( idA, idB, world->customFilterContext );+			if ( canCollide == false )+			{+				return true;+			}+		}+	}++	// Early out on fast parallel movement over a chain shape.+	if ( shape->type == b2_chainSegmentShape )+	{+		b2Transform transform = b2ToRelativeTransform( bodySim->transform, continuousContext->base );+		b2Vec2 p1 = b2TransformPoint( transform, shape->chainSegment.segment.point1 );+		b2Vec2 p2 = b2TransformPoint( transform, shape->chainSegment.segment.point2 );+		b2Vec2 e = b2Sub( p2, p1 );+		float length;+		e = b2GetLengthAndNormalize( &length, e );+		if ( length > B2_LINEAR_SLOP )+		{+			b2Vec2 c1 = continuousContext->centroid1;+			float separation1 = b2Cross( b2Sub( c1, p1 ), e );+			b2Vec2 c2 = continuousContext->centroid2;+			float separation2 = b2Cross( b2Sub( c2, p1 ), e );++			float coreDistance = B2_CORE_FRACTION * fastBodySim->minExtent;++			if ( separation1 < 0.0f || ( separation1 - separation2 < coreDistance && separation2 > coreDistance ) )+			{+				// Minimal clipping+				return true;+			}+		}+	}++	// todo_erin testing early out for segments+#if 0+	if ( shape->type == b2_segmentShape )+	{+		b2Transform transform = bodySim->transform;+		b2Vec2 p1 = b2TransformPoint( transform, shape->segment.point1 );+		b2Vec2 p2 = b2TransformPoint( transform, shape->segment.point2 );+		b2Vec2 e = b2Sub( p2, p1 );+		b2Vec2 c1 = continuousContext->centroid1;+		b2Vec2 c2 = continuousContext->centroid2;+		float offset1 = b2Cross( b2Sub( c1, p1 ), e );+		float offset2 = b2Cross( b2Sub( c2, p1 ), e );++		if ( offset1 > 0.0f && offset2 > 0.0f )+		{+			// Started behind or finished in front+			return true;+		}++		if ( offset1 < 0.0f && offset2 < 0.0f )+		{+			// Started behind or finished in front+			return true;+		}+	}+#endif++	b2TOIInput input;+	input.proxyA = b2MakeShapeDistanceProxy( shape );+	input.proxyB = b2MakeShapeDistanceProxy( fastShape );+	input.sweepA = b2MakeRelativeSweep( bodySim, continuousContext->base );+	input.sweepB = continuousContext->sweep;+	input.maxFraction = continuousContext->fraction;++	b2TOIOutput output = b2ComputeTimeOfImpact( &input );+	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+			b2SensorHit sensorHit = {+				.sensorId = shape->id,+				.visitorId = fastShape->id,+			};++			continuousContext->sensorHits[index] = sensorHit;+			continuousContext->sensorFractions[index] = output.fraction;+			continuousContext->sensorCount += 1;+		}+	}+	else+	{+		float hitFraction = continuousContext->fraction;+		bool didHit = false;++		if ( 0.0f < output.fraction && output.fraction < continuousContext->fraction )+		{+			hitFraction = output.fraction;+			didHit = true;+		}+		else if ( 0.0f == output.fraction )+		{+			// fallback to TOI of a small circle around the fast shape centroid+			b2Vec2 centroid = b2GetShapeCentroid( fastShape );+			b2ShapeExtent extent = b2ComputeShapeExtent( fastShape, centroid );+			float radius = B2_CORE_FRACTION * extent.minExtent;+			input.proxyB = b2MakeProxy( &centroid, 1, radius );+			output = b2ComputeTimeOfImpact( &input );+			if ( 0.0f < output.fraction && output.fraction < continuousContext->fraction )+			{+				hitFraction = output.fraction;+				didHit = true;+			}+		}++		if ( didHit && ( shape->enablePreSolveEvents || fastShape->enablePreSolveEvents ) && world->preContinuousFcn != NULL )+		{+			b2ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->generation };+			b2ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->generation };++			// TOI runs in the base frame, lift the hit point back to world for the callback+			b2Pos worldPoint = b2OffsetPos( continuousContext->base, output.point );+			didHit = world->preContinuousFcn( shapeIdA, shapeIdB, worldPoint, output.normal, world->preSolveContext );+		}++		if ( didHit )+		{+			continuousContext->fraction = hitFraction;+		}+	}++	// Continue query+	return true;+}++// Continuous collision of dynamic versus static+static void b2SolveContinuous( b2World* world, int bodySimIndex, b2TaskContext* taskContext, float dt )+{+	b2TracyCZoneNC( ccd, "CCD", b2_colorDarkGoldenRod, true );++	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	b2BodySim* fastBodySim = b2Array_Get( awakeSet->bodySims, bodySimIndex );+	B2_VALIDATE( fastBodySim->flags & b2_isFast );++	// Re-center the sweep on the fast body so the TOI and the swept query stay in float precision+	b2Pos base = fastBodySim->center0;+	b2Sweep sweep = b2MakeRelativeSweep( fastBodySim, base );++	b2Transform xf1;+	xf1.q = sweep.q1;+	xf1.p = b2Sub( sweep.c1, b2RotateVector( sweep.q1, sweep.localCenter ) );++	b2Transform xf2;+	xf2.q = sweep.q2;+	xf2.p = b2Sub( sweep.c2, b2RotateVector( sweep.q2, sweep.localCenter ) );++	b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody;+	b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody;+	b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody;+	b2Body* fastBody = b2Array_Get( world->bodies, fastBodySim->bodyId );++	struct b2ContinuousContext context = { 0 };+	context.world = world;+	context.sweep = sweep;+	context.base = base;+	context.fastBodySim = fastBodySim;+	context.fraction = 1.0f;++	bool isBullet = ( fastBodySim->flags & b2_isBullet ) != 0;++	int shapeId = fastBody->headShapeId;+	while ( shapeId != B2_NULL_INDEX )+	{+		b2Shape* fastShape = b2Array_Get( world->shapes, shapeId );+		shapeId = fastShape->nextShapeId;++		context.fastShape = fastShape;+		context.centroid1 = b2TransformPoint( xf1, fastShape->localCentroid );+		context.centroid2 = b2TransformPoint( xf2, fastShape->localCentroid );++		b2AABB box1 = fastShape->aabb;++		// xf2 is in the base frame, compute the tight box near the origin then lift to world+		b2AABB box2 = b2OffsetAABB( b2ComputeShapeAABB( fastShape, b2MakeWorldTransform( xf2 ) ), base );++		// Store this to avoid double computation in the case there is no impact event+		fastShape->aabb = box2;++		// No continuous collision for sensors (but still need the updated bounds)+		if ( fastShape->sensorIndex != B2_NULL_INDEX )+		{+			continue;+		}++		b2AABB sweptBox = b2AABB_Union( box1, box2 );++		b2DynamicTree_OverlapAABB( staticTree, sweptBox, B2_DEFAULT_MASK_BITS, b2ContinuousQueryCallback, &context );++		if ( isBullet )+		{+			b2DynamicTree_OverlapAABB( kinematicTree, sweptBox, B2_DEFAULT_MASK_BITS, b2ContinuousQueryCallback, &context );+			b2DynamicTree_OverlapAABB( dynamicTree, sweptBox, B2_DEFAULT_MASK_BITS, b2ContinuousQueryCallback, &context );+		}+	}++	const float speculativeDistance = B2_SPECULATIVE_DISTANCE;++	if ( context.fraction < 1.0f )+	{+		// Handle time of impact event+		b2Rot q = b2NLerp( sweep.q1, sweep.q2, context.fraction );+		b2Vec2 c = b2Lerp( sweep.c1, sweep.c2, context.fraction );+		b2Vec2 origin = b2Sub( c, b2RotateVector( q, sweep.localCenter ) );++		// Advance body, lifting the base frame result back to world+		fastBodySim->transform.q = q;+		fastBodySim->transform.p = b2OffsetPos( base, origin );+		fastBodySim->center = b2OffsetPos( base, c );+		fastBodySim->rotation0 = q;+		fastBodySim->center0 = fastBodySim->center;++		// Warning: writing to the body sim flags means we should not read from other body sim flags in this function.+		fastBodySim->flags |= b2_hadTimeOfImpact;++		// Timeloss means there is a lost gravity contribution. Other forces and torques are ignored for now.+		b2BodyState* fastBodyState = b2Array_Get( awakeSet->bodyStates, bodySimIndex );+		b2Vec2 v = fastBodyState->linearVelocity;+		float timeLoss = ( 1.0f - context.fraction ) * dt;+		b2Vec2 dv = b2MulSV( -timeLoss * fastBodySim->gravityScale, world->gravity );+		dv.x = ( fastBodyState->flags & b2_lockLinearX ) ? 0.0f : dv.x;+		dv.y = ( fastBodyState->flags & b2_lockLinearY ) ? 0.0f : dv.y;+		fastBodyState->linearVelocity = b2Add( v, dv );++		// Update body move event+		b2BodyMoveEvent* event = b2Array_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 != B2_NULL_INDEX )+		{+			b2Shape* shape = b2Array_Get( world->shapes, shapeId );++			// Must recompute aabb at the interpolated transform+			b2AABB aabb = b2ComputeFatShapeAABB( shape, fastBodySim->transform, speculativeDistance );+			shape->aabb = aabb;++			b2AABB* shapeFatAABB = world->fatAABBs.data + shapeId;+			if ( b2AABB_Contains( *shapeFatAABB, aabb ) == false )+			{+				float margin = shape->aabbMargin;+				b2AABB fatAABB;+				fatAABB.lowerBound.x = aabb.lowerBound.x - margin;+				fatAABB.lowerBound.y = aabb.lowerBound.y - margin;+				fatAABB.upperBound.x = aabb.upperBound.x + margin;+				fatAABB.upperBound.y = aabb.upperBound.y + margin;+				*shapeFatAABB = fatAABB;++				// Regular bodies mark the hierarchy as enlarged using atomic operations.+				// Bullets are handled separately at a later stage.+				if ( isBullet == false )+				{+					b2BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, fatAABB );+				}+				else+				{+					fastBodySim->flags |= b2_enlargeBulletBounds;+				}+			}++			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 != B2_NULL_INDEX )+		{+			b2Shape* shape = b2Array_Get( world->shapes, shapeId );++			// shape->aabb is still valid from above++			b2AABB* shapeFatAABB = world->fatAABBs.data + shapeId;+			if ( b2AABB_Contains( *shapeFatAABB, shape->aabb ) == false )+			{+				float margin = shape->aabbMargin;++				// Note: far from the origin the margin can be snapped to the nearest ULP.+				// This relevant for DP mode. So we lose the broad-phase hysteresis.+				b2AABB fatAABB;+				fatAABB.lowerBound.x = shape->aabb.lowerBound.x - margin;+				fatAABB.lowerBound.y = shape->aabb.lowerBound.y - margin;+				fatAABB.upperBound.x = shape->aabb.upperBound.x + margin;+				fatAABB.upperBound.y = shape->aabb.upperBound.y + margin;+				*shapeFatAABB = fatAABB;++				if ( isBullet == false )+				{+					b2BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, fatAABB );+				}+				else+				{+					fastBodySim->flags |= b2_enlargeBulletBounds;+				}+			}++			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 )+		{+			b2Array_Push( taskContext->sensorHits, context.sensorHits[i] );+		}+	}++	b2TracyCZoneEnd( ccd );+}++// Implements b2ParallelForCallback+static void b2FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, void* context )+{+	b2TracyCZoneNC( finalize_transforms, "Finalize", b2_colorMediumSeaGreen, true );++	b2StepContext* stepContext = context;+	b2World* world = stepContext->world;+	b2Body* bodies = world->bodies.data;+	b2BodyState* states = stepContext->states;+	b2BodySim* sims = stepContext->sims;++	B2_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 should already have the correct size+	b2BodyMoveEvent* moveEvents = world->bodyMoveEvents.data;++	b2TaskContext* taskContext = world->taskContexts.data + workerIndex;+	b2BitSet* enlargedSimBitSet = &taskContext->enlargedSimBitSet;+	b2BitSet* awakeIslandBitSet = &taskContext->awakeIslandBitSet;++	const float speculativeDistance = B2_SPECULATIVE_DISTANCE;++	for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex )+	{+		b2BodyState* state = states + simIndex;+		b2BodySim* sim = sims + simIndex;++		b2Vec2 v = state->linearVelocity;+		float w = state->angularVelocity;++		if ( b2IsValidVec2( v ) == false || b2IsValidFloat( w ) == false )+		{+			b2Body* debugBody = bodies + sim->bodyId;+			b2Log( "unstable: %s\n", debugBody->name );+		}++		B2_ASSERT( b2IsValidVec2( v ) );+		B2_ASSERT( b2IsValidFloat( w ) );++		sim->center = b2OffsetPos( sim->center, state->deltaPosition );+		sim->transform.q = b2NormalizeRot( b2MulRot( state->deltaRotation, sim->transform.q ) );++		// Use the velocity of the farthest point on the body to account for rotation.+		float maxVelocity = b2Length( v ) + b2AbsFloat( w ) * sim->maxExtent;++		// Sleep needs to observe position correction as well as true velocity.+		float maxDeltaPosition = b2Length( state->deltaPosition ) + b2AbsFloat( state->deltaRotation.s ) * sim->maxExtent;++		// Position correction is not as important for sleep as true velocity.+		float positionSleepFactor = 0.5f;+		float sleepVelocity = b2MaxFloat( maxVelocity, positionSleepFactor * invTimeStep * maxDeltaPosition );++		// reset state deltas+		state->deltaPosition = b2Vec2_zero;+		state->deltaRotation = b2Rot_identity;++		sim->transform.p = b2OffsetPos( sim->center, b2Neg( b2RotateVector( sim->transform.q, sim->localCenter ) ) );++		// cache miss here, however I need the shape list below+		b2Body* body = bodies + sim->bodyId;+		body->bodyMoveIndex = simIndex;+		moveEvents[simIndex].transform = sim->transform;+		moveEvents[simIndex].bodyId = (b2BodyId){ sim->bodyId + 1, worldId, body->generation };+		moveEvents[simIndex].userData = body->userData;+		moveEvents[simIndex].fellAsleep = false;++		// reset applied force and torque+		sim->force = b2Vec2_zero;+		sim->torque = 0.0f;++		// If you hit this then it means you deferred mass computation but never called b2Body_UpdateMassFromShapes+		B2_ASSERT( ( body->flags & b2_dirtyMass ) == 0 );++		// Clear the transient flags (fast, speed capped, had TOI). These flags are conditionally set+		// as part of the code below.+		body->flags &= ~b2_bodyTransientFlags;+		sim->flags &= ~( b2_isFast | b2_bodyTransientFlags );++		// The body state flag knows about speed capping (used for debug draw).+		body->flags |= ( state->flags & b2_isSpeedCapped );+		state->flags &= ~b2_bodyTransientFlags;++		if ( enableSleep == false || ( body->flags & b2_enableSleep ) == 0 || sleepVelocity > body->sleepThreshold )+		{+			// Body is not sleepy+			body->sleepTime = 0.0f;++			float safetyFactor = body->safetyFactor;+			float maxMotion = b2MaxFloat( maxDeltaPosition, maxVelocity * timeStep );+			if ( body->type == b2_dynamicBody && enableContinuous && maxMotion > safetyFactor * sim->minExtent )+			{+				// This flag is used for debug draw and contact recycling.+				sim->flags |= b2_isFast;++				// Store fast bullets for processing later.+				// This is deterministic because the order of TOI sweeps doesn't matter.+				if ( sim->flags & b2_isBullet )+				{+					int bulletIndex = b2AtomicFetchAddInt( &stepContext->bulletBodyCount, 1 );+					stepContext->bulletBodies[bulletIndex] = simIndex;+				}+				else+				{+					b2SolveContinuous( world, simIndex, taskContext, stepContext->dt );+				}+			}+			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;+		}++		// Any single body in an island can keep it awake+		b2Island* island = b2Array_Get( world->islands, body->islandId );+		if ( body->sleepTime < B2_TIME_TO_SLEEP )+		{+			// keep island awake+			int islandIndex = island->localIndex;+			b2SetBit( awakeIslandBitSet, islandIndex );+		}+		else if ( island->constraintRemoveCount > 0 )+		{+			// Body wants to sleep but its island needs splitting first. 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 ) )+			{+				taskContext->splitIslandId = body->islandId;+				taskContext->splitSleepTime = body->sleepTime;+			}+		}++		// Update shapes AABBs+		b2WorldTransform transform = sim->transform;+		bool isFast = ( sim->flags & b2_isFast ) != 0;+		int shapeId = body->headShapeId;+		while ( shapeId != B2_NULL_INDEX )+		{+			b2Shape* shape = b2Array_Get( world->shapes, shapeId );++			if ( isFast )+			{+				// For fast non-bullet bodies the AABB has already been updated in b2SolveContinuous+				// For fast bullet bodies the AABB will be updated at a later stage++				// Add to enlarged shapes regardless of AABB changes.+				// Bit-set to keep the move array sorted+				b2SetBit( enlargedSimBitSet, simIndex );+			}+			else+			{+				// Update bounds for slow bodies, including slow bullets.++				b2AABB aabb = b2ComputeFatShapeAABB( shape, transform, speculativeDistance );+				shape->aabb = aabb;++				b2AABB* shapeFatAABB = world->fatAABBs.data + shapeId;+				if ( b2AABB_Contains( *shapeFatAABB, aabb ) == false )+				{+					float margin = shape->aabbMargin;+					b2AABB fatAABB;+					fatAABB.lowerBound.x = aabb.lowerBound.x - margin;+					fatAABB.lowerBound.y = aabb.lowerBound.y - margin;+					fatAABB.upperBound.x = aabb.upperBound.x + margin;+					fatAABB.upperBound.y = aabb.upperBound.y + margin;+					*shapeFatAABB = fatAABB;++					// Mark the hierarchy as enlarged using atomic operations.+					b2BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, fatAABB );++					// Bit-set to keep the move array sorted+					b2SetBit( enlargedSimBitSet, simIndex );+				}+			}++			shapeId = shape->nextShapeId;+		}+	}++	b2TracyCZoneEnd( finalize_transforms );+}++typedef struct b2BlockDim+{+	// number of items per block (except last block)+	int size;++	// total number of blocks+	int count;+} b2BlockDim;++// 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 b2BlockDim b2ComputeBlockCount( int itemCount, int minSize, int maxBlockCount )+{+	b2BlockDim 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;++	B2_ASSERT( dim.count >= 1 );+	B2_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 b2ComputeBlockCount. The atomic claim counter is zeroed+// so workers can CAS (0, 1) on the first stage that owns these blocks.+static void b2InitBlocks( b2SyncBlock* blocks, b2BlockDim dim, int itemCount, uint8_t blockType, uint8_t colorIndex )+{+	if ( dim.count == 0 )+	{+		return;+	}++	B2_ASSERT( itemCount >= dim.count );++	// Compute the number of elements per block+	int blockSize = dim.size;++	// Simulation too big+	B2_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;+		b2AtomicStoreInt( &blocks[i].syncIndex, 0 );+	}++	// The last block may not be full+	blocks[dim.count - 1].block.count = (uint16_t)( itemCount - ( dim.count - 1 ) * blockSize );++	B2_VALIDATE( blocks[dim.count - 1].block.count <= blockSize );+	B2_VALIDATE( ( dim.count - 1 ) * dim.size + blocks[dim.count - 1].block.count == itemCount );+}++static inline b2SolverStage* b2InitStage( b2SolverStage* stage, b2SolverStageType type, b2SyncBlock* blocks, int blockCount,+										  uint8_t colorIndex )+{+	stage->type = type;+	stage->blocks = blocks;+	stage->blockCount = blockCount;+	stage->colorIndex = colorIndex;+	b2AtomicStoreInt( &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 b2SyncBlock array so the per-block syncIndex+// grows monotonically across stages within that color.+static b2SolverStage* b2InitColorStages( b2SolverStage* stage, b2SolverStageType type, int iterations, int activeColorCount,+										 b2SyncBlock** colorBlocks, int* colorBlockCounts, int* activeColorIndices )+{+	for ( int j = 0; j < iterations; ++j )+	{+		for ( int i = 0; i < activeColorCount; ++i )+		{+			stage = b2InitStage( stage, type, colorBlocks[i], colorBlockCounts[i], (uint8_t)activeColorIndices[i] );+		}+	}+	return stage;+}++static void b2ExecuteBlock( b2SolverStage* stage, b2StepContext* context, b2SolverBlock block, int workerIndex )+{+	b2SolverStageType stageType = stage->type;+	b2SolverBlockType blockType = block.blockType;++	switch ( stageType )+	{+		case b2_stagePrepareJoints:+			b2PrepareJointsTask( block, context );+			break;++		case b2_stagePrepareContacts:+			b2PrepareContacts_Wide( block, context );+			break;++		case b2_stageIntegrateVelocities:+			b2IntegrateVelocitiesTask( block, context );+			break;++		case b2_stageWarmStart:+			if ( blockType == b2_graphContactBlock )+			{+				b2WarmStartContacts_Wide( block, context );+			}+			else if ( blockType == b2_graphJointBlock )+			{+				b2WarmStartJointsTask( block, context );+			}+			break;++		case b2_stageSolve:+			if ( blockType == b2_graphContactBlock )+			{+				b2PushContacts_Wide( block, context );+			}+			else if ( blockType == b2_graphJointBlock )+			{+				bool useBias = true;+				b2SolveJointsTask( block, context, useBias, workerIndex );+			}+			break;++		case b2_stageIntegratePositions:+			b2IntegratePositionsTask( block, context );+			break;++		case b2_stageRelax:+			if ( blockType == b2_graphContactBlock )+			{+				b2SolveContacts_Wide( block, context );+			}+			else if ( blockType == b2_graphJointBlock )+			{+				bool useBias = false;+				b2SolveJointsTask( block, context, useBias, workerIndex );+			}+			break;++		case b2_stageRestitution:+			if ( blockType == b2_graphContactBlock )+			{+				b2ApplyRestitution_Wide( block, context );+			}+			break;++		case b2_stageStoreImpulses:+			b2StoreImpulses_Wide( 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 : B2_NULL_INDEX;+	}++	int blocksPerWorker = blockCount / workerCount;+	int remainder = blockCount - blocksPerWorker * workerCount;+	return blocksPerWorker * workerIndex + b2MinInt( 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 b2ExecuteStage( b2SolverStage* stage, b2StepContext* context, int previousSyncIndex, int syncIndex, int workerIndex )+{+	int completedCount = 0;+	b2SyncBlock* blocks = stage->blocks;+	int blockCount = stage->blockCount;++	int startIndex = GetWorkerStartIndex( workerIndex, blockCount, context->workerCount );+	if ( startIndex == B2_NULL_INDEX )+	{+		return;+	}++	B2_ASSERT( 0 <= startIndex && startIndex < blockCount );++	int blockIndex = startIndex;+	for ( int i = 0; i < blockCount; ++i )+	{+		// Read before attempting the CAS.+		if ( b2AtomicLoadInt( &blocks[blockIndex].syncIndex ) == previousSyncIndex &&+			 b2AtomicCompareExchangeInt( &blocks[blockIndex].syncIndex, previousSyncIndex, syncIndex ) )+		{+			B2_ASSERT( stage->type != b2_stagePrepareContacts || syncIndex < 2 );+			B2_ASSERT( completedCount < blockCount );++			// Pass the descriptor by value -- the wrapping b2SyncBlock holds the atomic+			// syncIndex but we only copy .block, so the struct copy never aliases the CAS target.+			b2ExecuteBlock( stage, context, blocks[blockIndex].block, workerIndex );+			completedCount += 1;+		}++		blockIndex += 1;+		if ( blockIndex >= blockCount )+		{+			blockIndex = 0;+		}+	}++	(void)b2AtomicFetchAddInt( &stage->completionCount, completedCount );+}++// Execute a stage on worker 0 (main thread).+static void b2ExecuteMainStage( b2SolverStage* stage, b2StepContext* context, uint32_t syncBits )+{+	int blockCount = stage->blockCount;+	if ( blockCount == 0 )+	{+		return;+	}++	int workerIndex = 0;++	if ( blockCount == 1 )+	{+		b2ExecuteBlock( stage, context, stage->blocks[0].block, workerIndex );+	}+	else+	{+		b2AtomicStoreU32( &context->atomicSyncBits, syncBits );++		int syncIndex = ( syncBits >> 16 ) & 0xFFFF;+		B2_ASSERT( syncIndex > 0 );+		int previousSyncIndex = syncIndex - 1;++		b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex );++		// Spin waiting for thieves to finish+		while ( b2AtomicLoadInt( &stage->completionCount ) != blockCount )+		{+			b2Pause();+		}++		b2AtomicStoreInt( &stage->completionCount, 0 );+	}+}++// Parallel solver task+static void b2SolverTask( void* taskContext )+{+	b2WorkerContext* workerContext = taskContext;+	int workerIndex = workerContext->workerIndex;+	b2StepContext* context = workerContext->context;+	int activeColorCount = context->activeColorCount;+	b2SolverStage* stages = context->stages;+	b2Profile* profile = &context->world->profile;++	if ( workerIndex == 0 )+	{+		// The orchestrator slot is a race. The calling thread of b2World_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 ( b2AtomicCompareExchangeInt( &context->mainClaimed, 0, 1 ) == false )+		{+			return;+		}++		// Main thread synchronizes the workers and does work itself.+		//+		// This single task is able to fully complete all work even if all other workers are+		// blocked, so a fully serial task system still drives the simulation forward.++		// 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.++		/*+		Stage sequence+		b2_stagePrepareJoints,+		b2_stagePrepareContacts,+		b2_stageIntegrateVelocities,+		b2_stageWarmStart,+		b2_stageSolve,+		b2_stageIntegratePositions,+		b2_stageRelax,+		b2_stageRestitution,+		b2_stageStoreImpulses+		*/++		uint64_t ticks = b2GetTicks();++		int bodySyncIndex = 1;+		int stageIndex = 0;++		// Prepare joint constraints+		uint32_t jointSyncIndex = 1;+		uint32_t syncBits = ( jointSyncIndex << 16 ) | stageIndex;+		B2_ASSERT( stages[stageIndex].type == b2_stagePrepareJoints );+		b2ExecuteMainStage( stages + stageIndex, context, syncBits );+		stageIndex += 1;+		jointSyncIndex += 1;++		// Prepare contact constraints+		uint32_t contactSyncIndex = 1;+		syncBits = ( contactSyncIndex << 16 ) | stageIndex;+		B2_ASSERT( stages[stageIndex].type == b2_stagePrepareContacts );+		b2ExecuteMainStage( stages + stageIndex, context, syncBits );+		stageIndex += 1;+		contactSyncIndex += 1;++		// Single-threaded overflow work. These constraints don't fit in the graph coloring.+		b2PrepareJoints_Overflow( context );+		b2PrepareContacts_Overflow( context );++		profile->prepareConstraints += b2GetMillisecondsAndReset( &ticks );++		int graphSyncIndex = 1;+		int subStepCount = context->subStepCount;+		for ( int subStepIndex = 0; subStepIndex < subStepCount; ++subStepIndex )+		{+			// stage index restarted each iteration+			// syncBits still increases monotonically because the upper bits increase each iteration+			int iterationStageIndex = stageIndex;++			// Integrate velocities+			syncBits = ( bodySyncIndex << 16 ) | iterationStageIndex;+			B2_ASSERT( stages[iterationStageIndex].type == b2_stageIntegrateVelocities );+			b2ExecuteMainStage( stages + iterationStageIndex, context, syncBits );+			iterationStageIndex += 1;+			bodySyncIndex += 1;++			profile->integrateVelocities += b2GetMillisecondsAndReset( &ticks );++			// Warm start constraints+			b2WarmStartJoints_Overflow( context );+			b2WarmStartContacts_Overflow( context );++			for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )+			{+				syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex;+				B2_ASSERT( stages[iterationStageIndex].type == b2_stageWarmStart );+				b2ExecuteMainStage( stages + iterationStageIndex, context, syncBits );+				iterationStageIndex += 1;+			}+			graphSyncIndex += 1;++			profile->warmStart += b2GetMillisecondsAndReset( &ticks );++			// Solve constraints+			bool useBias = true;+			for ( int j = 0; j < ITERATIONS; ++j )+			{+				// Overflow constraints have lower priority. Typically these are dynamic-vs-dynamic.+				b2SolveJoints_Overflow( context, useBias );+				b2PushContacts_Overflow( context );++				for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )+				{+					syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex;+					B2_ASSERT( stages[iterationStageIndex].type == b2_stageSolve );+					b2ExecuteMainStage( stages + iterationStageIndex, context, syncBits );+					iterationStageIndex += 1;+				}+				graphSyncIndex += 1;+			}++			profile->solveImpulses += b2GetMillisecondsAndReset( &ticks );++			// Integrate positions+			B2_ASSERT( stages[iterationStageIndex].type == b2_stageIntegratePositions );+			syncBits = ( bodySyncIndex << 16 ) | iterationStageIndex;+			b2ExecuteMainStage( stages + iterationStageIndex, context, syncBits );+			iterationStageIndex += 1;+			bodySyncIndex += 1;++			profile->integratePositions += b2GetMillisecondsAndReset( &ticks );++			// Relax constraints+			useBias = false;+			for ( int j = 0; j < RELAX_ITERATIONS; ++j )+			{+				b2SolveJoints_Overflow( context, useBias );+				b2SolveContacts_Overflow( context );+				for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )+				{+					syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex;+					B2_ASSERT( stages[iterationStageIndex].type == b2_stageRelax );+					b2ExecuteMainStage( stages + iterationStageIndex, context, syncBits );+					iterationStageIndex += 1;+				}+				graphSyncIndex += 1;+			}++			profile->relaxImpulses += b2GetMillisecondsAndReset( &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;++		int restitutionIterations = context->world->restitutionIterations;+		if ( restitutionIterations > 0 && b2AtomicLoadInt( &context->anyRestitution ) != 0 )+		{+			for ( int j = 0; j < restitutionIterations; ++j )+			{+				b2ApplyRestitution_Overflow( context );++				int iterationStageIndex = stageIndex;+				for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )+				{+					syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex;+					B2_ASSERT( stages[iterationStageIndex].type == b2_stageRestitution );+					b2ExecuteMainStage( stages + iterationStageIndex, context, syncBits );+					iterationStageIndex += 1;+				}+				graphSyncIndex += 1;+			}++			profile->restitution += b2GetMillisecondsAndReset( &ticks );+		}++		stageIndex += activeColorCount;++		// Store impulses+		b2StoreImpulses_Overflow( context );++		syncBits = ( contactSyncIndex << 16 ) | stageIndex;+		B2_ASSERT( stages[stageIndex].type == b2_stageStoreImpulses );+		b2ExecuteMainStage( stages + stageIndex, context, syncBits );++		profile->storeImpulses += b2GetMillisecondsAndReset( &ticks );++		// Signal workers to finish+		b2AtomicStoreU32( &context->atomicSyncBits, UINT_MAX );++		B2_ASSERT( stageIndex + 1 == 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 = b2AtomicLoadU32( &context->atomicSyncBits ) ) == lastSyncBits )+		{+			if ( spinCount > 5 )+			{+				b2Yield();+				spinCount = 0;+			}+			else+			{+				// Using the cycle counter helps to account for variation in mm_pause timing across different+				// CPUs. However, this is X64 only.+				// uint64_t prev = __rdtsc();+				// do+				//{+				//	_mm_pause();+				//}+				// while ((__rdtsc() - prev) < maxSpinTime);+				// maxSpinTime += 10;++				b2Pause();+				b2Pause();+				spinCount += 1;+			}+		}++		if ( syncBits == UINT_MAX )+		{+			// sentinel hit+			break;+		}++		int stageIndex = syncBits & 0xFFFF;+		B2_ASSERT( stageIndex < context->stageCount );++		int syncIndex = ( syncBits >> 16 ) & 0xFFFF;+		B2_ASSERT( syncIndex > 0 );++		int previousSyncIndex = syncIndex - 1;++		b2SolverStage* stage = stages + stageIndex;+		b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex );++		lastSyncBits = syncBits;+	}+}++static void b2BulletBodyTask( int startIndex, int endIndex, int workerIndex, void* context )+{+	b2TracyCZoneNC( bullet_body_task, "Bullet", b2_colorLightSkyBlue, true );++	b2StepContext* stepContext = context;+	b2TaskContext* taskContext = stepContext->world->taskContexts.data + workerIndex;++	B2_ASSERT( startIndex <= endIndex );++	for ( int i = startIndex; i < endIndex; ++i )+	{+		int simIndex = stepContext->bulletBodies[i];+		b2SolveContinuous( stepContext->world, simIndex, taskContext, stepContext->dt );+	}++	b2TracyCZoneEnd( bullet_body_task );+}++// Solve with graph coloring+void b2Solve( b2World* world, b2StepContext* stepContext )+{+	int simdShift = world->simdWidth == 8 ? 3 : 2;++	// Only count steps that advance the simulation+	world->stepIndex += 1;++	// Are there any awake bodies? This scenario should not be important for profiling.+	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	int awakeBodyCount = awakeSet->bodySims.count;+	if ( awakeBodyCount == 0 )+	{+		return;+	}++	// Solve constraints using graph coloring+	{+		b2TracyCZoneNC( solver_setup, "Solver Setup", b2_colorDarkOrange, true );+		uint64_t setupTicks = b2GetTicks();++		// Prepare buffers for bullets+		b2AtomicStoreInt( &stepContext->bulletBodyCount, 0 );+		stepContext->bulletBodies = b2StackAlloc( &world->stack, awakeBodyCount * sizeof( int ), "bullet bodies" );++		b2ConstraintGraph* graph = &world->constraintGraph;+		b2GraphColor* 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 < B2_GRAPH_COLOR_COUNT - 1; ++i )+		{+			int perColorContactCount = colors[i].contactSims.count;+			int perColorJointCount = colors[i].jointSims.count;+			int occupancyCount = perColorContactCount + perColorJointCount;+			activeColorCount += occupancyCount > 0 ? 1 : 0;+		}++		// prepare for move events+		b2Array_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;+		b2BlockDim bodyDim = b2ComputeBlockCount( 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 wide contact blocks and joint blocks+		int activeColorIndices[B2_GRAPH_COLOR_COUNT];+		int colorContactCounts[B2_GRAPH_COLOR_COUNT];+		int colorJointCounts[B2_GRAPH_COLOR_COUNT];+		b2BlockDim graphContactDims[B2_GRAPH_COLOR_COUNT];+		b2BlockDim graphJointDims[B2_GRAPH_COLOR_COUNT];+		int graphBlockCount = 0;++		// c is the active color index+		int wideContactCount = 0;+		int jointCount = 0;+		int c = 0;+		for ( int i = 0; i < B2_GRAPH_COLOR_COUNT - 1; ++i )+		{+			int colorContactCount = colors[i].contactSims.count;+			int colorJointCount = colors[i].jointSims.count;++			if ( colorContactCount + colorJointCount == 0 )+			{+				continue;+			}++			activeColorIndices[c] = i;++			// Ceiling for wide constraint count+			int colorContactCountW = colorContactCount > 0 ? ( ( colorContactCount - 1 ) >> simdShift ) + 1 : 0;+			wideContactCount += colorContactCountW;+			colorContactCounts[c] = colorContactCountW;++			colorJointCounts[c] = colorJointCount;+			jointCount += colorJointCount;++			// Graph solver block dimensions+			graphContactDims[c] = b2ComputeBlockCount( colorContactCountW, minContactsPerBlock, maxBlockCount );+			graphJointDims[c] = b2ComputeBlockCount( colorJointCount, minJointsPerBlock, maxBlockCount );+			graphBlockCount += 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.+		b2BlockDim contactPrepareDim = b2ComputeBlockCount( wideContactCount, minContactsPerBlock, maxBlockCount );+		b2BlockDim jointPrepareDim = b2ComputeBlockCount( jointCount, minJointsPerBlock, maxBlockCount );++		int wideContactConstraintByteCount = b2GetWideContactConstraintByteCount( world->simdWidth );+		void* wideContactConstraints =+			b2StackAlloc( &world->stack, wideContactCount * wideContactConstraintByteCount, "contact constraint" );++		b2GraphColor* overflow = colors + B2_OVERFLOW_INDEX;+		int overflowCount = overflow->contactSims.count;+		b2ContactConstraint* overflowContacts =+			b2StackAlloc( &world->stack, overflowCount * sizeof( b2ContactConstraint ), "overflow contact constraint" );+		overflow->overflowConstraints = overflowContacts;++		// 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.+		b2ContactPrepareSpan contactPrepareSpans[B2_GRAPH_COLOR_COUNT + 1];+		b2JointPrepareSpan jointPrepareSpans[B2_GRAPH_COLOR_COUNT + 1];++		// Distribute transient constraints to each graph color and prepare spans+		{+			int wideBase = 0;+			int jointBase = 0;+			for ( int i = 0; i < activeColorCount; ++i )+			{+				int j = activeColorIndices[i];+				b2GraphColor* color = colors + j;++				int colorContactCount = color->contactSims.count;+				contactPrepareSpans[i].start = wideBase;+				contactPrepareSpans[i].count = colorContactCount;+				contactPrepareSpans[i].contacts = color->contactSims.data;++				if ( colorContactCount == 0 )+				{+					color->wideConstraints = NULL;+					color->wideConstraintCount = 0;+				}+				else+				{+					color->wideConstraints = (uint8_t*)wideContactConstraints + wideBase * wideContactConstraintByteCount;++					int colorContactCountW = ( ( colorContactCount - 1 ) >> simdShift ) + 1;+					color->wideConstraintCount = colorContactCountW;++					// Zero remainder lanes in the tail wide slot so prepare workers don't need to+					// initialize them.+					if ( ( colorContactCount & ( world->simdWidth - 1 ) ) != 0 )+					{+						memset( (uint8_t*)color->wideConstraints + ( colorContactCountW - 1 ) * wideContactConstraintByteCount, 0,+								wideContactConstraintByteCount );+					}++					wideBase += colorContactCountW;+				}++				jointPrepareSpans[i].start = jointBase;+				jointPrepareSpans[i].count = color->jointSims.count;+				jointPrepareSpans[i].joints = color->jointSims.data;+				jointBase += color->jointSims.count;+			}++			// Sentinel+			contactPrepareSpans[activeColorCount].start = wideContactCount;+			contactPrepareSpans[activeColorCount].count = 0;+			contactPrepareSpans[activeColorCount].contacts = NULL;+			B2_ASSERT( wideBase == wideContactCount );++			jointPrepareSpans[activeColorCount].start = jointCount;+			jointPrepareSpans[activeColorCount].count = 0;+			jointPrepareSpans[activeColorCount].joints = NULL;+			B2_ASSERT( jointBase == jointCount );+		}++		int stageCount = 0;++		// b2_stagePrepareJoints+		stageCount += 1;+		// b2_stagePrepareContacts+		stageCount += 1;+		// b2_stageIntegrateVelocities+		stageCount += 1;+		// b2_stageWarmStart+		stageCount += activeColorCount;+		// b2_stageSolve+		stageCount += ITERATIONS * activeColorCount;+		// b2_stageIntegratePositions+		stageCount += 1;+		// b2_stageRelax+		stageCount += RELAX_ITERATIONS * activeColorCount;+		// b2_stageRestitution+		stageCount += activeColorCount;+		// b2_stageStoreImpulses+		stageCount += 1;++		b2SolverStage* stages = b2StackAlloc( &world->stack, stageCount * sizeof( b2SolverStage ), "stages" );+		b2SyncBlock* bodyBlocks = b2StackAlloc( &world->stack, bodyDim.count * sizeof( b2SyncBlock ), "body blocks" );+		b2SyncBlock* contactBlocks =+			b2StackAlloc( &world->stack, contactPrepareDim.count * sizeof( b2SyncBlock ), "contact blocks" );+		b2SyncBlock* jointBlocks = b2StackAlloc( &world->stack, jointPrepareDim.count * sizeof( b2SyncBlock ), "joint blocks" );+		b2SyncBlock* graphBlocks = b2StackAlloc( &world->stack, graphBlockCount * sizeof( b2SyncBlock ), "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 != B2_NULL_INDEX )+		{+			if ( world->taskCount < B2_MAX_TASKS )+			{+				splitIslandTask = world->enqueueTaskFcn( &b2SplitIslandTask, world, world->userTaskContext );+				world->taskCount += 1;+				world->activeTaskCount += splitIslandTask == NULL ? 0 : 1;+			}+			else+			{+				b2SplitIslandTask( world );+			}+		}++		// Prepare body blocks+		b2InitBlocks( bodyBlocks, bodyDim, awakeBodyCount, b2_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.+		b2InitBlocks( contactBlocks, contactPrepareDim, wideContactCount, b2_contactBlock, UINT8_MAX );+		b2InitBlocks( jointBlocks, jointPrepareDim, jointCount, b2_jointBlock, UINT8_MAX );++		// Prepare graph work blocks. Each color gets joint blocks followed by contact blocks.+		b2SyncBlock* graphColorBlocks[B2_GRAPH_COLOR_COUNT] = { 0 };+		b2SyncBlock* baseGraphBlock = graphBlocks;+		int graphBlockCounts[B2_GRAPH_COLOR_COUNT] = { 0 };+		for ( int i = 0; i < activeColorCount; ++i )+		{+			graphColorBlocks[i] = baseGraphBlock;++			uint8_t colorIndex = (uint8_t)activeColorIndices[i];+			b2InitBlocks( baseGraphBlock, graphJointDims[i], colorJointCounts[i], b2_graphJointBlock, colorIndex );+			baseGraphBlock += graphJointDims[i].count;++			b2InitBlocks( baseGraphBlock, graphContactDims[i], colorContactCounts[i], b2_graphContactBlock, colorIndex );+			baseGraphBlock += graphContactDims[i].count;++			graphBlockCounts[i] = graphJointDims[i].count + graphContactDims[i].count;+		}++		B2_ASSERT( (ptrdiff_t)( baseGraphBlock - graphBlocks ) == graphBlockCount );++		b2SolverStage* stage = stages;+		stage = b2InitStage( stage, b2_stagePrepareJoints, jointBlocks, jointPrepareDim.count, UINT8_MAX );+		stage = b2InitStage( stage, b2_stagePrepareContacts, contactBlocks, contactPrepareDim.count, UINT8_MAX );+		stage = b2InitStage( stage, b2_stageIntegrateVelocities, bodyBlocks, bodyDim.count, UINT8_MAX );+		stage = b2InitColorStages( stage, b2_stageWarmStart, 1, activeColorCount, graphColorBlocks, graphBlockCounts,+								   activeColorIndices );+		stage = b2InitColorStages( stage, b2_stageSolve, ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts,+								   activeColorIndices );+		stage = b2InitStage( stage, b2_stageIntegratePositions, bodyBlocks, bodyDim.count, UINT8_MAX );+		stage = b2InitColorStages( stage, b2_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts,+								   activeColorIndices );+		stage = b2InitColorStages( stage, b2_stageRestitution, 1, activeColorCount, graphColorBlocks, graphBlockCounts,+								   activeColorIndices );+		stage = b2InitStage( stage, b2_stageStoreImpulses, contactBlocks, contactPrepareDim.count, UINT8_MAX );++		B2_ASSERT( (int)( stage - stages ) == stageCount );++		B2_ASSERT( workerCount <= B2_MAX_WORKERS );+		b2WorkerContext workerContext[B2_MAX_WORKERS];++		stepContext->graph = graph;+		stepContext->activeColorCount = activeColorCount;+		stepContext->workerCount = workerCount;+		stepContext->stageCount = stageCount;+		stepContext->stages = stages;+		stepContext->wideContactConstraints = wideContactConstraints;+		stepContext->contactPrepareSpans = contactPrepareSpans;+		stepContext->wideContactCount = wideContactCount;+		stepContext->jointPrepareSpans = jointPrepareSpans;+		b2AtomicStoreU32( &stepContext->atomicSyncBits, 0 );+		b2AtomicStoreInt( &stepContext->mainClaimed, 0 );++		world->profile.solverSetup = b2GetMillisecondsAndReset( &setupTicks );+		b2TracyCZoneEnd( solver_setup );++		b2TracyCZoneNC( solve_constraints, "Solve Constraints", b2_colorIndigo, true );+		uint64_t constraintTicks = b2GetTicks();++		int jointIdCapacity = b2GetIdCapacity( &world->jointIdPool );+		int contactIdCapacity = b2GetIdCapacity( &world->contactIdPool );+		for ( int i = 0; i < workerCount; ++i )+		{+			b2TaskContext* taskContext = b2Array_Get( world->taskContexts, i );+			b2SetBitCountAndClear( &taskContext->jointStateBitSet, jointIdCapacity );+			b2SetBitCountAndClear( &taskContext->hitEventBitSet, contactIdCapacity );+			taskContext->hasHitEvents = false;++			workerContext[i].context = stepContext;+			workerContext[i].workerIndex = i;++			if ( world->taskCount < B2_MAX_TASKS )+			{+				workerContext[i].userTask = world->enqueueTaskFcn( &b2SolverTask, workerContext + i, world->userTaskContext );+				world->taskCount += 1;+				world->activeTaskCount += workerContext[i].userTask == NULL ? 0 : 1;+			}+			else+			{+				workerContext[i].userTask = NULL;+				b2SolverTask( workerContext + i );+			}+		}++		// The calling thread of b2World_Step also enters b2SolverTask as worker 0 and races for the+		// orchestrator slot via the CAS inside. This guarantees progress even when the user's task+		// system can't run the queued worker 0 promptly: it might schedule out of order, have fewer+		// threads than workerCount, or invert priority by parking the calling thread in finishTaskFcn.+		// Whoever wins the CAS becomes the orchestrator; the loser returns and lets the spinner-only+		// path handle workers >0.+		b2WorkerContext callerContext = { stepContext, 0, NULL };+		b2SolverTask( &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 = B2_NULL_INDEX;++		world->profile.constraints = b2GetMillisecondsAndReset( &constraintTicks );+		b2TracyCZoneEnd( solve_constraints );++		b2TracyCZoneNC( update_transforms, "Update Transforms", b2_colorMediumSeaGreen, true );+		uint64_t transformTicks = b2GetTicks();++		// 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 )+		{+			b2TaskContext* taskContext = world->taskContexts.data + i;+			taskContext->sensorHits.count = 0;+			b2SetBitCountAndClear( &taskContext->enlargedSimBitSet, awakeBodyCount );+			b2SetBitCountAndClear( &taskContext->awakeIslandBitSet, awakeIslandCount );+			taskContext->splitIslandId = B2_NULL_INDEX;+			taskContext->splitSleepTime = 0.0f;+		}++		// 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;+		}++		b2ValidateNoEnlarged( &world->broadPhase );++		// Finalize bodies. Must happen after the constraint solver and after island splitting.+		b2ParallelFor( world, &b2FinalizeBodiesTask, awakeBodyCount, 64, stepContext );++		b2StackFree( &world->stack, graphBlocks );+		b2StackFree( &world->stack, jointBlocks );+		b2StackFree( &world->stack, contactBlocks );+		b2StackFree( &world->stack, bodyBlocks );+		b2StackFree( &world->stack, stages );+		b2StackFree( &world->stack, overflowContacts );+		b2StackFree( &world->stack, wideContactConstraints );++		world->profile.transforms = b2GetMilliseconds( transformTicks );+		b2TracyCZoneEnd( update_transforms );+	}++	// Report joint events+	{+		b2TracyCZoneNC( joint_events, "Joint Events", b2_colorPeru, true );+		uint64_t jointEventTicks = b2GetTicks();++		// Gather bits for all joints that have force/torque events+		b2BitSet* jointStateBitSet = &world->taskContexts.data[0].jointStateBitSet;+		for ( int i = 1; i < world->workerCount; ++i )+		{+			b2InPlaceUnion( jointStateBitSet, &world->taskContexts.data[i].jointStateBitSet );+		}++		{+			uint32_t wordCount = jointStateBitSet->blockCount;+			uint64_t* bits = jointStateBitSet->bits;++			b2Joint* 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 = b2CTZ64( word );+					int jointId = (int)( 64 * k + ctz );++					B2_ASSERT( jointId < world->joints.capacity );++					b2Joint* joint = jointArray + jointId;++					B2_ASSERT( joint->setIndex == b2_awakeSet );++					b2JointEvent event = {+						.jointId =+							{+								.index1 = jointId + 1,+								.world0 = worldIndex0,+								.generation = joint->generation,+							},+						.userData = joint->userData,+					};++					b2Array_Push( world->jointEvents, event );++					// Clear the smallest set bit+					word = word & ( word - 1 );+				}+			}+		}++		world->profile.jointEvents = b2GetMilliseconds( jointEventTicks );+		b2TracyCZoneEnd( joint_events );+	}++	// Report hit events+	{+		b2TracyCZoneNC( hit_events, "Hit Events", b2_colorRosyBrown, true );+		uint64_t hitTicks = b2GetTicks();++		B2_ASSERT( world->contactHitEvents.count == 0 );++		// Fast path: skip if no worker flagged any hit-event candidates during impulse storing.+		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.+			b2BitSet* hitEventBitSet = &world->taskContexts.data[0].hitEventBitSet;+			for ( int i = 1; i < world->workerCount; ++i )+			{+				if ( world->taskContexts.data[i].hasHitEvents )+				{+					b2InPlaceUnion( hitEventBitSet, &world->taskContexts.data[i].hitEventBitSet );+				}+			}++			float threshold = world->hitEventThreshold;+			b2GraphColor* colors = world->constraintGraph.colors;+			b2Contact* contactArray = world->contacts.data;+			b2Shape* shapeArray = world->shapes.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 = b2CTZ64( word );+					int contactId = (int)( 64 * k + ctz );++					b2Contact* contact = contactArray + contactId;+					B2_ASSERT( contact->setIndex == b2_awakeSet && contact->colorIndex != B2_NULL_INDEX );++					b2GraphColor* color = colors + contact->colorIndex;+					b2ContactSim* contactSim = color->contactSims.data + contact->localIndex;++					b2ContactHitEvent event = { 0 };+					event.approachSpeed = threshold;++					b2ManifoldPoint* bestPoint = NULL;+					int pointCount = contactSim->manifold.pointCount;+					for ( int p = 0; p < pointCount; ++p )+					{+						b2ManifoldPoint* mp = contactSim->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;+							bestPoint = mp;+						}+					}++					B2_VALIDATE( bestPoint != NULL );++					if ( bestPoint != NULL )+					{+						event.normal = contactSim->manifold.normal;++						b2Shape* shapeA = shapeArray + contactSim->shapeIdA;+						b2Shape* shapeB = shapeArray + contactSim->shapeIdB;++						// World contact point reconstructed from a body center of mass and the matching+						// anchor. The anchors were built with the manifold, so a body that has moved since+						// drags the point with it. A static body has not moved, prefer one so the common+						// case of a fast body striking the world stays exact.+						b2Body* bodyA = b2Array_Get( world->bodies, shapeA->bodyId );+						b2Body* bodyB = b2Array_Get( world->bodies, shapeB->bodyId );+						if ( bodyA->type != b2_staticBody && bodyB->type == b2_staticBody )+						{+							b2BodySim* bodySimB = b2GetBodySim( world, bodyB );+							event.point = b2OffsetPos( bodySimB->center, bestPoint->anchorB );+						}+						else+						{+							b2BodySim* bodySimA = b2GetBodySim( world, bodyA );+							event.point = b2OffsetPos( bodySimA->center, bestPoint->anchorA );+						}++						event.shapeIdA = (b2ShapeId){ shapeA->id + 1, worldId, shapeA->generation };+						event.shapeIdB = (b2ShapeId){ shapeB->id + 1, worldId, shapeB->generation };++						event.contactId = (b2ContactId){+							.index1 = contact->contactId + 1,+							.world0 = worldId,+							.padding = 0,+							.generation = contact->generation,+						};++						b2Array_Push( world->contactHitEvents, event );+					}++					// Clear the smallest set bit+					word = word & ( word - 1 );+				}+			}+		}++		world->profile.hitEvents = b2GetMilliseconds( hitTicks );+		b2TracyCZoneEnd( hit_events );+	}++	{+		b2TracyCZoneNC( refit_bvh, "Refit BVH", b2_colorFireBrick, true );+		uint64_t refitTicks = b2GetTicks();++		b2BroadPhase* bp = &world->broadPhase;+		b2DynamicTree_Refit( bp->trees + b2_kinematicBody );+		b2DynamicTree_Refit( bp->trees + b2_dynamicBody );++		b2ValidateBroadphase( &world->broadPhase );++		world->profile.refit = b2GetMilliseconds( refitTicks );+		b2TracyCZoneEnd( refit_bvh );+	}++	// Bullets are processed after the broad-phase refit so they can query the+	// final non-bullet world.+	int bulletBodyCount = b2AtomicLoadInt( &stepContext->bulletBodyCount );+	if ( bulletBodyCount > 0 )+	{+		b2TracyCZoneNC( bullets, "Bullets", b2_colorLightYellow, true );+		uint64_t bulletTicks = b2GetTicks();++		// Fast bullet bodies+		// Note: a bullet body may be moving slow+		int minRange = 8;+		b2ParallelFor( world, &b2BulletBodyTask, bulletBodyCount, minRange, stepContext );++		b2BroadPhase* broadPhase = &world->broadPhase;+		b2DynamicTree* dynamicTree = broadPhase->trees + b2_dynamicBody;++		// Fast array access is important here+		b2Body* bodyArray = world->bodies.data;+		b2BodySim* bodySimArray = awakeSet->bodySims.data;+		b2Shape* shapeArray = world->shapes.data;+		int* bulletBodySimIndices = stepContext->bulletBodies;++		// Serially enlarge broad-phase proxies for bullet shapes.+		// This loop has non-deterministic order but it shouldn't affect the result.+		for ( int i = 0; i < bulletBodyCount; ++i )+		{+			b2BodySim* bulletBodySim = bodySimArray + bulletBodySimIndices[i];++			// It is worth tracking this flag because bullets may be moving slowly.+			if ( ( bulletBodySim->flags & b2_enlargeBulletBounds ) == 0 )+			{+				continue;+			}++			// Clear flag+			bulletBodySim->flags &= ~b2_enlargeBulletBounds;++			int bodyId = bulletBodySim->bodyId;+			B2_ASSERT( 0 <= bodyId && bodyId < world->bodies.count );+			b2Body* bulletBody = bodyArray + bodyId;++			int shapeId = bulletBody->headShapeId;+			while ( shapeId != B2_NULL_INDEX )+			{+				b2Shape* shape = shapeArray + shapeId;+				int proxyKey = shape->proxyKey;+				int proxyId = B2_PROXY_ID( proxyKey );+				B2_VALIDATE( B2_PROXY_TYPE( proxyKey ) == b2_dynamicBody );++				b2AABB treeAABB = b2DynamicTree_GetProxyBounds( dynamicTree, proxyId );+				b2AABB shapeFatAABB = world->fatAABBs.data[shapeId];++				// Double check containment because a bullet can have multiple+				// shapes and maybe just one needs an update.+				if ( b2AABB_Contains( treeAABB, shapeFatAABB ) == false )+				{+					b2DynamicTree_EnlargeProxy( dynamicTree, proxyId, shapeFatAABB );+				}++				shapeId = shape->nextShapeId;+			}+		}++		world->profile.bullets = b2GetMilliseconds( bulletTicks );+		b2TracyCZoneEnd( bullets );+	}++	// Need to free this even if no bullets got processed.+	b2StackFree( &world->stack, stepContext->bulletBodies );+	stepContext->bulletBodies = NULL;+	b2AtomicStoreInt( &stepContext->bulletBodyCount, 0 );++	// Report sensor hits. This may include bullets sensor hits.+	{+		b2TracyCZoneNC( sensor_hits, "Sensor Hits", b2_colorPowderBlue, true );+		uint64_t sensorHitTicks = b2GetTicks();++		int workerCount = world->workerCount;+		B2_ASSERT( workerCount == world->taskContexts.count );++		for ( int i = 0; i < workerCount; ++i )+		{+			b2TaskContext* taskContext = world->taskContexts.data + i;+			int hitCount = taskContext->sensorHits.count;+			b2SensorHit* hits = taskContext->sensorHits.data;++			for ( int j = 0; j < hitCount; ++j )+			{+				b2SensorHit hit = hits[j];+				b2Shape* sensorShape = b2Array_Get( world->shapes, hit.sensorId );+				b2Shape* visitor = b2Array_Get( world->shapes, hit.visitorId );++				b2Sensor* sensor = b2Array_Get( world->sensors, sensorShape->sensorIndex );+				b2Visitor shapeRef = {+					.shapeId = hit.visitorId,+					.generation = visitor->generation,+				};+				b2Array_Push( sensor->hits, shapeRef );+			}+		}++		world->profile.sensorHits = b2GetMilliseconds( sensorHitTicks );+		b2TracyCZoneEnd( 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 )+	{+		b2TracyCZoneNC( sleep_islands, "Island Sleep", b2_colorLightSlateGray, true );+		uint64_t sleepTicks = b2GetTicks();++		// Collect split island candidate for the next time step. No need to split if sleeping is disabled.+		B2_ASSERT( world->splitIslandId == B2_NULL_INDEX );+		float splitSleepTimer = 0.0f;+		for ( int i = 0; i < world->workerCount; ++i )+		{+			b2TaskContext* taskContext = world->taskContexts.data + i;+			if ( taskContext->splitIslandId != B2_NULL_INDEX && taskContext->splitSleepTime >= splitSleepTimer )+			{+				B2_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;+			}+		}++		b2BitSet* awakeIslandBitSet = &world->taskContexts.data[0].awakeIslandBitSet;+		for ( int i = 1; i < world->workerCount; ++i )+		{+			b2InPlaceUnion( awakeIslandBitSet, &world->taskContexts.data[i].awakeIslandBitSet );+		}++		// Need to process in reverse because this moves islands to sleeping solver sets.+		b2IslandSim* islands = awakeSet->islandSims.data;+		int count = awakeSet->islandSims.count;+		for ( int islandIndex = count - 1; islandIndex >= 0; islandIndex -= 1 )+		{+			if ( b2GetBit( awakeIslandBitSet, islandIndex ) == true )+			{+				// this island is still awake+				continue;+			}++			b2IslandSim* island = islands + islandIndex;+			int islandId = island->islandId;++			b2TrySleepIsland( world, islandId );+		}++		b2ValidateSolverSets( world );++		world->profile.sleepIslands = b2GetMilliseconds( sleepTicks );+		b2TracyCZoneEnd( sleep_islands );+	}+}
+ upstream/src/solver.h view
@@ -0,0 +1,286 @@+// SPDX-FileCopyrightText: 2023 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 (b2SolverBlock)+// and the atomic counter sit in a wrapping b2SyncBlock 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 "core.h"++#include "box2d/math_functions.h"++#include <stdbool.h>+#include <stdint.h>++typedef struct b2BodySim b2BodySim;+typedef struct b2BodyState b2BodyState;+typedef struct b2ContactSim b2ContactSim;+typedef struct b2JointSim b2JointSim;+typedef struct b2World b2World;++// Solver stages. Prepare joints and prepare contacts are split up+// because there is no need to store joint impulses.+typedef enum b2SolverStageType+{+	b2_stagePrepareJoints,+	b2_stagePrepareContacts,+	b2_stageIntegrateVelocities,+	b2_stageWarmStart,+	b2_stageSolve,+	b2_stageIntegratePositions,+	b2_stageRelax,+	b2_stageRestitution,+	b2_stageStoreImpulses+} b2SolverStageType;++typedef enum b2SolverBlockType+{+	b2_bodyBlock,+	b2_jointBlock,+	b2_contactBlock,+	b2_graphJointBlock,+	b2_graphContactBlock+} b2SolverBlockType;++// Solver block describes a multithreaded unit of work.+typedef struct b2SolverBlock+{+	int startIndex;+	uint16_t count;+	// b2SolverBlockType+	uint8_t blockType;+	uint8_t colorIndex;+} b2SolverBlock;++// 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 b2SyncBlock+{+	b2SolverBlock block;+	b2AtomicInt syncIndex;+	char padding[64 - sizeof( b2SolverBlock ) - sizeof( b2AtomicInt )];+} b2SyncBlock;++// 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 b2SolverStage+{+	b2SyncBlock* blocks;+	b2SolverStageType type;+	int blockCount;+	uint8_t colorIndex;+	char padding1[64];+	b2AtomicInt completionCount;+	char padding2[64];+} b2SolverStage;++// Constraint softness+typedef struct b2Softness+{+	float biasRate;+	float massScale;+	float impulseScale;+} b2Softness;++// 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 color's+// contact sims 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 b2ContactPrepareSpan+{+	int start;+	int count;+	b2ContactSim* contacts;+} b2ContactPrepareSpan;++// Similar for the narrow phase.+typedef struct b2ContactCollideSpan+{+	int start;+	b2ContactSim* contacts;+} b2ContactCollideSpan;++// Similar for joints+typedef struct b2JointPrepareSpan+{+	int start;+	int count;+	b2JointSim* joints;+} b2JointPrepareSpan;++// Context for a time step. Recreated each time step.+typedef struct b2StepContext+{+	// time step+	float dt;++	// inverse time step (0 if dt == 0).+	float inv_dt;++	// sub-step+	float h;+	float inv_h;++	int subStepCount;++	b2Softness contactSoftness;+	b2Softness staticSoftness;++	float restitutionThreshold;+	float maxLinearVelocity;++	struct b2World* world;+	struct b2ConstraintGraph* graph;++	// shortcut to body states from awake set+	b2BodyState* states;++	// shortcut to body sims from awake set+	b2BodySim* 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;+	b2AtomicInt bulletBodyCount;++	b2AtomicInt anyRestitution;++	// Graph color spans for the narrow-phase, includes non-touching contacts as well.+	const b2ContactCollideSpan* collideSpans;++	// 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.+	void* wideContactConstraints;+	b2ContactPrepareSpan* contactPrepareSpans;+	int wideContactCount;++	b2JointPrepareSpan* jointPrepareSpans;+	int jointCount;++	int activeColorCount;+	int workerCount;++	b2SolverStage* 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)+	b2AtomicU32 atomicSyncBits;++	// padding to prevent false sharing+	char padding2[64];++	// Race flag claimed by whichever runner reaches b2SolverTask with workerIndex 0 first.+	// The calling thread of b2World_Step also races for this slot so the orchestrator can+	// always make progress, regardless of how the user's task system schedules tasks (out+	// of order, fewer threads than workers, or synchronously inside enqueueTaskFcn). The+	// loser of the race no-ops as workerIndex 0.+	b2AtomicInt mainClaimed;++	// padding to prevent false sharing+	char padding3[64];++} b2StepContext;++static inline b2Softness b2MakeSoft( float hertz, float zeta, float h )+{+	if ( hertz == 0.0f )+	{+		return (b2Softness){+			.biasRate = 0.0f,+			.massScale = 0.0f,+			.impulseScale = 0.0f,+		};+	}++	float omega = 2.0f * B2_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 (b2Softness){+		.biasRate = omega / a1,+		.massScale = a2 * a3,+		.impulseScale = a3,+	};+}++void b2Solve( b2World* world, b2StepContext* stepContext );
+ upstream/src/solver_set.c view
@@ -0,0 +1,678 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "solver_set.h"++#include "body.h"+#include "constraint_graph.h"+#include "contact.h"+#include "core.h"+#include "island.h"+#include "joint.h"+#include "physics_world.h"++#include <string.h>++void b2DestroySolverSet( b2World* world, int setIndex )+{+	b2SolverSet* set = b2Array_Get( world->solverSets, setIndex );+	b2Array_Destroy( set->bodySims );+	b2Array_Destroy( set->bodyStates );+	b2Array_Destroy( set->contactSims );+	b2Array_Destroy( set->jointSims );+	b2Array_Destroy( set->islandSims );+	b2FreeId( &world->solverSetIdPool, setIndex );+	*set = (b2SolverSet){ 0 };+	set->setIndex = B2_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 b2WakeSolverSet( b2World* world, int setIndex )+{+	B2_ASSERT( setIndex >= b2_firstSleepingSet );+	b2SolverSet* set = b2Array_Get( world->solverSets, setIndex );+	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	b2SolverSet* disabledSet = b2Array_Get( world->solverSets, b2_disabledSet );++	b2Body* bodies = world->bodies.data;++	int bodyCount = set->bodySims.count;+	for ( int i = 0; i < bodyCount; ++i )+	{+		b2BodySim* simSrc = set->bodySims.data + i;++		b2Body* body = bodies + simSrc->bodyId;+		B2_ASSERT( body->setIndex == setIndex );+		body->setIndex = b2_awakeSet;+		body->localIndex = awakeSet->bodySims.count;++		// Reset sleep timer+		body->sleepTime = 0.0f;++		b2BodySim* simDst = b2Array_Emplace( awakeSet->bodySims );+		memcpy( simDst, simSrc, sizeof( b2BodySim ) );++		b2BodyState* state = b2Array_Emplace( awakeSet->bodyStates );+		*state = b2_identityBodyState;+		state->flags = body->flags;++		// Move non-touching contacts from disabled set to awake set.+		int encodedBodySimIndex = body->localIndex;+		int contactKey = body->headContactKey;+		while ( contactKey != B2_NULL_INDEX )+		{+			int edgeIndex = contactKey & 1;+			int contactId = contactKey >> 1;++			b2Contact* contact = b2Array_Get( world->contacts, contactId );++			contactKey = contact->edges[edgeIndex].nextKey;++			if ( contact->setIndex != b2_disabledSet )+			{+				B2_ASSERT( contact->setIndex == b2_awakeSet || contact->setIndex == setIndex );++				// If this contact is awake but not touching I need to fixup the body sim index here.+				if ( contact->setIndex == b2_awakeSet )+				{+					B2_ASSERT( contact->colorIndex == B2_NULL_INDEX );+					b2ContactSim* awakeContactSim = b2Array_Get( awakeSet->contactSims, contact->localIndex );+					if ( edgeIndex == 0 )+					{+						awakeContactSim->encodedBodySimA = encodedBodySimIndex;+					}+					else+					{+						awakeContactSim->encodedBodySimB = encodedBodySimIndex;+					}+				}++				continue;+			}++			int localIndex = contact->localIndex;+			b2ContactSim* contactSim = b2Array_Get( disabledSet->contactSims, localIndex );++			B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) == 0 && contactSim->manifold.pointCount == 0 );++			contact->setIndex = b2_awakeSet;+			contact->localIndex = awakeSet->contactSims.count;+			b2ContactSim* awakeContactSim = b2Array_Emplace( awakeSet->contactSims );+			memcpy( awakeContactSim, contactSim, sizeof( b2ContactSim ) );++			if ( edgeIndex == 0 )+			{+				awakeContactSim->encodedBodySimA = encodedBodySimIndex;+			}+			else+			{+				awakeContactSim->encodedBodySimB = encodedBodySimIndex;+			}++			int movedLocalIndex = b2Array_RemoveSwap( disabledSet->contactSims, localIndex );+			if ( movedLocalIndex != B2_NULL_INDEX )+			{+				// fix moved element+				b2ContactSim* movedContactSim = disabledSet->contactSims.data + localIndex;+				b2Contact* movedContact = b2Array_Get( world->contacts, movedContactSim->contactId );+				B2_ASSERT( movedContact->localIndex == movedLocalIndex );+				movedContact->localIndex = localIndex;+			}+		}+	}++	// transfer touching contacts from sleeping set to contact graph+	{+		int contactCount = set->contactSims.count;+		for ( int i = 0; i < contactCount; ++i )+		{+			b2ContactSim* contactSim = set->contactSims.data + i;+			b2Contact* contact = b2Array_Get( world->contacts, contactSim->contactId );+			B2_ASSERT( contact->flags & b2_contactTouchingFlag );+			B2_ASSERT( contactSim->simFlags & b2_simTouchingFlag );+			B2_ASSERT( contactSim->manifold.pointCount > 0 );+			B2_ASSERT( contact->setIndex == setIndex );+			b2AddContactToGraph( world, contactSim, contact );+			contact->setIndex = b2_awakeSet;+		}+	}++	// transfer joints from sleeping set to awake set+	{+		int jointCount = set->jointSims.count;+		for ( int i = 0; i < jointCount; ++i )+		{+			b2JointSim* jointSim = set->jointSims.data + i;+			b2Joint* joint = b2Array_Get( world->joints, jointSim->jointId );+			B2_ASSERT( joint->setIndex == setIndex );+			b2AddJointToGraph( world, jointSim, joint );+			joint->setIndex = b2_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 )+		{+			b2IslandSim* islandSrc = set->islandSims.data + i;+			b2Island* island = b2Array_Get( world->islands, islandSrc->islandId );+			island->setIndex = b2_awakeSet;+			island->localIndex = awakeSet->islandSims.count;+			b2IslandSim* islandDst = b2Array_Emplace( awakeSet->islandSims );+			memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) );+		}+	}++	// destroy the sleeping set+	b2DestroySolverSet( world, setIndex );+}++// Islands need to have a deterministic order because data is moved to a sleeping set according+// to island order.+void b2TrySleepIsland( b2World* world, int islandId )+{+	b2Island* island = b2Array_Get( world->islands, islandId );+	B2_ASSERT( island->setIndex == b2_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 = b2AllocId( &world->solverSetIdPool );+	if ( sleepSetId == world->solverSets.count )+	{+		b2SolverSet set = { 0 };+		set.setIndex = B2_NULL_INDEX;+		b2Array_Push( world->solverSets, set );+	}++	b2SolverSet* sleepSet = b2Array_Get( world->solverSets, sleepSetId );+	*sleepSet = (b2SolverSet){ 0 };++	// grab awake set after creating the sleep set because the solver set array may have been resized+	b2SolverSet* awakeSet = b2Array_Get( world->solverSets, b2_awakeSet );+	B2_ASSERT( 0 <= island->localIndex && island->localIndex < awakeSet->islandSims.count );++	sleepSet->setIndex = sleepSetId;+	b2Array_CreateN( sleepSet->bodySims, island->bodies.count );+	b2Array_CreateN( sleepSet->contactSims, island->contacts.count );+	b2Array_CreateN( sleepSet->jointSims, island->joints.count );++	// move awake bodies to sleeping set+	// this shuffles around bodies in the awake set+	{+		b2SolverSet* disabledSet = b2Array_Get( world->solverSets, b2_disabledSet );+		for ( int i = 0; i < island->bodies.count; ++i )+		{+			int bodyId = island->bodies.data[i];+			b2Body* body = b2Array_Get( world->bodies, bodyId );+			B2_ASSERT( body->setIndex == b2_awakeSet );+			B2_ASSERT( body->islandId == islandId );+			B2_ASSERT( body->islandIndex == i );++			body->flags &= ~b2_bodyTransientFlags;++			// 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 != B2_NULL_INDEX )+			{+				b2BodyMoveEvent* moveEvent = b2Array_Get( world->bodyMoveEvents, body->bodyMoveIndex );+				B2_ASSERT( moveEvent->bodyId.index1 - 1 == bodyId );+				B2_ASSERT( moveEvent->bodyId.generation == body->generation );+				moveEvent->fellAsleep = true;+				body->bodyMoveIndex = B2_NULL_INDEX;+			}++			int awakeBodyIndex = body->localIndex;+			b2BodySim* awakeSim = b2Array_Get( awakeSet->bodySims, awakeBodyIndex );+			awakeSim->flags &= ~b2_bodyTransientFlags;++			// move body sim to sleep set+			int sleepBodyIndex = sleepSet->bodySims.count;+			b2BodySim* sleepBodySim = b2Array_Emplace( sleepSet->bodySims );+			memcpy( sleepBodySim, awakeSim, sizeof( b2BodySim ) );++			b2Body* movedBody = b2RemoveBodySim( world, awakeSet, awakeBodyIndex );+			if ( movedBody != NULL && movedBody->islandId != islandId )+			{+				// Update the contact sims for the moved body.+				b2RefreshBodyContactIndices( world, movedBody );+			}++			// Destroy state, no need to clone.+			(void)b2Array_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 != B2_NULL_INDEX )+			{+				int contactId = contactKey >> 1;+				int edgeIndex = contactKey & 1;++				b2Contact* contact = b2Array_Get( world->contacts, contactId );++				B2_ASSERT( contact->setIndex == b2_awakeSet );++				contactKey = contact->edges[edgeIndex].nextKey;++				if ( contact->colorIndex != B2_NULL_INDEX )+				{+					// contact is touching and will be moved separately+					B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) != 0 );+					continue;+				}++				int localIndex = contact->localIndex;+				b2ContactSim* contactSim = b2Array_Get( awakeSet->contactSims, localIndex );++				if ( edgeIndex == 0 )+				{+					contactSim->encodedBodySimA = B2_NULL_INDEX;+				}+				else+				{+					contactSim->encodedBodySimB = B2_NULL_INDEX;+				}++				// 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;+				b2Body* otherBody = b2Array_Get( world->bodies, otherBodyId );+				if ( otherBody->setIndex == b2_awakeSet )+				{+					continue;+				}++				B2_ASSERT( contactSim->manifold.pointCount == 0 );+				B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) == 0 );++				// move the non-touching contact to the disabled set+				contact->setIndex = b2_disabledSet;+				contact->localIndex = disabledSet->contactSims.count;+				b2ContactSim* disabledContactSim = b2Array_Emplace( disabledSet->contactSims );+				memcpy( disabledContactSim, contactSim, sizeof( b2ContactSim ) );++				int movedLocalIndex = b2Array_RemoveSwap( awakeSet->contactSims, localIndex );+				if ( movedLocalIndex != B2_NULL_INDEX )+				{+					// fix moved element+					b2ContactSim* movedContactSim = awakeSet->contactSims.data + localIndex;+					b2Contact* movedContact = b2Array_Get( world->contacts, movedContactSim->contactId );+					B2_ASSERT( movedContact->localIndex == movedLocalIndex );+					movedContact->localIndex = localIndex;+				}+			}+		}+	}++	// move touching contacts+	// this shuffles contacts in the awake set+	{+		for ( int i = 0; i < island->contacts.count; ++i )+		{+			b2ContactLink* link = island->contacts.data + i;+			b2Contact* contact = b2Array_Get( world->contacts, link->contactId );+			B2_ASSERT( contact->setIndex == b2_awakeSet );+			B2_ASSERT( contact->islandId == islandId );+			int colorIndex = contact->colorIndex;+			B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );++			b2GraphColor* color = world->constraintGraph.colors + colorIndex;++			// Remove bodies from graph coloring associated with this constraint+			if ( colorIndex != B2_OVERFLOW_INDEX )+			{+				// might clear a bit for a static body, but this has no effect+				b2ClearBit( &color->bodySet, contact->edges[0].bodyId );+				b2ClearBit( &color->bodySet, contact->edges[1].bodyId );+			}++			int localIndex = contact->localIndex;+			b2ContactSim* awakeContactSim = b2Array_Get( color->contactSims, localIndex );++			int sleepContactIndex = sleepSet->contactSims.count;+			b2ContactSim* sleepContactSim = b2Array_Emplace( sleepSet->contactSims );+			memcpy( sleepContactSim, awakeContactSim, sizeof( b2ContactSim ) );++			sleepContactSim->encodedBodySimA = b2SleepBodySimIndex( sleepContactSim->encodedBodySimA );+			sleepContactSim->encodedBodySimB = b2SleepBodySimIndex( sleepContactSim->encodedBodySimB );++			int movedLocalIndex = b2Array_RemoveSwap( color->contactSims, localIndex );+			if ( movedLocalIndex != B2_NULL_INDEX )+			{+				// fix moved element+				b2ContactSim* movedContactSim = color->contactSims.data + localIndex;+				b2Contact* movedContact = b2Array_Get( world->contacts, movedContactSim->contactId );+				B2_ASSERT( movedContact->localIndex == movedLocalIndex );+				movedContact->localIndex = localIndex;+			}++			contact->setIndex = sleepSetId;+			contact->colorIndex = B2_NULL_INDEX;+			contact->localIndex = sleepContactIndex;+		}+	}++	// move joints+	// this shuffles joints in the awake set+	{+		for ( int i = 0; i < island->joints.count; ++i )+		{+			b2JointLink* link = island->joints.data + i;+			b2Joint* joint = b2Array_Get( world->joints, link->jointId );+			B2_ASSERT( joint->setIndex == b2_awakeSet );+			B2_ASSERT( joint->islandId == islandId );+			int colorIndex = joint->colorIndex;+			int localIndex = joint->localIndex;++			B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );++			b2GraphColor* color = world->constraintGraph.colors + colorIndex;++			b2JointSim* awakeJointSim = b2Array_Get( color->jointSims, localIndex );++			if ( colorIndex != B2_OVERFLOW_INDEX )+			{+				// might clear a bit for a static body, but this has no effect+				b2ClearBit( &color->bodySet, joint->edges[0].bodyId );+				b2ClearBit( &color->bodySet, joint->edges[1].bodyId );+			}++			int sleepJointIndex = sleepSet->jointSims.count;+			b2JointSim* sleepJointSim = b2Array_Emplace( sleepSet->jointSims );+			memcpy( sleepJointSim, awakeJointSim, sizeof( b2JointSim ) );++			int movedIndex = b2Array_RemoveSwap( color->jointSims, localIndex );+			if ( movedIndex != B2_NULL_INDEX )+			{+				// fix moved element+				b2JointSim* movedJointSim = color->jointSims.data + localIndex;+				int movedId = movedJointSim->jointId;+				b2Joint* movedJoint = b2Array_Get( world->joints, movedId );+				B2_ASSERT( movedJoint->localIndex == movedIndex );+				movedJoint->localIndex = localIndex;+			}++			joint->setIndex = sleepSetId;+			joint->colorIndex = B2_NULL_INDEX;+			joint->localIndex = sleepJointIndex;+		}+	}++	// move island struct+	{+		B2_ASSERT( island->setIndex == b2_awakeSet );++		int islandIndex = island->localIndex;+		b2IslandSim* sleepIsland = b2Array_Emplace( sleepSet->islandSims );+		sleepIsland->islandId = islandId;++		int movedIslandIndex = b2Array_RemoveSwap( awakeSet->islandSims, islandIndex );+		if ( movedIslandIndex != B2_NULL_INDEX )+		{+			// fix index on moved element+			b2IslandSim* movedIslandSim = awakeSet->islandSims.data + islandIndex;+			int movedIslandId = movedIslandSim->islandId;+			b2Island* movedIsland = b2Array_Get( world->islands, movedIslandId );+			B2_ASSERT( movedIsland->localIndex == movedIslandIndex );+			movedIsland->localIndex = islandIndex;+		}++		island->setIndex = sleepSetId;+		island->localIndex = 0;+	}++	if ( world->splitIslandId == islandId )+	{+		world->splitIslandId = B2_NULL_INDEX;+	}++	b2ValidateSolverSets( 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 waked.+void b2MergeSolverSets( b2World* world, int setId1, int setId2 )+{+	B2_ASSERT( setId1 >= b2_firstSleepingSet );+	B2_ASSERT( setId2 >= b2_firstSleepingSet );+	b2SolverSet* set1 = b2Array_Get( world->solverSets, setId1 );+	b2SolverSet* set2 = b2Array_Get( world->solverSets, setId2 );++	// Move the fewest number of bodies+	if ( set1->bodySims.count < set2->bodySims.count )+	{+		b2SolverSet* tempSet = set1;+		set1 = set2;+		set2 = tempSet;++		int tempId = setId1;+		setId1 = setId2;+		setId2 = tempId;+	}++	// transfer bodies+	{+		b2Body* bodies = world->bodies.data;+		int bodyCount = set2->bodySims.count;+		for ( int i = 0; i < bodyCount; ++i )+		{+			b2BodySim* simSrc = set2->bodySims.data + i;++			b2Body* body = bodies + simSrc->bodyId;+			B2_ASSERT( body->setIndex == setId2 );+			body->setIndex = setId1;+			body->localIndex = set1->bodySims.count;++			b2BodySim* simDst = b2Array_Emplace( set1->bodySims );+			memcpy( simDst, simSrc, sizeof( b2BodySim ) );+		}+	}++	// transfer contacts+	{+		int contactCount = set2->contactSims.count;+		for ( int i = 0; i < contactCount; ++i )+		{+			b2ContactSim* contactSrc = set2->contactSims.data + i;++			b2Contact* contact = b2Array_Get( world->contacts, contactSrc->contactId );+			B2_ASSERT( contact->setIndex == setId2 );+			contact->setIndex = setId1;+			contact->localIndex = set1->contactSims.count;++			b2ContactSim* contactDst = b2Array_Emplace( set1->contactSims );+			memcpy( contactDst, contactSrc, sizeof( b2ContactSim ) );+		}+	}++	// transfer joints+	{+		int jointCount = set2->jointSims.count;+		for ( int i = 0; i < jointCount; ++i )+		{+			b2JointSim* jointSrc = set2->jointSims.data + i;++			b2Joint* joint = b2Array_Get( world->joints, jointSrc->jointId );+			B2_ASSERT( joint->setIndex == setId2 );+			joint->setIndex = setId1;+			joint->localIndex = set1->jointSims.count;++			b2JointSim* jointDst = b2Array_Emplace( set1->jointSims );+			memcpy( jointDst, jointSrc, sizeof( b2JointSim ) );+		}+	}++	// transfer islands+	{+		int islandCount = set2->islandSims.count;+		for ( int i = 0; i < islandCount; ++i )+		{+			b2IslandSim* islandSrc = set2->islandSims.data + i;+			int islandId = islandSrc->islandId;++			b2Island* island = b2Array_Get( world->islands, islandId );+			island->setIndex = setId1;+			island->localIndex = set1->islandSims.count;++			b2IslandSim* islandDst = b2Array_Emplace( set1->islandSims );+			memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) );+		}+	}++	// destroy the merged set+	b2DestroySolverSet( world, setId2 );++	b2ValidateSolverSets( world );+}++// Remove a body from a solver set and return the body swapped into the slot.+b2Body* b2RemoveBodySim( b2World* world, b2SolverSet* set, int localIndex )+{+	b2Array( b2BodySim )* bodySims = &set->bodySims;+	B2_ASSERT( 0 <= localIndex && localIndex < bodySims->count );+	int lastIndex = bodySims->count - 1;+	bodySims->count -= 1;++	if ( localIndex == lastIndex )+	{+		return NULL;+	}++	bodySims->data[localIndex] = bodySims->data[lastIndex];+	b2Body* movedBody = b2Array_Get( world->bodies, bodySims->data[localIndex].bodyId );+	B2_ASSERT( movedBody->localIndex == lastIndex );+	movedBody->localIndex = localIndex;++	return movedBody;+}++void b2TransferBody( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Body* body )+{+	B2_ASSERT( body->headContactKey == B2_NULL_INDEX );++	if ( targetSet == sourceSet )+	{+		return;+	}++	// The use cases for transfer body (set body type, disable, etc) destroy all the contacts+	// first. Double check here because this function wouldn't work correctly otherwise.+	B2_ASSERT( body->headContactKey == B2_NULL_INDEX );++	int sourceIndex = body->localIndex;+	b2BodySim* sourceSim = b2Array_Get( sourceSet->bodySims, sourceIndex );++	int targetIndex = targetSet->bodySims.count;+	b2BodySim* targetSim = b2Array_Emplace( targetSet->bodySims );+	memcpy( targetSim, sourceSim, sizeof( b2BodySim ) );++	// Clear transient body flags+	body->flags &= ~b2_bodyTransientFlags;+	targetSim->flags &= ~( b2_isFast | b2_bodyTransientFlags );++	// Remove body sim from solver set that owns it+	b2Body* movedBody = b2RemoveBodySim( world, sourceSet, sourceIndex );+	if ( movedBody != NULL )+	{+		// Fixup the moved body's contact sims.+		b2RefreshBodyContactIndices( world, movedBody );+	}++	if ( sourceSet->setIndex == b2_awakeSet )+	{+		(void)b2Array_RemoveSwap( sourceSet->bodyStates, sourceIndex );+	}+	else if ( targetSet->setIndex == b2_awakeSet )+	{+		b2BodyState* state = b2Array_Emplace( targetSet->bodyStates );+		*state = b2_identityBodyState;+		state->flags = body->flags;+	}++	body->setIndex = targetSet->setIndex;+	body->localIndex = targetIndex;+}++void b2TransferJoint( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Joint* joint )+{+	if ( targetSet == sourceSet )+	{+		return;+	}++	int localIndex = joint->localIndex;+	int colorIndex = joint->colorIndex;++	// Retrieve source.+	b2JointSim* sourceSim;+	if ( sourceSet->setIndex == b2_awakeSet )+	{+		B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );+		b2GraphColor* color = world->constraintGraph.colors + colorIndex;++		sourceSim = b2Array_Get( color->jointSims, localIndex );+	}+	else+	{+		B2_ASSERT( colorIndex == B2_NULL_INDEX );+		sourceSim = b2Array_Get( sourceSet->jointSims, localIndex );+	}++	// Create target and copy. Fix joint.+	if ( targetSet->setIndex == b2_awakeSet )+	{+		b2AddJointToGraph( world, sourceSim, joint );+		joint->setIndex = b2_awakeSet;+	}+	else+	{+		joint->setIndex = targetSet->setIndex;+		joint->localIndex = targetSet->jointSims.count;+		joint->colorIndex = B2_NULL_INDEX;++		b2JointSim* targetSim = b2Array_Emplace( targetSet->jointSims );+		memcpy( targetSim, sourceSim, sizeof( b2JointSim ) );+	}++	// Destroy source.+	if ( sourceSet->setIndex == b2_awakeSet )+	{+		b2RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, colorIndex, localIndex );+	}+	else+	{+		int movedIndex = b2Array_RemoveSwap( sourceSet->jointSims, localIndex );+		if ( movedIndex != B2_NULL_INDEX )+		{+			// fix swapped element+			b2JointSim* movedJointSim = sourceSet->jointSims.data + localIndex;+			int movedId = movedJointSim->jointId;+			b2Joint* movedJoint = b2Array_Get( world->joints, movedId );+			movedJoint->localIndex = localIndex;+		}+	}+}
+ upstream/src/solver_set.h view
@@ -0,0 +1,80 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "contact.h"+#include "container.h"+#include "island.h"+#include "joint.h"++typedef struct b2Body b2Body;+typedef struct b2World b2World;++// The solver set type by index+enum b2SolverSetType+{+	// Static set for static bodies and joints between static bodies+	b2_staticSet = 0,++	// Disabled set for disabled bodies and their joints+	b2_disabledSet = 1,++	// Awake set for awake bodies and awake non-touching contacts. Awake touching contacts+	// and awake joints live in the constraint graph+	b2_awakeSet = 2,++	// The index of the first sleeping set. Each island that goes to sleep is put into+	// a sleeping set. This holds all bodies, contacts, and joints from the sleeping island.+	// A separate set for each sleeping island makes it very efficient to wake a single island.+	b2_firstSleepingSet = 3,+};++// 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 b2SolverSet+{+	// Body array. Empty for unused set.+	b2Array( b2BodySim ) bodySims;++	// Body state only exists for active set+	b2Array( b2BodyState ) bodyStates;++	// This holds sleeping/disabled joints. Empty for static/active set.+	b2Array( b2JointSim ) jointSims;++	// This holds all contacts for sleeping sets.+	// This holds non-touching contacts for the awake set.+	b2Array( b2ContactSim ) contactSims;++	// 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.+	b2Array( b2IslandSim ) islandSims;++	// Aligns with b2World::solverSetIdPool. Used to create a stable id for body/contact/joint/islands.+	int setIndex;+} b2SolverSet;++void b2DestroySolverSet( b2World* world, int setIndex );++void b2WakeSolverSet( b2World* world, int setIndex );+void b2TrySleepIsland( b2World* world, int islandId );++// Merge set 2 into set 1 then destroy set 2.+// Warning: any pointers into these sets will be orphaned.+void b2MergeSolverSets( b2World* world, int setIndex1, int setIndex2 );++b2Body* b2RemoveBodySim( b2World* world, b2SolverSet* set, int localIndex );++void b2TransferBody( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Body* body );+void b2TransferJoint( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Joint* joint );++b2DeclareArray( b2SolverSet );
+ upstream/src/table.c view
@@ -0,0 +1,238 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "table.h"++#include "bitset.h"+#include "core.h"+#include "platform.h"++#include <stdbool.h>+#include <string.h>++#if B2_SNOOP_TABLE_COUNTERS+b2AtomicInt b2_findCount;+b2AtomicInt b2_probeCount;+#endif++b2HashSet b2CreateSet( int capacity )+{+	b2HashSet set = { 0 };++	// Capacity must be a power of 2+	if ( capacity > 16 )+	{+		set.capacity = b2RoundUpPowerOf2( capacity );+	}+	else+	{+		set.capacity = 16;+	}++	set.count = 0;+	set.items = b2Alloc( set.capacity * sizeof( b2SetItem ) );+	memset( set.items, 0, set.capacity * sizeof( b2SetItem ) );++	return set;+}++void b2DestroySet( b2HashSet* set )+{+	b2Free( set->items, set->capacity * sizeof( b2SetItem ) );+	set->items = NULL;+	set->count = 0;+	set->capacity = 0;+}++void b2ClearSet( b2HashSet* set )+{+	set->count = 0;+	memset( set->items, 0, set->capacity * sizeof( b2SetItem ) );+}++static int b2FindSlot( const b2HashSet* set, uint64_t key, uint64_t hash )+{+#if B2_SNOOP_TABLE_COUNTERS+	b2AtomicFetchAddInt( &b2_findCount, 1 );+#endif++	uint32_t capacity = set->capacity;+	uint32_t index = (uint32_t)hash & ( capacity - 1 );+	const b2SetItem* items = set->items;+	while ( items[index].key != 0 && items[index].key != key )+	{+#if B2_SNOOP_TABLE_COUNTERS+		b2AtomicFetchAddInt( &b2_probeCount, 1 );+#endif+		index = ( index + 1 ) & ( capacity - 1 );+	}++	return index;+}++static void b2AddKeyHaveCapacity( b2HashSet* set, uint64_t key, uint64_t hash )+{+	int index = b2FindSlot( set, key, hash );+	b2SetItem* items = set->items;++	B2_ASSERT( items[index].key == 0 );+	items[index].key = key;+	set->count += 1;+}++static void b2GrowTable( b2HashSet* set )+{+	uint32_t oldCount = set->count;+	B2_UNUSED( oldCount );++	uint32_t oldCapacity = set->capacity;+	b2SetItem* oldItems = set->items;++	set->count = 0;+	// Capacity must be a power of 2+	set->capacity = 2 * oldCapacity;+	set->items = b2Alloc( set->capacity * sizeof( b2SetItem ) );+	memset( set->items, 0, set->capacity * sizeof( b2SetItem ) );++	// Transfer items into new array+	for ( uint32_t i = 0; i < oldCapacity; ++i )+	{+		b2SetItem* item = oldItems + i;+		if ( item->key == 0 )+		{+			// this item was empty+			continue;+		}++		uint64_t hash = b2KeyHash( item->key );+		b2AddKeyHaveCapacity( set, item->key, hash );+	}++	B2_ASSERT( set->count == oldCount );++	b2Free( oldItems, oldCapacity * sizeof( b2SetItem ) );+}++bool b2ContainsKey( const b2HashSet* set, uint64_t key )+{+	// key of zero is a sentinel+	B2_VALIDATE( key != 0 );+	uint64_t hash = b2KeyHash( key );+	int index = b2FindSlot( set, key, hash );+	return set->items[index].key == key;+}++bool b2ContainsHashedKey( const b2HashSet* set, uint64_t key, uint64_t hash )+{+	// key of zero is a sentinel+	B2_VALIDATE( key != 0 );+	int index = b2FindSlot( set, key, hash );+	return set->items[index].key == key;+}++int b2GetHashSetBytes( b2HashSet* set )+{+	return set->capacity * (int)sizeof( b2SetItem );+}++bool b2AddKey( b2HashSet* set, uint64_t key )+{+	// key of zero is a sentinel+	B2_ASSERT( key != 0 );++	uint64_t hash = b2KeyHash( key );+	B2_ASSERT( hash != 0 );++	int index = b2FindSlot( set, key, hash );+	if ( set->items[index].key != 0 )+	{+		// Already in set+		B2_ASSERT( set->items[index].key == key );+		return true;+	}++	if ( 2 * set->count >= set->capacity )+	{+		b2GrowTable( set );+	}++	b2AddKeyHaveCapacity( set, key, hash );+	return false;+}++// See https://en.wikipedia.org/wiki/Open_addressing+bool b2RemoveKey( b2HashSet* set, uint64_t key )+{+	uint64_t hash = b2KeyHash( key );+	int i = b2FindSlot( set, key, hash );+	b2SetItem* items = set->items;+	if ( items[i].key == 0 )+	{+		// Not in set+		return false;+	}++	// Mark item i as unoccupied+	items[i].key = 0;++	B2_ASSERT( set->count > 0 );+	set->count -= 1;++	// Attempt to fill item i+	int j = i;+	uint32_t capacity = set->capacity;+	for ( ;; )+	{+		j = ( j + 1 ) & ( capacity - 1 );+		if ( items[j].key == 0 )+		{+			break;+		}++		// k is the first item for the hash of j+		uint64_t hash_j = b2KeyHash( items[j].key );+		int k = hash_j & ( 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;++		i = j;+	}++	return true;+}++// This function is here because ctz.h is included by+// this file but not in bitset.c+int b2CountSetBits( b2BitSet* bitSet )+{+	int popCount = 0;+	uint32_t blockCount = bitSet->blockCount;+	for ( uint32_t i = 0; i < blockCount; ++i )+	{+		popCount += b2PopCount64( bitSet->bits[i] );+	}++	return popCount;+}
+ upstream/src/table.h view
@@ -0,0 +1,80 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#pragma once++#include "box2d/base.h"++#include <stdbool.h>+#include <stdint.h>++#define B2_SHAPE_PAIR_KEY( K1, K2 ) K1 < K2 ? (uint64_t)K1 << 32 | (uint64_t)K2 : (uint64_t)K2 << 32 | (uint64_t)K1++// 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/++// I compared with CC on https://jacksonallan.github.io/c_cpp_hash_tables_benchmark/ and got slightly better performance+// in the washer benchmark.+// I compared with verstable across 8 benchmarks and the performance was similar.++B2_FORCE_INLINE uint64_t b2KeyHash( uint64_t key )+{+	// Murmur hash+	uint64_t h = key;+	h ^= h >> 33;+	h *= 0xff51afd7ed558ccduLL;+	h ^= h >> 33;+	h *= 0xc4ceb9fe1a85ec53uLL;+	h ^= h >> 33;+	return h;+}++typedef struct b2SetItem+{+	uint64_t key;++	// storing lower 32 bits of hash+	// this is wasteful because I just need to know if the item is occupied+	// I could require the key to be non-zero and use 0 to indicate an empty slot+	// Update: looks like I store this to make growing the table faster, however this is wasteful once+	// the table has hit the high water mark+	// uint32_t hash;+} b2SetItem;++typedef struct b2HashSet+{+	b2SetItem* items;+	uint32_t capacity;+	uint32_t count;+} b2HashSet;++b2HashSet b2CreateSet( int capacity );+void b2DestroySet( b2HashSet* set );++void b2ClearSet( b2HashSet* set );++// Returns true if key was already in set+bool b2AddKey( b2HashSet* set, uint64_t key );++// Returns true if the key was found+bool b2RemoveKey( b2HashSet* set, uint64_t key );++bool b2ContainsKey( const b2HashSet* set, uint64_t key );+bool b2ContainsHashedKey( const b2HashSet* set, uint64_t key, uint64_t hash );++int b2GetHashSetBytes( b2HashSet* set );++static inline int b2GetSetCount( b2HashSet* set )+{+	return set->count;+}++static inline int b2GetSetCapacity( b2HashSet* set )+{+	return set->capacity;+}
+ upstream/src/timer.c view
@@ -0,0 +1,683 @@+// SPDX-FileCopyrightText: 2023 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 "box2d/base.h"++#include <stddef.h>+#include <stdio.h>++#if defined( _WIN32 )++#ifndef WIN32_LEAN_AND_MEAN+#define WIN32_LEAN_AND_MEAN 1+#endif++#ifndef NOGDI+#define NOGDI+#endif++#ifndef NOUSER+#define NOUSER+#endif++#ifndef NOMINMAX+#define NOMINMAX+#endif++// On Windows this is capitalized, but not in Linux mingw.+#include <windows.h>+#include <limits.h>++static double s_invFrequency = 0.0;++uint64_t b2GetTicks( void )+{+	LARGE_INTEGER counter;+	QueryPerformanceCounter( &counter );+	return (uint64_t)counter.QuadPart;+}++float b2GetMilliseconds( 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 = b2GetTicks();+	return (float)( s_invFrequency * ( ticksNow - ticks ) );+}++float b2GetMillisecondsAndReset( 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 = b2GetTicks();+	float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) );+	*ticks = ticksNow;+	return ms;+}++void b2Yield( void )+{+	SwitchToThread();+}++typedef struct b2Mutex+{+	CRITICAL_SECTION cs;+} b2Mutex;++b2Mutex* b2CreateMutex( void )+{+	b2Mutex* m = b2Alloc( sizeof( b2Mutex ) );+	InitializeCriticalSection( &m->cs );+	return m;+}++void b2DestroyMutex( b2Mutex* m )+{+	DeleteCriticalSection( &m->cs );+	*m = (b2Mutex){ 0 };+	b2Free( m, sizeof( b2Mutex ) );+}++void b2LockMutex( b2Mutex* m )+{+	EnterCriticalSection( &m->cs );+}++void b2UnlockMutex( b2Mutex* m )+{+	LeaveCriticalSection( &m->cs );+}++typedef struct b2Semaphore+{+	HANDLE semaphore;+} b2Semaphore;++b2Semaphore* b2CreateSemaphore( int initCount )+{+	b2Semaphore* s = b2Alloc( sizeof( b2Semaphore ) );+	s->semaphore = CreateSemaphoreExW( NULL, initCount, INT_MAX, NULL, 0, SEMAPHORE_ALL_ACCESS );+	return s;+}++void b2DestroySemaphore( b2Semaphore* s )+{+	CloseHandle( s->semaphore );+	*s = (b2Semaphore){ 0 };+	b2Free( s, sizeof( b2Semaphore ) );+}++void b2WaitSemaphore( b2Semaphore* s )+{+	WaitForSingleObjectEx( s->semaphore, INFINITE, FALSE );+}++void b2SignalSemaphore( b2Semaphore* s )+{+	ReleaseSemaphore( s->semaphore, 1, NULL );+}++typedef struct b2Thread+{+	HANDLE thread;+	b2ThreadFunction* function;+	void* context;+	char name[32];+} b2Thread;++typedef HRESULT( WINAPI* b2SetThreadDescriptionFn )( 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 b2SetCurrentThreadName( const char* name )+{+	if ( name == NULL || name[0] == 0 )+	{+		return;+	}++	static b2SetThreadDescriptionFn pfn = NULL;+	static int resolved = 0;++	if ( resolved == 0 )+	{+		HMODULE kernel = GetModuleHandleW( L"kernel32.dll" );+		if ( kernel != NULL )+		{+			// MSVC /Wall warns C4191 and GCC/Clang -Wcast-function-type+			// warns on every FARPROC function-pointer cast. This is the+			// intended use of GetProcAddress, so suppress locally.+#if defined( __clang__ )+#pragma clang diagnostic push+#pragma clang diagnostic ignored "-Wcast-function-type"+#elif defined( _MSC_VER )+#pragma warning( push )+#pragma warning( disable : 4191 )+#elif defined( __GNUC__ )+#pragma GCC diagnostic push+#pragma GCC diagnostic ignored "-Wcast-function-type"+#endif+			pfn = (b2SetThreadDescriptionFn)GetProcAddress( kernel, "SetThreadDescription" );+#if defined( __clang__ )+#pragma clang diagnostic pop+#elif defined( _MSC_VER )+#pragma warning( pop )+#elif defined( __GNUC__ )+#pragma GCC diagnostic pop+#endif+		}+		resolved = 1;+	}++	if ( pfn == NULL )+	{+		return;+	}++	wchar_t wide[32];+	int n = MultiByteToWideChar( CP_UTF8, 0, name, -1, wide, (int)( sizeof( wide ) / sizeof( wide[0] ) ) );+	if ( n > 0 )+	{+		pfn( GetCurrentThread(), wide );+	}+}++static DWORD WINAPI b2ThreadStart( LPVOID param )+{+	b2Thread* t = (b2Thread*)param;+	b2SetCurrentThreadName( t->name );+	b2TracyCSetThreadName( t->name );+	t->function( t->context );+	return 0;+}++b2Thread* b2CreateThread( b2ThreadFunction* function, void* context, const char* name )+{+	b2Thread* t = b2Alloc( sizeof( b2Thread ) );+	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, b2ThreadStart, t, 0, NULL );+	return t;+}++void b2JoinThread( b2Thread* t )+{+	WaitForSingleObject( t->thread, INFINITE );+	CloseHandle( t->thread );+	*t = (b2Thread){ 0 };+	b2Free( t, sizeof( b2Thread ) );+}++#elif defined( __linux__ ) || defined( __EMSCRIPTEN__ )++#include <sched.h>+#include <time.h>++uint64_t b2GetTicks( void )+{+	struct timespec ts;+	clock_gettime( CLOCK_MONOTONIC, &ts );+	return ts.tv_sec * 1000000000LL + ts.tv_nsec;+}++float b2GetMilliseconds( uint64_t ticks )+{+	uint64_t ticksNow = b2GetTicks();+	return (float)( ( ticksNow - ticks ) / 1000000.0 );+}++float b2GetMillisecondsAndReset( uint64_t* ticks )+{+	uint64_t ticksNow = b2GetTicks();+	float ms = (float)( ( ticksNow - *ticks ) / 1000000.0 );+	*ticks = ticksNow;+	return ms;+}++void b2Yield( void )+{+	sched_yield();+}++#include <pthread.h>+typedef struct b2Mutex+{+	pthread_mutex_t mtx;+} b2Mutex;++b2Mutex* b2CreateMutex( void )+{+	b2Mutex* m = b2Alloc( sizeof( b2Mutex ) );+	pthread_mutex_init( &m->mtx, NULL );+	return m;+}++void b2DestroyMutex( b2Mutex* m )+{+	pthread_mutex_destroy( &m->mtx );+	*m = (b2Mutex){ 0 };+	b2Free( m, sizeof( b2Mutex ) );+}++void b2LockMutex( b2Mutex* m )+{+	pthread_mutex_lock( &m->mtx );+}++void b2UnlockMutex( b2Mutex* m )+{+	pthread_mutex_unlock( &m->mtx );+}++#include <semaphore.h>++typedef struct b2Semaphore+{+	sem_t semaphore;+} b2Semaphore;++b2Semaphore* b2CreateSemaphore( int initCount )+{+	b2Semaphore* s = b2Alloc( sizeof( b2Semaphore ) );+	sem_init( &s->semaphore, 0, (unsigned int)initCount );+	return s;+}++void b2DestroySemaphore( b2Semaphore* s )+{+	sem_destroy( &s->semaphore );+	*s = (b2Semaphore){ 0 };+	b2Free( s, sizeof( b2Semaphore ) );+}++void b2WaitSemaphore( b2Semaphore* s )+{+	sem_wait( &s->semaphore );+}++void b2SignalSemaphore( b2Semaphore* s )+{+	sem_post( &s->semaphore );+}++typedef struct b2Thread+{+	pthread_t thread;+	b2ThreadFunction* function;+	void* context;+	char name[32];+} b2Thread;++static void b2SetCurrentThreadName( const char* name )+{+	if ( name == NULL || name[0] == 0 )+	{+		return;+	}++#if defined( __linux__ )+	// Linux caps thread names at 15 chars + null terminator.+	char truncated[16];+	snprintf( truncated, sizeof( truncated ), "%.15s", name );+	pthread_setname_np( pthread_self(), truncated );+#else+	(void)name;+#endif+}++static void* b2ThreadStart( void* param )+{+	b2Thread* t = (b2Thread*)param;+	b2SetCurrentThreadName( t->name );+	b2TracyCSetThreadName( t->name );+	t->function( t->context );+	return NULL;+}++b2Thread* b2CreateThread( b2ThreadFunction* function, void* context, const char* name )+{+	b2Thread* t = b2Alloc( sizeof( b2Thread ) );+	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, b2ThreadStart, t );+	return t;+}++void b2JoinThread( b2Thread* t )+{+	pthread_join( t->thread, NULL );+	*t = (b2Thread){ 0 };+	b2Free( t, sizeof( b2Thread ) );+}++#elif defined( __APPLE__ )++#include <mach/mach_time.h>+#include <sched.h>+#include <sys/time.h>++static double s_invFrequency = 0.0;++uint64_t b2GetTicks( void )+{+	return mach_absolute_time();+}++float b2GetMilliseconds( 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 = b2GetTicks();+	return (float)( s_invFrequency * ( ticksNow - ticks ) );+}++float b2GetMillisecondsAndReset( 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 = b2GetTicks();+	float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) );+	*ticks = ticksNow;+	return ms;+}++void b2Yield( void )+{+	sched_yield();+}++#include <pthread.h>+typedef struct b2Mutex+{+	pthread_mutex_t mtx;+} b2Mutex;++b2Mutex* b2CreateMutex( void )+{+	b2Mutex* m = b2Alloc( sizeof( b2Mutex ) );+	pthread_mutex_init( &m->mtx, NULL );+	return m;+}++void b2DestroyMutex( b2Mutex* m )+{+	pthread_mutex_destroy( &m->mtx );+	*m = (b2Mutex){ 0 };+	b2Free( m, sizeof( b2Mutex ) );+}++void b2LockMutex( b2Mutex* m )+{+	pthread_mutex_lock( &m->mtx );+}++void b2UnlockMutex( b2Mutex* m )+{+	pthread_mutex_unlock( &m->mtx );+}++#include <dispatch/dispatch.h>++typedef struct b2Semaphore+{+	dispatch_semaphore_t semaphore;+	int initialCount;+} b2Semaphore;++b2Semaphore* b2CreateSemaphore( int initCount )+{+	b2Semaphore* s = b2Alloc( sizeof( b2Semaphore ) );+	s->semaphore = dispatch_semaphore_create( (long)initCount );+	s->initialCount = initCount;+	return s;+}++void b2DestroySemaphore( b2Semaphore* 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 = (b2Semaphore){ 0 };+	b2Free( s, sizeof( b2Semaphore ) );+}++void b2WaitSemaphore( b2Semaphore* s )+{+	dispatch_semaphore_wait( s->semaphore, DISPATCH_TIME_FOREVER );+}++void b2SignalSemaphore( b2Semaphore* s )+{+	dispatch_semaphore_signal( s->semaphore );+}++typedef struct b2Thread+{+	pthread_t thread;+	b2ThreadFunction* function;+	void* context;+	char name[32];+} b2Thread;++// macOS pthread_setname_np takes only the name — it always names the calling thread.+static void b2SetCurrentThreadName( const char* name )+{+	if ( name == NULL || name[0] == 0 )+	{+		return;+	}+	pthread_setname_np( name );+}++static void* b2ThreadStart( void* param )+{+	b2Thread* t = (b2Thread*)param;+	b2SetCurrentThreadName( t->name );+	b2TracyCSetThreadName( t->name );+	t->function( t->context );+	return NULL;+}++b2Thread* b2CreateThread( b2ThreadFunction* function, void* context, const char* name )+{+	b2Thread* t = b2Alloc( sizeof( b2Thread ) );+	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, b2ThreadStart, t );+	return t;+}++void b2JoinThread( b2Thread* t )+{+	pthread_join( t->thread, NULL );+	*t = (b2Thread){ 0 };+	b2Free( t, sizeof( b2Thread ) );+}++#else++// Fallbacks for unknown platforms++uint64_t b2GetTicks( void )+{+	return 0;+}++float b2GetMilliseconds( uint64_t ticks )+{+	( (void)( ticks ) );+	return 0.0f;+}++float b2GetMillisecondsAndReset( uint64_t* ticks )+{+	( (void)( ticks ) );+	return 0.0f;+}++void b2Yield( void )+{+}++typedef struct b2Mutex+{+	int dummy;+} b2Mutex;++b2Mutex* b2CreateMutex( void )+{+	b2Mutex* m = b2Alloc( sizeof( b2Mutex ) );+	m->dummy = 42;+	return m;+}++void b2DestroyMutex( b2Mutex* m )+{+	*m = (b2Mutex){ 0 };+	b2Free( m, sizeof( b2Mutex ) );+}++void b2LockMutex( b2Mutex* m )+{+	(void)m;+}++void b2UnlockMutex( b2Mutex* m )+{+	(void)m;+}++typedef struct b2Semaphore+{+	int dummy;+} b2Semaphore;++b2Semaphore* b2CreateSemaphore( int initCount )+{+	b2Semaphore* s = b2Alloc( sizeof( b2Semaphore ) );+	(void)initCount;+	s->dummy = 42;+	return s;+}++void b2DestroySemaphore( b2Semaphore* s )+{+	*s = (b2Semaphore){ 0 };+	b2Free( s, sizeof( b2Semaphore ) );+}++void b2WaitSemaphore( b2Semaphore* s )+{+	(void)s;+}++void b2SignalSemaphore( b2Semaphore* s )+{+	(void)s;+}++typedef struct b2Thread+{+	int dummy;+} b2Thread;++b2Thread* b2CreateThread( b2ThreadFunction* function, void* context, const char* name )+{+	(void)name;+	function( context );+	b2Thread* t = b2Alloc( sizeof( b2Thread ) );+	t->dummy = 42;+	return t;+}++void b2JoinThread( b2Thread* t )+{+	*t = (b2Thread){ 0 };+	b2Free( t, sizeof( b2Thread ) );+}++#endif++// djb2 hash+// https://en.wikipedia.org/wiki/List_of_hash_functions+uint32_t b2Hash( uint32_t hash, const uint8_t* data, int count )+{+	uint32_t result = hash;+	for ( int i = 0; i < count; i++ )+	{+		result = ( result << 5 ) + result + data[i];+	}++	return result;+}
+ upstream/src/types.c view
@@ -0,0 +1,175 @@+// SPDX-FileCopyrightText: 2023 Erin Catto+// SPDX-License-Identifier: MIT++#include "core.h"++#include "box2d/math_functions.h"+#include "box2d/types.h"++#include <float.h>++b2WorldDef b2DefaultWorldDef( void )+{+	float lengthUnits = b2GetLengthUnitsPerMeter();+	b2WorldDef def = { 0 };+	def.gravity.x = 0.0f;+	def.gravity.y = -10.0f;+	def.hitEventThreshold = 1.0f * lengthUnits;+	def.restitutionThreshold = 1.0f * lengthUnits;+	def.restitutionIterations = 2;+	def.enableRestitutionPropagation = false;+	def.contactSpeed = 3.0f * lengthUnits;+	def.contactHertz = 30.0;+	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 = B2_SECRET_COOKIE;+	return def;+}++b2BodyDef b2DefaultBodyDef( void )+{+	b2BodyDef def = { 0 };+	def.type = b2_staticBody;+	def.rotation = b2Rot_identity;+	def.safetyFactor = 0.5f;+	def.sleepThreshold = 0.05f * b2GetLengthUnitsPerMeter();+	def.gravityScale = 1.0f;+	def.enableSleep = true;+	def.enableContactRecycling = true;+	def.isAwake = true;+	def.isEnabled = true;+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2Filter b2DefaultFilter( void )+{+	b2Filter filter = { B2_DEFAULT_CATEGORY_BITS, B2_DEFAULT_MASK_BITS, 0 };+	return filter;+}++b2QueryFilter b2DefaultQueryFilter( void )+{+	b2QueryFilter filter = { B2_DEFAULT_CATEGORY_BITS, B2_DEFAULT_MASK_BITS };+	return filter;+}++b2ShapeDef b2DefaultShapeDef( void )+{+	b2ShapeDef def = { 0 };+	def.material.friction = 0.6f;+	def.density = 1.0f;+	def.filter = b2DefaultFilter();+	def.updateBodyMass = true;+	def.invokeContactCreation = true;+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++b2SurfaceMaterial b2DefaultSurfaceMaterial( void )+{+	b2SurfaceMaterial material = {+		.friction = 0.6f,+	};++	return material;+}++b2ChainDef b2DefaultChainDef( void )+{+	static b2SurfaceMaterial defaultMaterial = {+		.friction = 0.6f,+	};++	b2ChainDef def = { 0 };+	def.materials = &defaultMaterial;+	def.materialCount = 1;+	// using infinity as a sentinel+	def.ghost1.x = INFINITY;+	def.ghost1.y = INFINITY;+	def.ghost2.x = INFINITY;+	def.ghost2.y = INFINITY;+	def.filter = b2DefaultFilter();+	def.internalValue = B2_SECRET_COOKIE;+	return def;+}++static void b2EmptyDrawPolygon( b2WorldTransform transform, const b2Vec2* vertices, int vertexCount, b2HexColor color, void* context )+{+	B2_UNUSED( transform, vertices, vertexCount, color, context );+}++static void b2EmptyDrawSolidPolygon( b2WorldTransform transform, const b2Vec2* vertices, int vertexCount, float radius,+									 b2HexColor color, void* context )+{+	B2_UNUSED( transform, vertices, vertexCount, radius, color, context );+}++static void b2EmptyDrawCircle( b2Pos center, float radius, b2HexColor color, void* context )+{+	B2_UNUSED( center, radius, color, context );+}++static void b2EmptyDrawSolidCircle( b2WorldTransform transform, b2Vec2 center, float radius, b2HexColor color, void* context )+{+	B2_UNUSED( transform, center, radius, color, context );+}++static void b2EmptyDrawSolidCapsule( b2Pos p1, b2Pos p2, float radius, b2HexColor color, void* context )+{+	B2_UNUSED( p1, p2, radius, color, context );+}++static void b2EmptyDrawSegment( b2Pos p1, b2Pos p2, b2HexColor color, void* context )+{+	B2_UNUSED( p1, p2, color, context );+}++static void b2EmptyDrawTransform( b2WorldTransform transform, void* context )+{+	B2_UNUSED( transform, context );+}++static void b2EmptyDrawPoint( b2Pos p, float size, b2HexColor color, void* context )+{+	B2_UNUSED( p, size, color, context );+}++static void b2EmptyDrawString( b2Pos p, const char* s, b2HexColor color, void* context )+{+	B2_UNUSED( p, s, color, context );+}++static void b2EmptyDrawBounds( b2AABB aabb, b2HexColor color, void* context )+{+	B2_UNUSED( aabb, color, context );+}++b2DebugDraw b2DefaultDebugDraw( void )+{+	b2DebugDraw draw = { 0 };++	// These allow the user to skip some implementations and not hit null exceptions.+	draw.DrawPolygonFcn = b2EmptyDrawPolygon;+	draw.DrawSolidPolygonFcn = b2EmptyDrawSolidPolygon;+	draw.DrawCircleFcn = b2EmptyDrawCircle;+	draw.DrawSolidCircleFcn = b2EmptyDrawSolidCircle;+	draw.DrawSolidCapsuleFcn = b2EmptyDrawSolidCapsule;+	draw.DrawLineFcn = b2EmptyDrawSegment;+	draw.DrawTransformFcn = b2EmptyDrawTransform;+	draw.DrawPointFcn = b2EmptyDrawPoint;+	draw.DrawStringFcn = b2EmptyDrawString;+	draw.DrawBoundsFcn = b2EmptyDrawBounds;++	draw.drawingBounds.lowerBound = (b2Vec2){ -FLT_MAX, -FLT_MAX };+	draw.drawingBounds.upperBound = (b2Vec2){ FLT_MAX, FLT_MAX };+	draw.forceScale = 1.0f;+	draw.jointScale = 1.0f;+	draw.drawShapes = true;++	return draw;+}
+ upstream/src/weld_joint.c view
@@ -0,0 +1,474 @@+// SPDX-FileCopyrightText: 2023 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 "box2d/box2d.h"++#define B2_WELD_BLOCK_SOLVE 0++#if B2_WELD_BLOCK_SOLVE+typedef struct+{+	float x, y, z;+} b2Vec3;++// A 3-by-3 matrix. Stored in column-major order.+typedef struct+{+	b2Vec3 cx, cy, cz;+} b2Mat33;++static inline float b2Dot3( b2Vec3 a, b2Vec3 b )+{+	return a.x * b.x + a.y * b.y + a.z * b.z;+}++static inline b2Vec3 b2Cross3( b2Vec3 a, b2Vec3 b )+{+	return (b2Vec3){ a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x };+}++static inline b2Vec3 b2Solve33(const b2Mat33* m, b2Vec3 b )+{+	float det = b2Dot3( m->cx, b2Cross3( m->cy, m->cz ) );+	if ( det != 0.0f )+	{+		det = 1.0f / det;+	}++	b2Vec3 x;+	x.x = det * b2Dot3( b, b2Cross3( m->cy, m->cz ) );+	x.y = det * b2Dot3( m->cx, b2Cross3( b, m->cz ) );+	x.z = det * b2Dot3( m->cx, b2Cross3( m->cy, b ) );+	return x;+}+#endif++void b2WeldJoint_SetLinearHertz( b2JointId jointId, float hertz )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WeldJointSetLinearHertz, jointId, hertz );+	B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );+	joint->weldJoint.linearHertz = hertz;+}++float b2WeldJoint_GetLinearHertz( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );+	return joint->weldJoint.linearHertz;+}++void b2WeldJoint_SetLinearDampingRatio( b2JointId jointId, float dampingRatio )+{+	B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WeldJointSetLinearDampingRatio, jointId, dampingRatio );+	B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );+	joint->weldJoint.linearDampingRatio = dampingRatio;+}++float b2WeldJoint_GetLinearDampingRatio( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );+	return joint->weldJoint.linearDampingRatio;+}++void b2WeldJoint_SetAngularHertz( b2JointId jointId, float hertz )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WeldJointSetAngularHertz, jointId, hertz );+	B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );+	joint->weldJoint.angularHertz = hertz;+}++float b2WeldJoint_GetAngularHertz( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );+	return joint->weldJoint.angularHertz;+}++void b2WeldJoint_SetAngularDampingRatio( b2JointId jointId, float dampingRatio )+{+	B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WeldJointSetAngularDampingRatio, jointId, dampingRatio );+	B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );+	joint->weldJoint.angularDampingRatio = dampingRatio;+}++float b2WeldJoint_GetAngularDampingRatio( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );+	return joint->weldJoint.angularDampingRatio;+}++b2Vec2 b2GetWeldJointForce( b2World* world, b2JointSim* base )+{+	b2Vec2 force = b2MulSV( world->inv_h, base->weldJoint.linearImpulse );+	return force;+}++float b2GetWeldJointTorque( b2World* world, b2JointSim* base )+{+	return world->inv_h * base->weldJoint.angularImpulse;+}++// Point-to-point constraint+// C = p2 - p1+// Cdot = v2 - v1+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)+// J = [-E -r1_skew E 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++// 3x3 Block+// K = [J1] * invM * [J1T J2T]+//     [J2]+//   = [J1] * [invM * J1T invM * J2T]+//     [J2]+//   = [J1 * invM * J1T J1 * invM * J2T]+//     [J2 * invM * J1T J2 * invM * J2T]++void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_weldJoint );++	// chase body id to the solver set where the body lives+	int idA = base->bodyIdA;+	int idB = base->bodyIdB;++	b2World* world = context->world;++	b2Body* bodyA = b2Array_Get( world->bodies, idA );+	b2Body* bodyB = b2Array_Get( world->bodies, idB );++	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );+	b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex );+	b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex );++	int localIndexA = bodyA->localIndex;+	int localIndexB = bodyB->localIndex;++	b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA );+	b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB );++	float mA = bodySimA->invMass;+	float iA = bodySimA->invInertia;+	float mB = bodySimB->invMass;+	float iB = bodySimB->invInertia;++	base->invMassA = mA;+	base->invMassB = mB;+	base->invIA = iA;+	base->invIB = iB;++	b2WeldJoint* joint = &base->weldJoint;+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;++	// Compute joint anchor frames with world space rotation, relative to center of mass+	joint->frameA.q = b2MulRot( bodySimA->transform.q, base->localFrameA.q );+	joint->frameA.p = b2RotateVector( bodySimA->transform.q, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );+	joint->frameB.q = b2MulRot( bodySimB->transform.q, base->localFrameB.q );+	joint->frameB.p = b2RotateVector( bodySimB->transform.q, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );++	// Compute the initial center delta. Incremental position updates are relative to this.+	joint->deltaCenter = b2SubPos( bodySimB->center, bodySimA->center );++	float ka = iA + iB;+	joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f;++	if ( joint->linearHertz == 0.0f )+	{+		joint->linearSpring = base->constraintSoftness;+	}+	else+	{+		joint->linearSpring = b2MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h );+	}++	if ( joint->angularHertz == 0.0f )+	{+		joint->angularSpring = base->constraintSoftness;+	}+	else+	{+		joint->angularSpring = b2MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h );+	}++	if ( context->enableWarmStarting == false )+	{+		joint->linearImpulse = b2Vec2_zero;+		joint->angularImpulse = 0.0f;+	}+}++void b2WarmStartWeldJoint( b2JointSim* base, b2StepContext* context )+{+	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2WeldJoint* joint = &base->weldJoint;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearImpulse );+		stateA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + joint->angularImpulse );+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearImpulse );+		stateB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse );+	}+}++void b2SolveWeldJoint( b2JointSim* base, b2StepContext* context, bool useBias )+{+	B2_ASSERT( base->type == b2_weldJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2WeldJoint* joint = &base->weldJoint;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 vA = stateA->linearVelocity;+	float wA = stateA->angularVelocity;+	b2Vec2 vB = stateB->linearVelocity;+	float wB = stateB->angularVelocity;++	// Block solve doesn't work correctly with mixed stiffness values+#if B2_WELD_BLOCK_SOLVE+	// J = [-I -r1_skew I r2_skew]+	//     [ 0       -1 0       1]+	// r_skew = [-ry; rx]++	// Matlab+	// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]+	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]+	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	b2Mat33 K;+	K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;+	K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;+	K.cz.x = -rA.y * iA - rB.y * iB;+	K.cx.y = K.cy.x;+	K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;+	K.cz.y = rA.x * iA + rB.x * iB;+	K.cx.z = K.cz.x;+	K.cy.z = K.cz.y;+	K.cz.z = iA + iB;++	b2Vec3 bias = {0.0f, 0.0f, 0.0f};+	float linearMassScale = 1.0f;+	float linearImpulseScale = 0.0f;+	if ( useBias || joint->linearHertz > 0.0f )+	{+		// linear+		b2Vec2 dcA = stateA->deltaPosition;+		b2Vec2 dcB = stateB->deltaPosition;+		b2Vec2 jointTranslation = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter );++		bias.x = joint->linearSpring.biasRate * jointTranslation.x;+		bias.y = joint->linearSpring.biasRate * jointTranslation.y;+		+		linearMassScale = joint->linearSpring.massScale;+		linearImpulseScale = joint->linearSpring.impulseScale;+	}++	float angularMassScale = 1.0f;+	float angularImpulseScale = 0.0f;+	if ( useBias || joint->angularHertz > 0.0f )+	{+		// angular+		b2Rot qA = b2MulRot( stateA->deltaRotation, joint->frameA.q );+		b2Rot qB = b2MulRot( stateB->deltaRotation, joint->frameB.q );+		b2Rot relQ = b2InvMulRot( qA, qB );+		float jointAngle = b2Rot_GetAngle( relQ );++		bias.z = joint->angularSpring.biasRate * jointAngle;++		angularMassScale = joint->angularSpring.massScale;+		angularImpulseScale = joint->angularSpring.impulseScale;+	}++	b2Vec2 Cdot1 = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );+	float Cdot2 = wB - wA;++	b2Vec3 Cdot = {Cdot1.x + bias.x, Cdot1.y + bias.y, Cdot2 + bias.z};++	b2Vec3 b = b2Solve33( &K, Cdot );++	b2Vec2 linearImpulse = {+		-linearMassScale * b.x - linearImpulseScale * joint->linearImpulse.x,+		-linearMassScale * b.y - linearImpulseScale * joint->linearImpulse.y,+	};+	joint->linearImpulse = b2Add( joint->linearImpulse, linearImpulse );++	float angularImpulse = -angularMassScale * b.z - angularImpulseScale * joint->angularImpulse;+	joint->angularImpulse += angularImpulse;++	vA = b2MulSub( vA, mA, linearImpulse );+	wA -= iA * (b2Cross( rA, linearImpulse ) + angularImpulse);+	vB = b2MulAdd( vB, mB, linearImpulse );+	wB += iB * (b2Cross( rB, linearImpulse ) + angularImpulse);++	// todo debugging+	Cdot1 = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );+	Cdot2 = wB - wA;++	if ( useBias == false && b2Length(Cdot1) > 0.0001f )+	{+		Cdot1.x += 0.0f;+	}++	if ( useBias == false && b2AbsFloat( Cdot2 ) > 0.0001f )+	{+		Cdot2 += 0.0f;+	}++#else++	// angular constraint+	{+		b2Rot qA = b2MulRot( stateA->deltaRotation, joint->frameA.q );+		b2Rot qB = b2MulRot( stateB->deltaRotation, joint->frameB.q );+		b2Rot relQ = b2InvMulRot( qA, qB );+		float jointAngle = b2Rot_GetAngle( relQ );++		float bias = 0.0f;+		float massScale = 1.0f;+		float impulseScale = 0.0f;+		if ( useBias || joint->angularHertz > 0.0f )+		{+			float C = jointAngle;+			bias = joint->angularSpring.biasRate * C;+			massScale = joint->angularSpring.massScale;+			impulseScale = joint->angularSpring.impulseScale;+		}++		float Cdot = wB - wA;+		float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->angularImpulse;+		joint->angularImpulse += impulse;++		wA -= iA * impulse;+		wB += iB * impulse;+	}++	// linear constraint+	{+		b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+		b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++		b2Vec2 bias = b2Vec2_zero;+		float massScale = 1.0f;+		float impulseScale = 0.0f;+		if ( useBias || joint->linearHertz > 0.0f )+		{+			b2Vec2 dcA = stateA->deltaPosition;+			b2Vec2 dcB = stateB->deltaPosition;+			b2Vec2 C = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter );++			bias = b2MulSV( joint->linearSpring.biasRate, C );+			massScale = joint->linearSpring.massScale;+			impulseScale = joint->linearSpring.impulseScale;+		}++		b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );++		b2Mat22 K;+		K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;+		K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;+		K.cx.y = K.cy.x;+		K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;+		b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) );++		b2Vec2 impulse = {+			-massScale * b.x - impulseScale * joint->linearImpulse.x,+			-massScale * b.y - impulseScale * joint->linearImpulse.y,+		};++		joint->linearImpulse = b2Add( joint->linearImpulse, impulse );++		vA = b2MulSub( vA, mA, impulse );+		wA -= iA * b2Cross( rA, impulse );+		vB = b2MulAdd( vB, mB, impulse );+		wB += iB * b2Cross( rB, impulse );+	}+#endif++	B2_ASSERT( b2IsValidVec2( vA ) );+	B2_ASSERT( b2IsValidFloat( wA ) );+	B2_ASSERT( b2IsValidVec2( vB ) );+	B2_ASSERT( b2IsValidFloat( wB ) );++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = vA;+		stateA->angularVelocity = wA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = vB;+		stateB->angularVelocity = wB;+	}+}++void b2DrawWeldJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB,+					  float drawScale )+{+	B2_ASSERT( base->type == b2_weldJoint );++	b2WorldTransform frameA = b2MulWorldTransforms( transformA, base->localFrameA );+	b2WorldTransform frameB = b2MulWorldTransforms( transformB, base->localFrameB );++	b2Polygon box = b2MakeBox( 0.25f * drawScale, 0.125f * drawScale );+	draw->DrawPolygonFcn( frameA, box.vertices, 4, b2_colorDarkOrange, draw->context );+	draw->DrawPolygonFcn( frameB, box.vertices, 4, b2_colorDarkCyan, draw->context );+}
+ upstream/src/wheel_joint.c view
@@ -0,0 +1,594 @@+// SPDX-FileCopyrightText: 2023 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 "box2d/box2d.h"++void b2WheelJoint_EnableSpring( b2JointId jointId, bool enableSpring )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WheelJointEnableSpring, jointId, enableSpring );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );++	if ( enableSpring != joint->wheelJoint.enableSpring )+	{+		joint->wheelJoint.enableSpring = enableSpring;+		joint->wheelJoint.springImpulse = 0.0f;+	}+}++bool b2WheelJoint_IsSpringEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return joint->wheelJoint.enableSpring;+}++void b2WheelJoint_SetSpringHertz( b2JointId jointId, float hertz )+{+	B2_CHECK_INPUT( b2IsValidFloat( hertz ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WheelJointSetSpringHertz, jointId, hertz );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	joint->wheelJoint.hertz = hertz;+}++float b2WheelJoint_GetSpringHertz( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return joint->wheelJoint.hertz;+}++void b2WheelJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )+{+	B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WheelJointSetSpringDampingRatio, jointId, dampingRatio );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	joint->wheelJoint.dampingRatio = dampingRatio;+}++float b2WheelJoint_GetSpringDampingRatio( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return joint->wheelJoint.dampingRatio;+}++void b2WheelJoint_EnableLimit( b2JointId jointId, bool enableLimit )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WheelJointEnableLimit, jointId, enableLimit );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	if ( joint->wheelJoint.enableLimit != enableLimit )+	{+		joint->wheelJoint.lowerImpulse = 0.0f;+		joint->wheelJoint.upperImpulse = 0.0f;+		joint->wheelJoint.enableLimit = enableLimit;+	}+}++bool b2WheelJoint_IsLimitEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return joint->wheelJoint.enableLimit;+}++float b2WheelJoint_GetLowerLimit( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return joint->wheelJoint.lowerTranslation;+}++float b2WheelJoint_GetUpperLimit( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return joint->wheelJoint.upperTranslation;+}++void b2WheelJoint_SetLimits( b2JointId jointId, float lower, float upper )+{+	B2_CHECK_INPUT( b2IsValidFloat( lower ) );+	B2_CHECK_INPUT( b2IsValidFloat( upper ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WheelJointSetLimits, jointId, lower, upper );+	B2_ASSERT( lower <= upper );++	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	joint->wheelJoint.lowerTranslation = b2MinFloat( lower, upper );+	joint->wheelJoint.upperTranslation = b2MaxFloat( lower, upper );+}++void b2WheelJoint_EnableMotor( b2JointId jointId, bool enableMotor )+{+	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WheelJointEnableMotor, jointId, enableMotor );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	if ( joint->wheelJoint.enableMotor != enableMotor )+	{+		joint->wheelJoint.motorImpulse = 0.0f;+		joint->wheelJoint.enableMotor = enableMotor;+	}+}++bool b2WheelJoint_IsMotorEnabled( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return joint->wheelJoint.enableMotor;+}++void b2WheelJoint_SetTargetMotorSpeed( b2JointId jointId, float motorSpeed )+{+	B2_CHECK_INPUT( b2IsValidFloat( motorSpeed ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WheelJointSetTargetMotorSpeed, jointId, motorSpeed );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	joint->wheelJoint.motorSpeed = motorSpeed;+}++float b2WheelJoint_GetTargetMotorSpeed( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return joint->wheelJoint.motorSpeed;+}++float b2WheelJoint_GetCurrentMotorTorque( b2JointId jointId )+{+	b2World* world = b2GetWorld( jointId.world0 );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return world->inv_h * joint->wheelJoint.motorImpulse;+}++void b2WheelJoint_SetMaxMotorTorque( b2JointId jointId, float torque )+{+	B2_CHECK_INPUT( b2IsValidFloat( torque ) );++	b2World* world = b2GetWorld( jointId.world0 );+	B2_REC( world, WheelJointSetMaxMotorTorque, jointId, torque );+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	joint->wheelJoint.maxMotorTorque = torque;+}++float b2WheelJoint_GetMaxMotorTorque( b2JointId jointId )+{+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );+	return joint->wheelJoint.maxMotorTorque;+}++b2Vec2 b2GetWheelJointForce( b2World* world, b2JointSim* base )+{+	b2Rot qA = b2GetBodyTransform( world, base->bodyIdA ).q;++	b2Vec2 localAxisA = b2RotateVector( base->localFrameA.q, (b2Vec2){ 1.0f, 0.0f } );+	b2Vec2 axisA = b2RotateVector( qA, localAxisA );+	b2Vec2 perpA = b2LeftPerp( axisA );++	b2WheelJoint* joint = &base->wheelJoint;++	float perpForce = world->inv_h * joint->perpImpulse;+	float axialForce = world->inv_h * ( joint->springImpulse + joint->lowerImpulse - joint->upperImpulse );++	b2Vec2 force = b2Add( b2MulSV( perpForce, perpA ), b2MulSV( axialForce, axisA ) );+	return force;+}++float b2GetWheelJointTorque( b2World* world, b2JointSim* base )+{+	return world->inv_h * base->wheelJoint.motorImpulse;+}++// Linear constraint (point-to-line)+// d = pB - pA = xB + rB - xA - rA+// C = dot(ay, d)+// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))+//      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)+// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]++// Spring linear constraint+// C = dot(ax, d)+// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)+// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]++// Motor rotational constraint+// Cdot = wB - wA+// J = [0 0 -1 0 0 1]++void b2PrepareWheelJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_wheelJoint );++	// chase body id to the solver set where the body lives+	int idA = base->bodyIdA;+	int idB = base->bodyIdB;++	b2World* world = context->world;++	b2Body* bodyA = b2Array_Get( world->bodies, idA );+	b2Body* bodyB = b2Array_Get( world->bodies, idB );++	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );+	b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex );+	b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex );++	int localIndexA = bodyA->localIndex;+	int localIndexB = bodyB->localIndex;++	b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA );+	b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB );++	float mA = bodySimA->invMass;+	float iA = bodySimA->invInertia;+	float mB = bodySimB->invMass;+	float iB = bodySimB->invInertia;++	base->invMassA = mA;+	base->invMassB = mB;+	base->invIA = iA;+	base->invIB = iB;++	b2WheelJoint* joint = &base->wheelJoint;++	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;++	// Compute joint anchor frames with world space rotation, relative to center of mass+	joint->frameA.q = b2MulRot( bodySimA->transform.q, base->localFrameA.q );+	joint->frameA.p = b2RotateVector( bodySimA->transform.q, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );+	joint->frameB.q = b2MulRot( bodySimB->transform.q, base->localFrameB.q );+	joint->frameB.p = b2RotateVector( bodySimB->transform.q, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );++	// Compute the initial center delta. Incremental position updates are relative to this.+	joint->deltaCenter = b2SubPos( bodySimB->center, bodySimA->center );++	b2Vec2 rA = joint->frameA.p;+	b2Vec2 rB = joint->frameB.p;++	b2Vec2 d = b2Add( joint->deltaCenter, b2Sub( rB, rA ) );+	b2Vec2 axisA = b2RotateVector( joint->frameA.q, (b2Vec2){ 1.0f, 0.0f } );+	b2Vec2 perpA = b2LeftPerp( axisA );++	// perpendicular constraint (keep wheel on line)+	float s1 = b2Cross( b2Add( d, rA ), perpA );+	float s2 = b2Cross( rB, perpA );++	float kp = mA + mB + iA * s1 * s1 + iB * s2 * s2;+	joint->perpMass = kp > 0.0f ? 1.0f / kp : 0.0f;++	// spring constraint+	float a1 = b2Cross( b2Add( d, rA ), axisA );+	float a2 = b2Cross( rB, axisA );++	float ka = mA + mB + iA * a1 * a1 + iB * a2 * a2;+	joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f;++	joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );++	float km = iA + iB;+	joint->motorMass = km > 0.0f ? 1.0f / km : 0.0f;++	if ( context->enableWarmStarting == false )+	{+		joint->perpImpulse = 0.0f;+		joint->springImpulse = 0.0f;+		joint->motorImpulse = 0.0f;+		joint->lowerImpulse = 0.0f;+		joint->upperImpulse = 0.0f;+	}+}++void b2WarmStartWheelJoint( b2JointSim* base, b2StepContext* context )+{+	B2_ASSERT( base->type == b2_wheelJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2WheelJoint* joint = &base->wheelJoint;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );+	b2Vec2 axisA = b2RotateVector( joint->frameA.q, (b2Vec2){ 1.0f, 0.0f } );+	axisA = b2RotateVector( stateA->deltaRotation, axisA );+	b2Vec2 perpA = b2LeftPerp( axisA );++	float a1 = b2Cross( b2Add( d, rA ), axisA );+	float a2 = b2Cross( rB, axisA );+	float s1 = b2Cross( b2Add( d, rA ), perpA );+	float s2 = b2Cross( rB, perpA );++	float axialImpulse = joint->springImpulse + joint->lowerImpulse - joint->upperImpulse;++	b2Vec2 P = b2Add( b2MulSV( axialImpulse, axisA ), b2MulSV( joint->perpImpulse, perpA ) );+	float LA = axialImpulse * a1 + joint->perpImpulse * s1 + joint->motorImpulse;+	float LB = axialImpulse * a2 + joint->perpImpulse * s2 + joint->motorImpulse;++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P );+		stateA->angularVelocity -= iA * LA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P );+		stateB->angularVelocity += iB * LB;+	}+}++void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias )+{+	B2_ASSERT( base->type == b2_wheelJoint );++	float mA = base->invMassA;+	float mB = base->invMassB;+	float iA = base->invIA;+	float iB = base->invIB;++	// dummy state for static bodies+	b2BodyState dummyState = b2_identityBodyState;++	b2WheelJoint* joint = &base->wheelJoint;++	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;++	b2Vec2 vA = stateA->linearVelocity;+	float wA = stateA->angularVelocity;+	b2Vec2 vB = stateB->linearVelocity;+	float wB = stateB->angularVelocity;++	bool fixedRotation = ( iA + iB == 0.0f );++	// current anchors+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );++	b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );+	b2Vec2 axisA = b2RotateVector( joint->frameA.q, (b2Vec2){ 1.0f, 0.0f } );+	axisA = b2RotateVector( stateA->deltaRotation, axisA );+	float translation = b2Dot( axisA, d );++	float a1 = b2Cross( b2Add( d, rA ), axisA );+	float a2 = b2Cross( rB, axisA );++	// motor constraint+	if ( joint->enableMotor && fixedRotation == false )+	{+		float Cdot = wB - wA - joint->motorSpeed;+		float impulse = -joint->motorMass * Cdot;+		float oldImpulse = joint->motorImpulse;+		float maxImpulse = context->h * joint->maxMotorTorque;+		joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse );+		impulse = joint->motorImpulse - oldImpulse;++		wA -= iA * impulse;+		wB += iB * impulse;+	}++	// spring constraint+	if ( joint->enableSpring )+	{+		// This is a real spring and should be applied even during relax+		float C = translation;+		float bias = joint->springSoftness.biasRate * C;+		float massScale = joint->springSoftness.massScale;+		float impulseScale = joint->springSoftness.impulseScale;++		float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;+		float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse;+		joint->springImpulse += impulse;++		b2Vec2 P = b2MulSV( impulse, axisA );+		float LA = impulse * a1;+		float LB = impulse * a2;++		vA = b2MulSub( vA, mA, P );+		wA -= iA * LA;+		vB = b2MulAdd( vB, mB, P );+		wB += iB * LB;+	}++	if ( joint->enableLimit )+	{+		// Lower limit+		{+			float C = translation - joint->lowerTranslation;+			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 = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;+			float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->lowerImpulse;+			float oldImpulse = joint->lowerImpulse;+			joint->lowerImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f );+			impulse = joint->lowerImpulse - oldImpulse;++			b2Vec2 P = b2MulSV( impulse, axisA );+			float LA = impulse * a1;+			float LB = impulse * a2;++			vA = b2MulSub( vA, mA, P );+			wA -= iA * LA;+			vB = b2MulAdd( vB, mB, P );+			wB += iB * LB;+		}++		// 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->upperTranslation - 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 = b2Dot( axisA, b2Sub( vA, vB ) ) + a1 * wA - a2 * wB;+			float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse;+			float oldImpulse = joint->upperImpulse;+			joint->upperImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f );+			impulse = joint->upperImpulse - oldImpulse;++			b2Vec2 P = b2MulSV( impulse, axisA );+			float LA = impulse * a1;+			float LB = impulse * a2;++			// sign flipped on applied impulse+			vA = b2MulAdd( vA, mA, P );+			wA += iA * LA;+			vB = b2MulSub( vB, mB, P );+			wB -= iB * LB;+		}+	}++	// point to line constraint+	{+		b2Vec2 perpA = b2LeftPerp( axisA );++		float bias = 0.0f;+		float massScale = 1.0f;+		float impulseScale = 0.0f;+		if ( useBias )+		{+			float C = b2Dot( perpA, d );+			bias = base->constraintSoftness.biasRate * C;+			massScale = base->constraintSoftness.massScale;+			impulseScale = base->constraintSoftness.impulseScale;+		}++		float s1 = b2Cross( b2Add( d, rA ), perpA );+		float s2 = b2Cross( rB, perpA );+		float Cdot = b2Dot( perpA, b2Sub( vB, vA ) ) + s2 * wB - s1 * wA;++		float impulse = -massScale * joint->perpMass * ( Cdot + bias ) - impulseScale * joint->perpImpulse;+		joint->perpImpulse += impulse;++		b2Vec2 P = b2MulSV( impulse, perpA );+		float LA = impulse * s1;+		float LB = impulse * s2;++		vA = b2MulSub( vA, mA, P );+		wA -= iA * LA;+		vB = b2MulAdd( vB, mB, P );+		wB += iB * LB;+	}++	if ( stateA->flags & b2_dynamicFlag )+	{+		stateA->linearVelocity = vA;+		stateA->angularVelocity = wA;+	}++	if ( stateB->flags & b2_dynamicFlag )+	{+		stateB->linearVelocity = vB;+		stateB->angularVelocity = wB;+	}+}++#if 0+void b2WheelJoint_Dump()+{+	int32 indexA = joint->bodyA->joint->islandIndex;+	int32 indexB = joint->bodyB->joint->islandIndex;++	b2Dump("  b2WheelJointDef jd;\n");+	b2Dump("  jd.bodyA = sims[%d];\n", indexA);+	b2Dump("  jd.bodyB = sims[%d];\n", indexB);+	b2Dump("  jd.collideConnected = bool(%d);\n", joint->collideConnected);+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y);+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y);+	b2Dump("  jd.referenceAngle = %.9g;\n", joint->referenceAngle);+	b2Dump("  jd.enableLimit = bool(%d);\n", joint->enableLimit);+	b2Dump("  jd.lowerAngle = %.9g;\n", joint->lowerAngle);+	b2Dump("  jd.upperAngle = %.9g;\n", joint->upperAngle);+	b2Dump("  jd.enableMotor = bool(%d);\n", joint->enableMotor);+	b2Dump("  jd.motorSpeed = %.9g;\n", joint->motorSpeed);+	b2Dump("  jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque);+	b2Dump("  joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index);+}+#endif++void b2DrawWheelJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB,+					   float drawScale )+{+	B2_ASSERT( base->type == b2_wheelJoint );++	b2WheelJoint* joint = &base->wheelJoint;++	b2WorldTransform frameA = b2MulWorldTransforms( transformA, base->localFrameA );+	b2WorldTransform frameB = b2MulWorldTransforms( transformB, base->localFrameB );+	b2Vec2 axisA = b2RotateVector( frameA.q, (b2Vec2){ 1.0f, 0.0f } );++	b2HexColor c1 = b2_colorGray;+	b2HexColor c2 = b2_colorGreen;+	b2HexColor c3 = b2_colorRed;+	b2HexColor c4 = b2_colorDimGray;+	b2HexColor c5 = b2_colorBlue;++	draw->DrawLineFcn( frameA.p, frameB.p, c5, draw->context );++	if ( joint->enableLimit )+	{+		b2Pos lower = b2OffsetPos( frameA.p, b2MulSV( joint->lowerTranslation, axisA ) );+		b2Pos upper = b2OffsetPos( frameA.p, b2MulSV( joint->upperTranslation, axisA ) );+		b2Vec2 perp = b2LeftPerp( axisA );+		draw->DrawLineFcn( lower, upper, c1, draw->context );+		draw->DrawLineFcn( b2OffsetPos( lower, b2MulSV( -0.1f * drawScale, perp ) ),+						   b2OffsetPos( lower, b2MulSV( 0.1f * drawScale, perp ) ), c2, draw->context );+		draw->DrawLineFcn( b2OffsetPos( upper, b2MulSV( -0.1f * drawScale, perp ) ),+						   b2OffsetPos( upper, b2MulSV( 0.1f * drawScale, perp ) ), c3, draw->context );+	}+	else+	{+		draw->DrawLineFcn( b2OffsetPos( frameA.p, b2Neg( axisA ) ), b2OffsetPos( frameA.p, axisA ), c1, draw->context );+	}++	draw->DrawPointFcn( frameA.p, 5.0f, c1, draw->context );+	draw->DrawPointFcn( frameB.p, 5.0f, c4, draw->context );+}