Added a compile time option to use double precision body positions. Most
math remains single precision with minimal performance loss.

`BOX3D_DOUBLE_PRECISION`

Improved accuracy of body inertia computation when shapes are far from
the body origin.

WARNING: several APIs changed but migration should be straight forward
if doubles are not enabled.
This commit is contained in:
Erin Catto 2026-06-18 20:51:16 -07:00 • committed by GitHub
commit 09c65b08ea
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
115 changed files with 7591 additions and 5295 deletions

View file

@ -99,7 +99,43 @@ jobs:
- name: Test
working-directory: ${{github.workspace}}/build
run: ./bin/${{env.BUILD_TYPE}}/test
build-ubuntu-double:
name: ubuntu-double-precision
runs-on: ubuntu-latest
if: github.event_name == 'push' || github.event.pull_request.draft == false
timeout-minutes: 4
steps:
- uses: actions/checkout@v6
- name: Configure CMake
run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX3D_DOUBLE_PRECISION=ON -DBOX3D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON
- name: Build
run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
- name: Test
working-directory: ${{github.workspace}}/build
run: ./bin/test
build-windows-double:
name: windows-double-precision
runs-on: windows-latest
if: github.event_name == 'push' || github.event.pull_request.draft == false
timeout-minutes: 4
steps:
- uses: actions/checkout@v6
- name: Configure CMake
run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX3D_DOUBLE_PRECISION=ON -DBOX3D_SAMPLES=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON
- name: Build
run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
- name: Test
working-directory: ${{github.workspace}}/build
run: ./bin/${{env.BUILD_TYPE}}/test
samples-windows:
name: samples-windows
runs-on: windows-latest
@ -130,7 +166,27 @@ jobs:
- name: Configure CMake
run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=Release -DBOX3D_SAMPLES=ON -DBUILD_SHARED_LIBS=OFF -DBOX3D_UNIT_TESTS=OFF
- name: Build
run: cmake --build ${{github.workspace}}/build --config Release
samples-windows-double:
name: samples-windows-double-precision
runs-on: windows-latest
if: github.event_name == 'push' || github.event.pull_request.draft == false
timeout-minutes: 6
steps:
- uses: actions/checkout@v6
- name: Setup MSVC dev command prompt
uses: TheMrMilchmann/setup-msvc-dev@v4
with:
arch: x64
- name: Configure CMake
run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=Release -DBOX3D_DOUBLE_PRECISION=ON -DBOX3D_SAMPLES=ON -DBUILD_SHARED_LIBS=OFF -DBOX3D_UNIT_TESTS=OFF
- name: Build
run: cmake --build ${{github.workspace}}/build --config Release

1
.gitignore vendored
View file

@ -11,3 +11,4 @@ CMakeUserPresets.json
.claude/
.tmp/
.fetchcontent-cache/
build-dp/

View file

@ -88,6 +88,7 @@ endif()
option(BOX3D_DISABLE_SIMD "Disable SIMD math (slower)" OFF)
option(BOX3D_COMPILE_WARNING_AS_ERROR "Compile warnings as errors" OFF)
option(BOX3D_DOUBLE_PRECISION "Enable double precision for large worlds" OFF)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set_property(GLOBAL PROPERTY USE_FOLDERS ON)

View file

@ -52,6 +52,7 @@ doxygen_add_docs(doc
"simulation.md"
"loose_ends.md"
"recording.md"
"large_worlds.md"
"character.md"
"reading.md"
ALL

View file

@ -45,6 +45,7 @@ Each frame:
```c
float b3World_CastMover(
b3WorldId worldId,
b3Pos origin, // world position the mover is relative to
const b3Capsule* mover,
b3Vec3 translation,
b3QueryFilter filter,
@ -79,6 +80,7 @@ information. Use `b3World_CollideMover` for that.
```c
void b3World_CollideMover(
b3WorldId worldId,
b3Pos origin, // mover and returned planes are relative to this
const b3Capsule* mover,
b3QueryFilter filter,
b3PlaneResultFcn* fcn,
@ -121,9 +123,10 @@ int b3Body_CollideMover(
b3BodyId bodyId,
b3BodyPlaneResult* bodyPlanes,
int planeCapacity,
b3Pos origin,
const b3Capsule* mover,
b3QueryFilter filter,
b3Transform bodyTransform
b3WorldTransform bodyTransform
);
```
@ -173,11 +176,15 @@ mover is pressed against a surface.
## Putting It Together
```c
// The mover capsule and the planes are relative to origin. Keep origin near the
// character (its world position) so the query stays precise far from the world origin.
b3Pos origin = b3Pos_zero;
// 1. Desired translation from input + gravity integration
b3Vec3 translation = b3MulSV(timeStep, velocity);
// 2. Swept cast
float fraction = b3World_CastMover(worldId, &mover, translation, filter, NULL, NULL);
float fraction = b3World_CastMover(worldId, origin, &mover, translation, filter, NULL, NULL);
b3Vec3 safeDelta = b3MulSV(fraction, translation);
// 3. Move the capsule
@ -190,7 +197,7 @@ b3CollisionPlane collisionPlanes[MAX_PLANES];
int planeCount = 0;
// (user callback stores planes into collisionPlanes / planeCount)
b3World_CollideMover(worldId, &mover, filter, MyPlaneCallback, &planeCtx);
b3World_CollideMover(worldId, origin, &mover, filter, MyPlaneCallback, &planeCtx);
// 5. Solve planes
b3PlaneSolverResult result = b3SolvePlanes(b3Vec3_zero, collisionPlanes, planeCount);

View file

@ -314,17 +314,20 @@ shapes, each expressed as a `b3ShapeProxy`. It uses the GJK algorithm.
```c
b3DistanceInput input = {0};
input.proxyA = proxyA; // b3ShapeProxy for shape A
input.proxyB = proxyB; // b3ShapeProxy for shape B
input.transformA = transformA;
input.transformB = transformB;
input.useRadii = true;
input.proxyA = proxyA; // b3ShapeProxy for shape A
input.proxyB = proxyB; // b3ShapeProxy for shape B
input.transform = b3InvMulWorldTransforms(worldA, worldB); // relative pose of B in A
input.useRadii = true;
b3SimplexCache cache = b3_emptyDistanceCache;
b3DistanceOutput output = b3ShapeDistance(&input, &cache, NULL, 0);
// output.distance, output.pointA, output.pointB, output.normal
// output.distance, output.pointA, output.pointB, output.normal are in shape A's frame
```
The query is origin independent and runs in frame A, so the witness points and
normal come back in shape A's frame. Lift them into world space with shape A's
transform if needed.
The simplex cache warm-starts the algorithm when shapes move by small amounts
between calls. On the first call zero-initialize the cache (or use
`b3_emptyDistanceCache`).
@ -487,14 +490,18 @@ The callback returns a new `maxFraction`. Return 0 to stop, a fraction less
than `input->maxFraction` to clip the ray (e.g. for closest-hit semantics),
or `input->maxFraction` to continue unclipped.
### Shape Cast
### Box Cast
```c
b3TreeStats stats = b3DynamicTree_ShapeCast(
&tree, &shapeCastInput, maskBits, requireAllBits,
myShapeCastCallback, context);
b3TreeStats stats = b3DynamicTree_BoxCast(
&tree, &boxCastInput, maskBits, requireAllBits,
myBoxCastCallback, context);
```
The tree sweeps the AABB in `boxCastInput`; the caller folds the cast shape's radius (and any
world origin) into that box. The callback then does the precise narrow-phase cast against each
leaf, taking only the advancing fraction from the tree.
![Ray cast](images/raycast.svg)
![Overlap test](images/overlap_test.svg)

125
docs/large_worlds.md Normal file
View file

@ -0,0 +1,125 @@
# Large Worlds (Double Precision) {#large-worlds}
Box3D can be built with double precision world positions for large worlds: simulations that range
far from the origin, where a single precision float can no longer resolve a position. At a
coordinate of 1e7 meters a float has a step of about one meter, so bodies snap to a coarse grid and
contacts jitter. Double precision keeps full sub-millimeter resolution out to planetary distances.
Only the world position boundary is double. Velocities, forces, shapes, contact manifolds, the
contact solver, and the broad-phase tree all stay float. This is the same boundary design Jolt
Physics uses for its big-world mode: doubles carry the absolute position, everything inside one
body's frame stays float, and the per-step motion the solver integrates is small and meters-scale
where float precision is ample. The cost is a few percent, not the 2x of an all-double build.
Double precision is **off by default**. With it off every double-precision type collapses to its
float counterpart through a typedef and the boundary helpers reduce to plain float operations, so a
float build behaves exactly as Box3D always has, with no measurable cost.
This implementation is inspired by [Jolt](https://jrouwe.github.io/JoltPhysics/index.html#big-worlds).
## Enabling it
Set the CMake option:
```cmake
set(BOX3D_DOUBLE_PRECISION ON)
```
The define is propagated to consumers as a `PUBLIC` compile definition, so anything that links Box3D
through CMake sees the same precision mode in the headers and cannot mismatch. For non-CMake
consumers there is a link-time guard: a float application linked against a double-precision library
(or the reverse) fails to link on the first Box3D call rather than miscompiling silently. At runtime
`b3IsDoublePrecision()` reports which mode the library was built in, for bindings and diagnostics.
## The two world-position types
Double precision adds two types and leaves the existing math types alone:
- `b3Pos` — a world position. Three doubles in large world mode, an alias for `b3Vec3`
otherwise.
- `b3WorldTransform` — a world transform: a `b3Pos` translation and a float `b3Quat` rotation.
An alias for `b3Transform` otherwise.
`b3Vec3`, `b3Quat`, `b3Transform`, and `b3AABB` stay float in both modes. `b3Transform` remains the
type for local and relative frames; `b3WorldTransform` is used for world space. Rotations are float
quaternions in both modes, so the trig and the cross-platform determinism properties are unchanged.
The public API uses these types wherever it accepts or returns a world position: `b3BodyDef.position`,
`b3Body_GetPosition` / `b3Body_GetTransform`, `b3Body_SetTransform`, `b3Body_GetWorldPoint` /
`b3Body_GetLocalPoint`, `b3Body_GetWorldCenterOfMass`, the explosion and ray-cast origins, contact
and ray-cast result points, and the body move event. With double precision off these are all the
float types they have always been, so existing code compiles unchanged.
With double precision **on**, `b3Pos` and `b3Vec3` are distinct structs by design. Code that
passed a `b3Vec3` where a world position is now required no longer compiles, which is the intended
cost: enabling large world mode is a deliberate source migration, and the compiler points at every
site that needs a conversion. Helpers cover the boundary:
```c
b3Pos p = b3ToPos( v ); // float vector -> world position
b3Vec3 v = b3ToVec3( p ); // world position -> float vector (lossy far from origin)
b3Vec3 d = b3SubPos( a, b ); // a - b, demoted to float (the precision boundary)
b3Pos q = b3OffsetPos( p, d ); // p + d
float x = b3RoundDownFloat( p.x ); // conservative narrowing, pair with b3RoundUpFloat to
// build a float box that always contains double bounds
```
## Operating range
Full simulation correctness holds everywhere `b3Pos` can represent. Body integration, the
contact solver, joints, and continuous collision all run in float relative to each body's own
moving frame, so a stack settles and a bullet is caught the same way at 1e7 as at the origin.
The practical limit comes from the broad phase, which stays float and stores conservative
(outward-rounded) float bounds. Far from the origin the float bound quantization grows: about one
meter at 1e7, sixteen meters at 1e8. Overlapping shapes always still produce a pair, so correctness
is preserved, but beyond roughly 1e7 to 1e8 meters the broad phase reports extra false pairs and
loses some margin hysteresis, which costs performance. Stay within about ±1e7 to ±1e8 meters.
## Queries far from the origin
Every spatial query takes a caller supplied `b3Pos` origin and re-differences each shape against a
nearby base at full precision, so hit points and fractions stay accurate far from the world origin.
The one shared limit is the broad phase: the tree is traversed in conservative outward rounded float,
so it never misses a pair, but a cast that grazes a shape by less than a coordinate float ULP far
from the origin can still miss at the tree level. Only the explosion is a pure float carve-out.
- `b3World_OverlapShape`, `b3World_CastShape`, `b3World_CastMover`, and `b3World_CollideMover` take a
`b3Pos` origin. Their proxy, mover, and returned planes are relative to that origin and each shape
is re-differenced against it in float, so a query around a mover at 1e7 is as precise as one at the
origin. Shape cast hit points come back as `b3Pos`.
- `b3World_CastRay` / `b3World_CastRayClosest` take a `b3Pos` origin and re-difference each shape
against its body in full precision, so hit points and fractions stay accurate far from the origin.
The tree traversal itself is float (see the broad phase limit above). Hit points come back as
`b3Pos`.
- `b3Shape_RayCast` takes a `b3Pos` origin and a translation and returns a `b3WorldCastOutput` whose
hit point is a world `b3Pos`, re-centered on the origin for full precision.
- `b3World_Explode` resolves the per-shape impulse in float around the explosion position.
The character controller (`b3World_CastMover` / `b3World_CollideMover`) drives this: pass the
character's world position as the origin and the mover stays precise even at 1e7. The only remaining
float carve-out is the broad phase tree traversal, which Box2D shares.
## Debug drawing
`b3World_Draw` hands every callback world coordinates in the double-capable `b3Pos` and
`b3WorldTransform` types, so the engine stays camera agnostic. The host shifts into its own camera
frame inside the callbacks: keep a draw origin near the camera and difference against it in double
before the coordinates demote to float, and a distant scene draws crisply instead of snapping to the
coarse float grid around the absolute origin. A zero origin reproduces absolute coordinates and leaves
a near-origin scene unchanged. The sample app sets the draw origin from the camera eye each frame, and
the Large World sample uses it to render a stack at 1e7 with no jitter.
## Determinism
Both precision modes are internally deterministic and reproduce across worker counts. The numerics
differ between modes — double precision accumulates body positions in double, so a body settles and
sleeps on a slightly different step and the state hash differs — so a double-precision build is not
bit-identical to a float build. The `DeterminismTest` carries a separate set of expected values for
each mode.
## SIMD
Double precision is orthogonal to SIMD. The wide types used in the broad phase and mesh collision
stay 4-wide float, and the contact solver is untouched in both modes. There is no double-precision
SIMD path and none is needed: the hot interior never sees an absolute world coordinate.

View file

@ -553,9 +553,9 @@ b3Vec3 angularVelocity = b3Body_GetAngularVelocity(myBodyId);
You can drive a body to a specific transform. This is useful for kinematic bodies.
```c
b3Vec3 targetPosition = {42.0f, 0.0f, -100.0f};
b3Pos targetPosition = {42.0f, 0.0f, -100.0f};
b3Quat targetRotation = b3MakeQuatFromAxisAngle(b3Vec3_axisY, B3_PI);
b3Transform target = {targetPosition, targetRotation};
b3WorldTransform target = {targetPosition, targetRotation};
float timeStep = 1.0f / 60.0f;
b3Body_SetTargetTransform(myBodyId, target, timeStep, true);
```
@ -1775,7 +1775,7 @@ b3ShapeProxy proxy;
proxy.points = &sphere.center;
proxy.count = 1;
proxy.radius = sphere.radius;
b3World_OverlapShape(myWorldId, &proxy, grenadeFilter, MyOverlapCallback, &myGame);
b3World_OverlapShape(myWorldId, b3Pos_zero, &proxy, grenadeFilter, MyOverlapCallback, &myGame);
```
### Ray-casts
@ -1879,7 +1879,7 @@ proxy.points = &sphere.center;
proxy.count = 1;
proxy.radius = sphere.radius;
b3Vec3 translation = {10.0f, -5.0f, 0.0f};
b3World_CastShape(myWorldId, &proxy, translation, grenadeFilter, MyCastCallback, &context);
b3World_CastShape(myWorldId, b3Pos_zero, &proxy, translation, grenadeFilter, MyCastCallback, &context);
```
Shape-casts are setup similarly to ray-casts. Shape-casts are generally slower

View file

@ -1,62 +0,0 @@
# Vendored sokol headers
Upstream: <https://github.com/floooh/sokol>
Pinned commit: `b210caee10d6834081cb46c617c3ff715a92be26`
License: zlib (see `LICENSE`)
These are the only sokol headers render3d actually `#include`s:
| File | Upstream path | Used for |
| ----------------- | ------------------------- | ----------------------------------------- |
| `sokol_gfx.h` | `sokol_gfx.h` | Graphics backend (D3D11 / Metal / GL 4.3) |
| `sokol_app.h` | `sokol_app.h` | Windowing and input |
| `sokol_glue.h` | `sokol_glue.h` | Bridges sokol_app and sokol_gfx |
| `sokol_log.h` | `sokol_log.h` | Default logger callback |
| `sokol_imgui.h` | `util/sokol_imgui.h` | Dear ImGui renderer/event handler |
## Local patches
`sokol_gfx.h` carries a local patch (search the file for `Local patch:`) wiring
`sg_push_debug_group` / `sg_pop_debug_group` on the D3D11 backend to
`ID3DUserDefinedAnnotation::BeginEvent` / `EndEvent`. Upstream sokol_gfx
silently no-ops these on every backend except Metal, so D3D11 captures in
RenderDoc / PIX / Nsight had no hierarchical event regions. The patch:
- includes `<d3d11_1.h>`,
- adds `ID3DUserDefinedAnnotation* annotation` to `_sg_d3d11_backend_t`,
- QueryInterfaces it in `_sg_d3d11_setup_backend`, releases in
`_sg_d3d11_discard_backend`,
- implements `_sg_d3d11_push_debug_group` / `_sg_d3d11_pop_debug_group`
(UTF-8 -> UTF-16 conversion, stack buffer, 256 char cap),
- routes the public `_sg_push_debug_group` / `_sg_pop_debug_group`
dispatcher to them under `SOKOL_D3D11`.
`sokol_app.h` carries a local patch (search the file for `Local patch:`) in
`_sapp_win32_create_window` that changes how `desc.width` / `desc.height`
are interpreted on Win32 when `desc.high_dpi == true`. Upstream always
multiplies the requested size by the monitor's `window_scale`, treating
`desc.width` as 100%-DPI logical pixels — so callers that want a specific
framebuffer size on a high-DPI display have to pre-divide by the DPI scale
themselves. The patch makes the high-DPI path interpret `desc.width` /
`desc.height` as physical/framebuffer pixels directly; the `high_dpi=false`
branch keeps upstream behavior so DPI-unaware apps are unaffected.
When bumping either header, re-apply the patch and rebuild. Each patch is
small enough to re-do by hand each time; if either grows, consider
upstreaming.
## Bumping
1. Pull the new headers from upstream at the desired commit:
```
git clone --depth 1 https://github.com/floooh/sokol /tmp/sokol-src
(cd /tmp/sokol-src && git fetch --depth 1 origin <sha> && git checkout <sha>)
cp /tmp/sokol-src/sokol_{gfx,app,glue,log}.h extern/sokol/
cp /tmp/sokol-src/util/sokol_imgui.h extern/sokol/
cp /tmp/sokol-src/LICENSE extern/sokol/
```
2. Update the pinned commit at the top of this file.
3. Rebuild + run the smoke test on all three OSes.
4. Commit as `deps: bump sokol to <sha> (<reason>)`.

View file

@ -3,6 +3,7 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
// clang-format off
@ -148,6 +149,9 @@ typedef struct b3Version
/// Get the current version of Box3D
B3_API b3Version b3GetVersion( void );
/// @return true if the library was built with BOX3D_DOUBLE_PRECISION (large world mode)
B3_API bool b3IsDoublePrecision( void );
/**@}*/
//! @cond

View file

@ -16,12 +16,19 @@
* 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/
* information to get contact points and normals as well as events. You can query the world, checking for overlaps and casting
* rays or shapes. There is also debugging information such as debug draw, timing information, and counters. You can find
* documentation here: https://box2d.org/
* @{
*/
#if defined( BOX3D_DOUBLE_PRECISION )
// Force a link error if the application and library disagree on precision. A float app linking
// a double precision library, or the reverse, gets one unresolved external on the first call
// every program makes. CMake consumers inherit the define and cannot mismatch.
#define b3CreateWorld b3CreateWorldDoublePrecision
#endif
/// Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You may create
/// up to 128 worlds. Each world is completely independent and may be simulated in parallel.
/// @return the world id.
@ -68,8 +75,9 @@ B3_API b3JointEvents b3World_GetJointEvents( b3WorldId worldId );
B3_API b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn,
void* context );
/// Overlap test for all shapes that overlap the provided shape proxy.
B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3QueryFilter filter,
/// Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative
/// to the world origin, which lets the query stay precise far from the world origin.
B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter,
b3OverlapResultFcn* fcn, void* context );
/// Cast a ray into the world to collect shapes in the path of the ray.
@ -82,35 +90,38 @@ B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, const b3ShapeProxy*
/// @param fcn A user implemented callback function
/// @param context A user context that is passed along to the callback function
/// @return traversal performance counters
B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Vec3 origin, b3Vec3 translation, b3QueryFilter filter,
B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter,
b3CastResultFcn* fcn, void* context );
/// Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores initial overlap.
/// This is less general than b3World_CastRay() and does not allow for custom filtering.
B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Vec3 origin, b3Vec3 translation, b3QueryFilter filter );
B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter );
/// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point.
/// The proxy points are relative to the origin and the hit points come back as world positions, so the
/// cast stays precise far from the world origin.
/// @see b3World_CastRay
B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3Vec3 translation, b3QueryFilter filter,
b3CastResultFcn* fcn, void* context );
B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,
b3QueryFilter filter, b3CastResultFcn* fcn, void* context );
/// Cast a capsule mover through the world. This is a special shape cast that handles sliding along other shapes while reducing
/// clipping. This is not a good source of information about what the mover is touching. Instead use the planes returned by
/// b3World_CollideMover.
/// @param worldId World to cast the mover against
/// @param mover Capsule mover in world space
/// @param origin World position the mover capsule is relative to
/// @param mover Capsule mover, relative to the origin
/// @param translation Desired mover translation
/// @param filter Contains bit flags to filter unwanted shapes from the results
/// @param fcn Optional callback for custom shape filtering
/// @param context A user context that is passed along to the callback function
/// @return the translation fraction
B3_API float b3World_CastMover( b3WorldId worldId, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter,
B3_API float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter,
b3MoverFilterFcn* fcn, void* context );
/// Collide a capsule mover with the world, gathering collision planes that can be fed to b3SolvePlanes. Useful for
/// kinematic character movement.
B3_API void b3World_CollideMover( b3WorldId worldId, const b3Capsule* mover, b3QueryFilter filter, b3PlaneResultFcn* fcn,
void* context );
/// kinematic character movement. The mover and the returned planes are relative to the origin.
B3_API void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter,
b3PlaneResultFcn* fcn, void* context );
/// Enable/disable sleep. If your application does not need sleeping, you can gain some performance
/// by disabling sleep completely at the world level.
@ -286,24 +297,24 @@ B3_API void b3Body_SetUserData( b3BodyId bodyId, void* userData );
B3_API void* b3Body_GetUserData( b3BodyId bodyId );
/// Get the world position of a body. This is the location of the body origin.
B3_API b3Vec3 b3Body_GetPosition( b3BodyId bodyId );
B3_API b3Pos b3Body_GetPosition( b3BodyId bodyId );
/// Get the world rotation of a body as a quaternion
B3_API b3Quat b3Body_GetRotation( b3BodyId bodyId );
/// Get the world transform of a body.
B3_API b3Transform b3Body_GetTransform( b3BodyId bodyId );
B3_API b3WorldTransform b3Body_GetTransform( b3BodyId bodyId );
/// Set the world transform of a body. This acts as a teleport and is fairly expensive.
/// @note Generally you should create a body with the intended transform.
/// @see b3BodyDef::position and b3BodyDef::rotation
B3_API void b3Body_SetTransform( b3BodyId bodyId, b3Vec3 position, b3Quat rotation );
B3_API void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation );
/// Get a local point on a body given a world point
B3_API b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Vec3 worldPoint );
B3_API b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Pos worldPoint );
/// Get a world point on a body given a local point
B3_API b3Vec3 b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint );
B3_API b3Pos b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint );
/// Get a local vector on a body given a world vector
B3_API b3Vec3 b3Body_GetLocalVector( b3BodyId bodyId, b3Vec3 worldVector );
@ -327,13 +338,13 @@ B3_API void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity )
/// The result will be close but maybe not exact. This is meant for kinematic bodies.
/// The target is not applied if the velocity would be below the sleep threshold.
/// This will optionally wake the body if asleep, but only if the movement is significant.
B3_API void b3Body_SetTargetTransform( b3BodyId bodyId, b3Transform target, float timeStep, bool wake );
B3_API void b3Body_SetTargetTransform( b3BodyId bodyId, b3WorldTransform target, float timeStep, bool wake );
/// Get the linear velocity of a local point attached to a body. Usually in meters per second.
B3_API b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint );
/// Get the linear velocity of a world point attached to a body. Usually in meters per second.
B3_API b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Vec3 worldPoint );
B3_API b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Pos worldPoint );
/// Apply a force at a world point. If the force is not applied at the center of mass,
/// it will generate a torque and affect the angular velocity. This optionally wakes up the body.
@ -342,7 +353,7 @@ B3_API b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Vec3 worldPoint )
/// @param force The world force vector, usually in newtons (N)
/// @param point The world position of the point of application
/// @param wake Option to wake up the body
B3_API void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Vec3 point, bool wake );
B3_API void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Pos point, bool wake );
/// Apply a force to the center of mass. This optionally wakes up the body.
/// The force is ignored if the body is not awake.
@ -368,7 +379,7 @@ B3_API void b3Body_ApplyTorque( b3BodyId bodyId, b3Vec3 torque, bool wake );
/// @param wake also wake up the body
/// @warning This should be used for one-shot impulses. If you need a steady force,
/// use a force instead, which will work better with the sub-stepping solver.
B3_API void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Vec3 point, bool wake );
B3_API void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Pos point, bool wake );
/// Apply an impulse to the center of mass. This immediately modifies the velocity.
/// The impulse is ignored if the body is not awake. This optionally wakes the body.
@ -404,7 +415,7 @@ B3_API b3Matrix3 b3Body_GetWorldInverseRotationalInertia( b3BodyId bodyId );
B3_API b3Vec3 b3Body_GetLocalCenterOfMass( b3BodyId bodyId );
/// Get the center of mass position of the body in world space
B3_API b3Vec3 b3Body_GetWorldCenterOfMass( b3BodyId bodyId );
B3_API b3Pos b3Body_GetWorldCenterOfMass( b3BodyId bodyId );
/// Override the body's mass properties. Normally this is computed automatically using the
/// shape geometry and density. This information is lost if a shape is added or removed or if the
@ -527,17 +538,21 @@ B3_API b3AABB b3Body_ComputeAABB( b3BodyId bodyId );
B3_API float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target );
/// Cast a ray at a specific body using a specified body transform.
B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, const b3BodyRayCastInput* input, b3Transform bodyTransform );
B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter,
float maxFraction, b3WorldTransform bodyTransform );
/// Cast a shape at a specific body using a specified body transform.
B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, const b3BodyShapeCastInput* input, b3Transform bodyTransform );
B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,
b3QueryFilter filter, float maxFraction, bool canEncroach,
b3WorldTransform bodyTransform );
/// Overlap a shape with a specific body using a specified body transform.
B3_API bool b3Body_OverlapShape( b3BodyId bodyId, const b3ShapeProxy* proxy, b3QueryFilter filter, b3Transform bodyTransform );
B3_API bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter,
b3WorldTransform bodyTransform );
/// Collide a character mover with a specific body using a specified body transform.
B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, const b3Capsule* mover,
b3QueryFilter filter, b3Transform bodyTransform );
B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, const b3Capsule* mover,
b3QueryFilter filter, b3WorldTransform bodyTransform );
/** @} */ // body
@ -567,8 +582,8 @@ B3_API b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, cons
/// Use this for non-uniform or mirrored scale or a baked local transform. The baked result is shared through the
/// world hull database. The shape definition and geometry are fully cloned. Contacts are not created until the next time step.
/// @return the shape id for accessing the shape
B3_API b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull, b3Transform transform,
b3Vec3 scale );
B3_API b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull,
b3Transform transform, b3Vec3 scale );
/// Create a mesh hull shape and attach it to a body. The shape definition is fully cloned but the mesh is not.
/// Contacts are not created until the next time step.
@ -694,8 +709,9 @@ B3_API void b3Shape_EnableHitEvents( b3ShapeId shapeId, bool flag );
/// Returns true if hit events are enabled
B3_API bool b3Shape_AreHitEventsEnabled( b3ShapeId shapeId );
/// Ray cast a shape directly
B3_API b3CastOutput b3Shape_RayCast( b3ShapeId shapeId, const b3RayCastInput* input );
/// Ray cast a shape directly. The ray runs from origin to origin + translation and the hit point
/// comes back as a world position, so the cast stays precise far from the world origin.
B3_API b3WorldCastOutput b3Shape_RayCast( b3ShapeId shapeId, b3Pos origin, b3Vec3 translation );
/// Get a copy of the shape's sphere. Asserts the type is correct.
B3_API b3Sphere b3Shape_GetSphere( b3ShapeId shapeId );

View file

@ -75,20 +75,11 @@ B3_API b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3
B3_API b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits,
bool requireAllBits, b3TreeRayCastCallbackFcn* callback, 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.
/// @param tree the dynamic tree to ray cast
/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
/// @param maskBits bit mask test: `bool accept = (maskBits & node->categoryBits) != 0;`
/// @param requireAllBits modifies bit mask test: `bool accept = (maskBits & node->categoryBits) == maskBits;`
/// @param callback a callback function that is called for each proxy that is hit by the shape
/// @param context user context that is passed to the callback
/// @return performance data
B3_API b3TreeStats b3DynamicTree_ShapeCast( const b3DynamicTree* tree, const b3ShapeCastInput* input, uint64_t maskBits,
bool requireAllBits, b3TreeShapeCastCallbackFcn* callback, void* context );
/// Sweep an AABB through the tree. The box is in the tree's world float frame and the callback
/// re-differences each shape at full precision against the query origin. Used by the large world
/// spatial queries so the tree traversal stays float while the narrow phase stays precise.
B3_API b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits,
bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context );
/// Validate this tree. For testing.
B3_API void b3DynamicTree_Validate( const b3DynamicTree* tree );
@ -203,6 +194,9 @@ B3_API b3HullData* b3CreateCylinder( float height, float radius, float yOffset,
/// Create a tessellated cone as a hull.
B3_API b3HullData* b3CreateCone( float height, float radius1, float radius2, int slices );
/// Create a rock shaped hull.
B3_API b3HullData* b3CreateRock( float radius );
/// Create a generic convex hull.
B3_API b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount );
@ -532,11 +526,13 @@ B3_API void b3QueryHeightField( const b3HeightField* heightField, b3AABB bounds,
/// Compute the closest points between two shapes represented as point clouds.
/// b3SimplexCache cache is input/output. On the first call set b3SimplexCache.count to zero.
/// The query runs in frame A, so the witness points and normal are returned in frame A.
/// The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these.
B3_API b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* cache, b3Simplex* simplexes,
int simplexCapacity );
/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction.
/// The query runs in frame A, so the hit point and normal are returned in frame A. Initially touching shapes are a miss.
B3_API b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input );
/// Evaluate the transform sweep at a specific time.

View file

@ -14,9 +14,14 @@ B3_API void b3SetLengthUnitsPerMeter( float lengthUnits );
/// Get the current length units per meter.
B3_API float b3GetLengthUnitsPerMeter( void );
// Used to detect bad values. Positions greater than about 16km will have precision
// problems, so 100km as a limit should be fine in all cases.
#define B3_HUGE ( 100000.0f * b3GetLengthUnitsPerMeter() )
// Used to detect bad values. In float mode positions greater than about 16km have precision
// problems, so 100km is a safe limit. Large world mode keeps coordinates accurate much farther
// from the origin, so the sanity limit widens to keep valid far-field positions from tripping it.
#if defined( BOX3D_DOUBLE_PRECISION )
#define B3_HUGE ( 1.0e9f * b3GetLengthUnitsPerMeter() )
#else
#define B3_HUGE ( 1.0e5f * b3GetLengthUnitsPerMeter() )
#endif
/// Maximum parallel workers. Used for some fixed size arrays.
#define B3_MAX_WORKERS 32

View file

@ -67,6 +67,33 @@ typedef struct b3Transform
b3Quat q;
} b3Transform;
#if defined( BOX3D_DOUBLE_PRECISION )
/// A world position. Double precision in large world mode so coordinates stay accurate far
/// from the origin.
typedef struct b3Pos
{
double x, y, z;
} b3Pos;
/// A world transform with double precision translation and float quaternion rotation. Rotation
/// is frame local and never needs the extra range, the same split as Jolt's DMat44.
typedef struct b3WorldTransform
{
b3Pos p;
b3Quat q;
} b3WorldTransform;
#else
/// In single precision mode these types are the same.
typedef b3Vec3 b3Pos;
/// In single precision mode these types are the same.
typedef b3Transform b3WorldTransform;
#endif
/// A 3x3 matrix.
typedef struct b3Matrix3
{
@ -106,6 +133,10 @@ static const b3Matrix3 b3Mat3_identity = {
{ 0.0f, 0.0f, 1.0f },
};
// Valid in both modes: 0.0f promotes to double, the identity rotation stays float
static const b3Pos b3Pos_zero = { 0.0f, 0.0f, 0.0f };
static const b3WorldTransform b3WorldTransform_identity = { { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f } };
/// @return the minimum of two integers.
B3_INLINE int b3MinInt( int a, int b )
{
@ -591,6 +622,136 @@ B3_INLINE b3Vec3 b3InvTransformPoint( b3Transform t, b3Vec3 v )
return b3InvRotateVector( t.q, b3Sub( v, t.p ) );
}
// World position boundary. These cross between the double precision world space at the public
// boundary and the float interior. One set of bodies serves both modes: the typedefs collapse
// the types in float mode and the explicit float casts become no-ops.
/// Convert a vector to a world position.
B3_INLINE b3Pos b3ToPos( b3Vec3 v )
{
return B3_LITERAL( b3Pos ){ v.x, v.y, v.z };
}
/// Lossy conversion of a world position to a float vector.
B3_INLINE b3Vec3 b3ToVec3( b3Pos p )
{
return B3_LITERAL( b3Vec3 ){ (float)p.x, (float)p.y, (float)p.z };
}
/// Narrow a world coordinate to float, rounding toward negative infinity. Use with
/// b3RoundUpFloat to build a conservative float box that always contains the double bounds,
/// where plain rounding far from the origin could clip. nextafterf is an exact IEEE operation,
/// so this is cross-platform deterministic. With large world mode off this is a plain conversion.
B3_INLINE float b3RoundDownFloat( double x )
{
#if defined( BOX3D_DOUBLE_PRECISION )
float f = (float)x;
return (double)f > x ? nextafterf( f, -FLT_MAX ) : f;
#else
return (float)x;
#endif
}
/// Narrow a world coordinate to float, rounding toward positive infinity.
B3_INLINE float b3RoundUpFloat( double x )
{
#if defined( BOX3D_DOUBLE_PRECISION )
float f = (float)x;
return (double)f < x ? nextafterf( f, FLT_MAX ) : f;
#else
return (float)x;
#endif
}
/// a - b, demoted to float. The primary precision boundary operation.
B3_INLINE b3Vec3 b3SubPos( b3Pos a, b3Pos b )
{
return B3_LITERAL( b3Vec3 ){ (float)( a.x - b.x ), (float)( a.y - b.y ), (float)( a.z - b.z ) };
}
/// p + d
B3_INLINE b3Pos b3OffsetPos( b3Pos p, b3Vec3 d )
{
return B3_LITERAL( b3Pos ){ p.x + d.x, p.y + d.y, p.z + d.z };
}
/// World position interpolation for sweeps and sampling.
B3_INLINE b3Pos b3LerpPosition( b3Pos a, b3Pos b, float t )
{
return B3_LITERAL( b3Pos ){
( 1.0f - t ) * a.x + t * b.x,
( 1.0f - t ) * a.y + t * b.y,
( 1.0f - t ) * a.z + t * b.z,
};
}
/// Transform a local point to a world position. Rotation in float, translation in double.
B3_INLINE b3Pos b3TransformWorldPoint( b3WorldTransform t, b3Vec3 p )
{
b3Vec3 r = b3RotateVector( t.q, p );
return B3_LITERAL( b3Pos ){ t.p.x + r.x, t.p.y + r.y, t.p.z + r.z };
}
/// Transform a world position to a local point. One double subtraction, then float.
B3_INLINE b3Vec3 b3InvTransformWorldPoint( b3WorldTransform t, b3Pos p )
{
b3Vec3 d = { (float)( p.x - t.p.x ), (float)( p.y - t.p.y ), (float)( p.z - t.p.z ) };
return b3InvRotateVector( t.q, d );
}
/// Relative transform of frame B in frame A. The narrow phase boundary.
B3_INLINE b3Transform b3InvMulWorldTransforms( b3WorldTransform A, b3WorldTransform B )
{
b3Transform C;
C.q = b3InvMulQuat( A.q, B.q );
b3Vec3 d = { (float)( B.p.x - A.p.x ), (float)( B.p.y - A.p.y ), (float)( B.p.z - A.p.z ) };
C.p = b3InvRotateVector( A.q, d );
return C;
}
/// Compose a world transform with a local transform.
B3_INLINE b3WorldTransform b3MulWorldTransforms( b3WorldTransform A, b3Transform B )
{
b3WorldTransform C;
C.q = b3MulQuat( A.q, B.q );
b3Vec3 r = b3RotateVector( A.q, B.p );
C.p = B3_LITERAL( b3Pos ){ A.p.x + r.x, A.p.y + r.y, A.p.z + r.z };
return C;
}
/// Shift a world transform into the frame of a base position.
B3_INLINE b3Transform b3ToRelativeTransform( b3WorldTransform t, b3Pos base )
{
b3Transform r;
r.q = t.q;
r.p = B3_LITERAL( b3Vec3 ){ (float)( t.p.x - base.x ), (float)( t.p.y - base.y ), (float)( t.p.z - base.z ) };
return r;
}
/// Promote a float transform to a world transform. Lossless.
B3_INLINE b3WorldTransform b3MakeWorldTransform( b3Transform t )
{
b3WorldTransform w;
w.p = b3ToPos( t.p );
w.q = t.q;
return w;
}
/// Translate a local AABB by a world origin, rounding outward so the float box always contains
/// the double box. Far from the origin a plain conversion could clip a shape out of its own box.
/// In float mode the origin is float and the rounding is a no-op.
B3_INLINE b3AABB b3OffsetAABB( b3AABB localBox, b3Pos origin )
{
b3AABB out;
out.lowerBound.x = b3RoundDownFloat( origin.x + localBox.lowerBound.x );
out.lowerBound.y = b3RoundDownFloat( origin.y + localBox.lowerBound.y );
out.lowerBound.z = b3RoundDownFloat( origin.z + localBox.lowerBound.z );
out.upperBound.x = b3RoundUpFloat( origin.x + localBox.upperBound.x );
out.upperBound.y = b3RoundUpFloat( origin.y + localBox.upperBound.y );
out.upperBound.z = b3RoundUpFloat( origin.z + localBox.upperBound.z );
return out;
}
/// Compute the determinant of a 3-by-3 matrix.
B3_INLINE float b3Det( b3Matrix3 m )
{
@ -890,12 +1051,21 @@ B3_API bool b3IsValidMatrix3( b3Matrix3 a );
/// Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound.
B3_API bool b3IsValidAABB( b3AABB a );
/// Is this AABB valid and not huge or very far from the origin?
/// Is this AABB reasonably close to the origin? See B3_HUGE.
B3_API bool b3IsBoundedAABB( b3AABB a );
/// Is this AABB valid and reasonable?
B3_API bool b3IsSaneAABB( b3AABB a );
/// Is this a valid plane? Normal is a unit vector. Not Nan or infinity.
B3_API bool b3IsValidPlane( b3Plane a );
/// Is this a valid world position? Not NaN or infinity.
B3_API bool b3IsValidPosition( b3Pos p );
/// Is this a valid world transform? Not NaN or infinity. Rotation is normalized.
B3_API bool b3IsValidWorldTransform( b3WorldTransform t );
/**@}*/ // math
/**
@ -971,6 +1141,28 @@ B3_FORCE_INLINE b3Vec3 operator-( b3Vec3 a, b3Vec3 b )
return { a.x - b.x, a.y - b.y, a.z - b.z };
}
#if defined( BOX3D_DOUBLE_PRECISION )
/// Offset a world position by a vector.
B3_FORCE_INLINE b3Pos operator+( b3Pos a, b3Vec3 b )
{
return { a.x + b.x, a.y + b.y, a.z + b.z };
}
/// Offset a world position by a vector.
B3_FORCE_INLINE b3Pos operator-( b3Pos a, b3Vec3 b )
{
return { a.x - b.x, a.y - b.y, a.z - b.z };
}
/// Delta between two world positions, demoted to float.
B3_FORCE_INLINE b3Vec3 operator-( b3Pos a, b3Pos b )
{
return { (float)( a.x - b.x ), (float)( a.y - b.y ), (float)( a.z - b.z ) };
}
#endif
#endif
/**@}*/ // math_cpp

View file

@ -80,7 +80,7 @@ typedef bool b3CustomFilterFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, void* co
/// full contact manifold.
/// @warning Do not attempt to modify the world inside this callback
/// @ingroup world
typedef bool b3PreSolveFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Vec3 point, b3Vec3 normal, void* context );
typedef bool b3PreSolveFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, void* context );
/// Prototype callback for overlap queries.
/// Called for each shape found in the query.
@ -107,7 +107,7 @@ typedef bool b3OverlapResultFcn( b3ShapeId shapeId, void* context );
/// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue
/// @see b3World_CastRay
/// @ingroup world
typedef float b3CastResultFcn( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t userMaterialId,
typedef float b3CastResultFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId,
int triangleIndex, int childIndex, void* context );
/// Optional world capacities that can be use to avoid run-time allocations
@ -268,7 +268,7 @@ typedef struct b3BodyDef
/// The initial world position of the body. Bodies should be created with the desired position.
/// @note Creating bodies at the origin and then moving them nearly doubles the cost of body creation, especially
/// if the body is moved after shapes have been added.
b3Vec3 position;
b3Pos position;
/// The initial world rotation of the body.
b3Quat rotation;
@ -1009,7 +1009,7 @@ typedef struct b3ExplosionDef
uint64_t maskBits;
/// The center of the explosion in world space
b3Vec3 position;
b3Pos position;
/// The radius of the explosion
float radius;
@ -1143,7 +1143,7 @@ typedef struct b3ContactHitEvent
/// Point where the shapes hit at the beginning of the time step.
/// This is a mid-point between the two surfaces. It could be at speculative
/// point where the two shapes were not touching at the beginning of the time step.
b3Vec3 point;
b3Pos point;
/// Normal vector pointing from shape A to shape B
b3Vec3 normal;
@ -1199,7 +1199,7 @@ typedef struct b3BodyMoveEvent
void* userData;
/// The body transform.
b3Transform transform;
b3WorldTransform transform;
/// The body id.
b3BodyId bodyId;
@ -1313,7 +1313,7 @@ typedef struct b3RayResult
b3ShapeId shapeId;
/// The world point of the hit.
b3Vec3 point;
b3Pos point;
/// The world normal of the shape surface at the hit point.
b3Vec3 normal;
@ -1373,6 +1373,21 @@ typedef struct b3ShapeCastInput
bool canEncroach;
} b3ShapeCastInput;
/// Input for sweeping an AABB through a dynamic tree. The box is in the tree's world float frame.
/// The caller folds the cast shape radius and any world origin into the box, so the tree traversal
/// stays a conservative box sweep and the precise narrow phase happens per shape in the callback.
typedef struct b3BoxCastInput
{
/// The AABB to cast, in the tree's frame.
b3AABB box;
/// The sweep translation.
b3Vec3 translation;
/// The maximum fraction of the translation to consider, typically 1.
float maxFraction;
} b3BoxCastInput;
/// Low level ray cast or shape-cast output data.
typedef struct b3CastOutput
{
@ -1401,43 +1416,42 @@ typedef struct b3CastOutput
bool hit;
} b3CastOutput;
/// Body ray cast for ray casting a specific body with a specified transform.
typedef struct b3BodyRayCastInput
#if defined( BOX3D_DOUBLE_PRECISION )
/// Ray cast or shape-cast output in world space. The hit point is a world position so the result
/// stays precise far from the world origin. Mirrors b3CastOutput with a double precision point.
typedef struct b3WorldCastOutput
{
/// Start point of the ray cast
b3Vec3 origin;
/// The surface normal at the hit point.
b3Vec3 normal;
/// Translation of the ray cast.
/// end = start + translation.
b3Vec3 translation;
/// The surface hit point in world space.
b3Pos point;
/// The query filter bits.
b3QueryFilter filter;
/// The fraction of the input translation at collision.
float fraction;
/// The maximum fraction to consider along the ray.
/// end = start + fraction * translation
float maxFraction;
} b3BodyRayCastInput;
/// The number of iterations used.
int iterations;
/// Body shape cast for shape casting a specific body with a specified transform.
typedef struct b3BodyShapeCastInput
{
/// A generic query shape.
b3ShapeProxy proxy;
/// The index of the mesh or height field triangle hit.
int triangleIndex;
/// The translation of the shape cast.
b3Vec3 translation;
/// The index of the compound child shape.
int childIndex;
/// The query filter bits.
b3QueryFilter filter;
/// The material index. May be -1 for null.
int materialIndex;
/// The maximum fraction to consider along the shape cast.
/// end = start + fraction * translation
float maxFraction;
/// Did the cast hit?
bool hit;
} b3WorldCastOutput;
/// Allow shape cast to encroach when initially touching. This only works if the radius is greater than zero.
bool canEncroach;
} b3BodyShapeCastInput;
#else
typedef b3CastOutput b3WorldCastOutput;
#endif
/// Body cast result for ray and shape casts.
typedef struct b3BodyCastResult
@ -1446,7 +1460,7 @@ typedef struct b3BodyCastResult
b3ShapeId shapeId;
/// The world point on the shape surface.
b3Vec3 point;
b3Pos point;
/// The world normal vector on the shape surface.
b3Vec3 normal;
@ -1495,13 +1509,12 @@ static const b3SimplexCache b3_emptyDistanceCache = B3_ZERO_INIT;
/// Input parameters for b3ShapeCast
typedef struct b3ShapeCastPairInput
{
b3ShapeProxy proxyA; ///< The proxy for shape A
b3ShapeProxy proxyB; ///< The proxy for shape B
b3Transform transformA; ///< The world transform for shape A
b3Transform transformB; ///< The world transform for shape B
b3Vec3 translationB; ///< The translation of shape B
float maxFraction; ///< The fraction of the translation to consider, typically 1
bool canEncroach; ///< Allows shapes with a radius to move slightly closer if already touching
b3ShapeProxy proxyA; ///< The proxy for shape A
b3ShapeProxy proxyB; ///< The proxy for shape B
b3Transform transform; ///< Transform of shape B in shape A's frame, the relative pose B in A
b3Vec3 translationB; ///< The translation of shape B, in A's frame
float maxFraction; ///< The fraction of the translation to consider, typically 1
bool canEncroach; ///< Allows shapes with a radius to move slightly closer if already touching
} b3ShapeCastPairInput;
/// Input for b3ShapeDistance
@ -1513,11 +1526,9 @@ typedef struct b3DistanceInput
/// The proxy for shape B
b3ShapeProxy proxyB;
/// The world transform for shape A
b3Transform transformA;
/// The world transform for shape B
b3Transform transformB;
/// Transform of shape B in shape A's frame, the relative pose B in A
/// (b3InvMulWorldTransforms( worldA, worldB )). The query is origin independent and runs in frame A.
b3Transform transform;
/// Should the proxy radius be considered?
bool useRadii;
@ -1526,9 +1537,9 @@ typedef struct b3DistanceInput
/// Output for b3ShapeDistance
typedef struct b3DistanceOutput
{
b3Vec3 pointA; ///< Closest point on shapeA
b3Vec3 pointB; ///< Closest point on shapeB
b3Vec3 normal; ///< Normal vector pointing from A to B
b3Vec3 pointA; ///< Closest point on shapeA, in shape A's frame
b3Vec3 pointB; ///< Closest point on shapeB, in shape A's frame
b3Vec3 normal; ///< A to B normal in shape A's frame. Invalid if distance is zero.
float distance; ///< The final distance, zero if overlapped
int iterations; ///< Number of GJK iterations used
int simplexCount; ///< The number of simplexes stored in the simplex array
@ -1637,7 +1648,7 @@ typedef struct b3TOIOutput
* @{
*/
/// Flags for dynamic tree nodes. Internal usage.
/// Flags for tree nodes. For internal usage.
typedef enum b3TreeNodeFlags
{
b3_allocatedNode = 0x0001,
@ -1645,7 +1656,7 @@ typedef enum b3TreeNodeFlags
b3_leafNode = 0x0004,
} b3TreeNodeFlags;
/// Flags for dynamic tree node child indices. Internal usage.
/// Tree node child indices. For internal usage.
typedef struct b3TreeNodeChildren
{
int child1; ///< child node index 1
@ -1750,12 +1761,12 @@ typedef bool b3TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* conte
/// @return minimum distance squared to user objects in the proxy
typedef float b3TreeQueryClosestCallbackFcn( float distanceSqrMin, int proxyId, uint64_t userData, void* context );
/// This function receives clipped ray cast input for a proxy. The function
/// returns the new ray fraction.
/// - return a value of 0 to terminate the ray cast
/// - return a value less than input->maxFraction to clip the ray
/// - return a value of input->maxFraction to continue the ray cast without clipping
typedef float b3TreeShapeCastCallbackFcn( const b3ShapeCastInput* input, int proxyId, uint64_t userData, void* context );
/// This function receives clipped AABB cast input for a proxy. The function returns the new cast
/// fraction.
/// - return a value of 0 to terminate the cast
/// - return a value less than input->maxFraction to clip the cast
/// - return a value of input->maxFraction to continue the cast without clipping
typedef float b3TreeBoxCastCallbackFcn( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context );
/// This function receives clipped ray cast input for a proxy. The function
/// returns the new ray fraction.
@ -1848,8 +1859,8 @@ typedef struct b3MassData
/// The local center of mass position.
b3Vec3 center;
/// The inertia tensor.
b3Matrix3 inertia; // todo this should be central inertia
/// The inertia tensor about the shape center of mass.
b3Matrix3 inertia;
} b3MassData;
/**
@ -2910,28 +2921,30 @@ typedef struct b3DebugShape
} b3DebugShape;
/// This struct is passed to b3World_Draw to draw a debug view of the simulation world.
/// Callbacks receive world coordinates. In large world mode the translation is double precision so
/// it stays accurate far from the origin. Shift into your own camera frame inside the callbacks.
typedef struct b3DebugDraw
{
/// Draws a shape and returns true if drawing should continue
bool ( *DrawShapeFcn )( void* userShape, b3Transform transform, b3HexColor color, void* context );
bool ( *DrawShapeFcn )( void* userShape, b3WorldTransform transform, b3HexColor color, void* context );
/// Draw a line segment.
void ( *DrawSegmentFcn )( b3Vec3 p1, b3Vec3 p2, b3HexColor color, void* context );
void ( *DrawSegmentFcn )( b3Pos p1, b3Pos p2, b3HexColor color, void* context );
/// Draw a transform. Choose your own length scale.
void ( *DrawTransformFcn )( b3Transform transform, void* context );
void ( *DrawTransformFcn )( b3WorldTransform transform, void* context );
/// Draw a point.
void ( *DrawPointFcn )( b3Vec3 p, float size, b3HexColor color, void* context );
void ( *DrawPointFcn )( b3Pos p, float size, b3HexColor color, void* context );
/// Draw a bounding box.
void ( *DrawBoundsFcn )( b3AABB aabb, b3HexColor color, void* context );
/// Draw a bounding box.
void ( *DrawBoxFcn )( b3Vec3 extents, b3Transform transform, b3HexColor color, void* context );
/// Draw an oriented box.
void ( *DrawBoxFcn )( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context );
/// Draw a string in world space
void ( *DrawStringFcn )( b3Vec3 p, const char* s, b3HexColor color, void* context );
void ( *DrawStringFcn )( b3Pos p, const char* s, b3HexColor color, void* context );
/// World bounds to use for debug draw
b3AABB drawingBounds;

View file

@ -118,14 +118,14 @@ if(BOX3D_BUILD_SHADERS)
unset(_shdc_exe)
endif()
# --- Renderer + shaders (ported verbatim from render3d). ---------------------
# --- Renderer + shaders
add_subdirectory(shaders)
add_subdirectory(gfx)
set(SAMPLE_FILES
main.cpp
# Host glue (ported from render3d's app/).
# Host
host/camera.cpp
host/camera.h
host/gui.cpp
@ -142,31 +142,31 @@ set(SAMPLE_FILES
earcut.h
# Samples
sample_stacking.cpp
sample_benchmark.cpp
sample_bodies.cpp
sample_character.cpp
sample_collision.cpp
sample_compound.cpp
sample_continuous.cpp
sample_determinism.cpp
sample_robustness.cpp
sample_benchmark.cpp
sample_ragdoll.cpp
sample_events.cpp
sample_geometry.cpp
sample_issues.cpp
sample_shapes.cpp
sample_tree.cpp
sample_mesh.cpp
sample_compound.cpp
sample_events.cpp
sample_bodies.cpp
sample_manifold.cpp
sample_continuous.cpp
sample_joint.cpp
sample_manifold.cpp
sample_mesh.cpp
sample_ragdoll.cpp
sample_robustness.cpp
sample_shapes.cpp
sample_stacking.cpp
sample_tree.cpp
sample_world.cpp
)
add_executable(samples ${SAMPLE_FILES})
if(APPLE)
# sokol_app's Cocoa backend is Objective-C; compile its impl TU as OBJC
# with ARC, matching render3d's app/CMakeLists.
# sokol_app's Cocoa backend is Objective-C. Compile its impl TU as OBJC with ARC.
set_source_files_properties(host/sokol_app_impl.c PROPERTIES LANGUAGE OBJC)
set_source_files_properties(host/sokol_app_impl.c PROPERTIES COMPILE_OPTIONS "-fobjc-arc")
endif()

View file

@ -24,7 +24,6 @@ set(GFX_FILES
ibl.h
overlay.c
overlay.h
picking.h
projection.h
qsort.h
renderer.c
@ -52,8 +51,7 @@ if(APPLE)
target_compile_options(gfx PRIVATE -fobjc-arc)
endif()
# Include root is the samples dir so "gfx/foo.h" / "host/foo.h" prefixes
# resolve (render3d used its repo root; here gfx lives under samples/).
# Include root is the samples dir so "gfx/foo.h" / "host/foo.h" prefixes resolve.
target_include_directories(gfx PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/..")
target_link_libraries(gfx
PUBLIC sokol box3d

View file

@ -652,9 +652,8 @@ static void DestroyDebugShape( void* userShape, void* context )
FreeDebugShape( index );
}
// World-space cell size for the procedural ground grid (meters per minor
// cell, major lines fire every 10 cells inside the shader).
#define BOX3D_GROUND_GRID_CELL_SIZE 1.0f
// BOX3D_GROUND_GRID_CELL_SIZE lives in the header so the host can wrap the draw
// origin to the same grid period.
// Alpha applied to non-static shapes when box3dAdapterSetTransparentDynamic is on.
#define BOX3D_TRANSPARENT_DYNAMIC_ALPHA 0.5f
@ -696,7 +695,7 @@ static void AppendResolvedShape( const DebugShape* us, b3Transform baseTransform
}
}
static bool DrawShape( void* userShape, b3Transform shapeTransform, b3HexColor color, void* context )
static bool DrawShape( void* userShape, b3WorldTransform shapeTransform, b3HexColor color, void* context )
{
(void)context;
if ( userShape == NULL )
@ -704,6 +703,9 @@ static bool DrawShape( void* userShape, b3Transform shapeTransform, b3HexColor c
return true; // unsupported shape type, skip and continue
}
// Shift the world transform into the camera relative frame the primitives render in
b3Transform shapeRelative = b3ToRelativeTransform( shapeTransform, GetDrawOrigin() );
DebugShape* us = (DebugShape*)userShape;
RefreshMaterialFromOverride( us );
@ -752,13 +754,13 @@ static bool DrawShape( void* userShape, b3Transform shapeTransform, b3HexColor c
{
for ( int ci = us->compound.firstChild; ci != -1; ci = s_adapter.pool[ci].nextChild )
{
b3Transform base = b3MulTransforms( shapeTransform, s_adapter.pool[ci].childTransform );
b3Transform base = b3MulTransforms( shapeRelative, s_adapter.pool[ci].childTransform );
AppendResolvedShape( &s_adapter.pool[ci], base, c, metallic, roughness, shadowCast, hk );
}
}
else
{
AppendResolvedShape( us, shapeTransform, c, metallic, roughness, shadowCast, hk );
AppendResolvedShape( us, shapeRelative, c, metallic, roughness, shadowCast, hk );
}
return true;
@ -767,18 +769,20 @@ static bool DrawShape( void* userShape, b3Transform shapeTransform, b3HexColor c
#define BOX3D_LINE_THICKNESS_PX 2.5f
#define BOX3D_TRANSFORM_LENGTH (0.25f * b3GetLengthUnitsPerMeter())
static void DrawSegmentFcn( b3Vec3 p1, b3Vec3 p2, b3HexColor color, void* context )
static void DrawSegmentFcn( b3Pos p1, b3Pos p2, b3HexColor color, void* context )
{
(void)context;
OverlayAppendLine( p1, p2, HexColorToVec4( color ), BOX3D_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS,
OVERLAY_OCCLUSION_HIDE );
b3Pos o = GetDrawOrigin();
OverlayAppendLine( b3SubPos( p1, o ), b3SubPos( p2, o ), HexColorToVec4( color ), BOX3D_LINE_THICKNESS_PX,
OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
}
static void DrawTransformFcn( b3Transform transform, void* context )
static void DrawTransformFcn( b3WorldTransform transform, void* context )
{
(void)context;
b3Vec3 origin = transform.p;
b3Matrix3 basis = b3MakeMatrixFromQuat( transform.q );
b3Transform local = b3ToRelativeTransform( transform, GetDrawOrigin() );
b3Vec3 origin = local.p;
b3Matrix3 basis = b3MakeMatrixFromQuat( local.q );
float L = BOX3D_TRANSFORM_LENGTH;
Vec4 red = MakeVec4( 1.0f, 0.0f, 0.0f, 1.0f );
Vec4 green = MakeVec4( 0.0f, 1.0f, 0.0f, 1.0f );
@ -791,18 +795,21 @@ static void DrawTransformFcn( b3Transform transform, void* context )
OVERLAY_OCCLUSION_HIDE );
}
static void DrawPointFcn( b3Vec3 p, float size, b3HexColor color, void* context )
static void DrawPointFcn( b3Pos p, float size, b3HexColor color, void* context )
{
(void)context;
OverlayAppendPoint( p, HexColorToVec4( color ), size, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
OverlayAppendPoint( b3SubPos( p, GetDrawOrigin() ), HexColorToVec4( color ), size, OVERLAY_THICKNESS_PIXELS,
OVERLAY_OCCLUSION_HIDE );
}
static void DrawBoundsFcn( b3AABB aabb, b3HexColor color, void* context )
{
(void)context;
b3Vec3 lower = aabb.lowerBound;
b3Vec3 upper = aabb.upperBound;
// The fat AABB is float world space, difference against the origin in double before the corners demote
b3Pos origin = GetDrawOrigin();
b3Vec3 lower = b3SubPos( b3ToPos( aabb.lowerBound ), origin );
b3Vec3 upper = b3SubPos( b3ToPos( aabb.upperBound ), origin );
Vec4 c = HexColorToVec4( color );
b3Vec3 c000 = { lower.x, lower.y, lower.z };
@ -839,11 +846,14 @@ static void DrawBoundsFcn( b3AABB aabb, b3HexColor color, void* context )
// Oriented box: 8 corners are transform * (+/-ex, +/-ey, +/-ez). 12 edges
// connect adjacent corners along each axis.
static void DrawBoxFcn( b3Vec3 extents, b3Transform transform, b3HexColor color, void* context )
static void DrawBoxFcn( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context )
{
(void)context;
Vec4 c = HexColorToVec4( color );
// Shift into the camera relative frame, then transform the local corners
b3Transform local = b3ToRelativeTransform( transform, GetDrawOrigin() );
// Local-space corners (8 = 2^3 sign combinations along each axis).
float signs[2] = { -1.0f, 1.0f };
b3Vec3 corners[8];
@ -856,7 +866,7 @@ static void DrawBoxFcn( b3Vec3 extents, b3Transform transform, b3HexColor color,
float lx = signs[xi] * extents.x;
float ly = signs[yi] * extents.y;
float lz = signs[zi] * extents.z;
corners[( xi << 2 ) | ( yi << 1 ) | zi] = b3TransformPoint( transform, (b3Vec3){ lx, ly, lz } );
corners[( xi << 2 ) | ( yi << 1 ) | zi] = b3TransformPoint( local, (b3Vec3){ lx, ly, lz } );
}
}
}
@ -879,14 +889,14 @@ static void DrawBoxFcn( b3Vec3 extents, b3Transform transform, b3HexColor color,
}
}
static void DrawStringFcn( b3Vec3 p, const char* s, b3HexColor color, void* context )
static void DrawStringFcn( b3Pos p, const char* s, b3HexColor color, void* context )
{
(void)context;
if ( s == NULL )
{
return;
}
DrawString( p, HexColorToVec4( color ), s );
DrawString( b3SubPos( p, GetDrawOrigin() ), HexColorToVec4( color ), s );
}
void AttachToWorldDef( b3WorldDef* def )

View file

@ -11,6 +11,11 @@ extern "C"
{
#endif
// World-space minor-cell size of the procedural ground grid (meters). Major
// lines fire every 10 cells inside the shader. Shared so the host can wrap the
// draw origin to the grid period and keep the grid crisp far from the origin.
#define BOX3D_GROUND_GRID_CELL_SIZE 1.0f
void InitAdapter( void );
// Unconditionally flush the debug-shape pool: release the GPU mesh references

View file

@ -10,65 +10,96 @@
#include <stdarg.h>
#include <stdio.h>
void DrawCubeEx( b3Transform transform, b3Vec3 scale, Vec4 baseColor, float metallic, float roughness,
TransparentShadowCast shadowCast )
// World position every draw demotes against. The host points it at the camera eye each
// frame, so the shift happens in double far from the origin and the overlay and impostor
// primitives only ever see small coordinates. Zero by default, which makes the demotion an
// identity and matches absolute coordinates.
static b3Pos s_drawOrigin;
// Shift a world position into the draw-origin-relative frame the primitives render in.
static inline b3Vec3 ToRelative( b3Pos worldPoint )
{
AppendCube( transform, scale, baseColor, metallic, roughness, shadowCast );
return b3SubPos( worldPoint, s_drawOrigin );
}
void DrawCube( b3Transform transform, b3Vec3 scale, Vec4 baseColor )
// Shift a world transform into the relative frame. Rotation rides through unchanged.
static inline b3Transform ToRelativeFrame( b3WorldTransform transform )
{
return b3ToRelativeTransform( transform, s_drawOrigin );
}
void SetDrawOrigin( b3Pos origin )
{
s_drawOrigin = origin;
}
b3Pos GetDrawOrigin( void )
{
return s_drawOrigin;
}
void DrawCubeEx( b3WorldTransform transform, b3Vec3 scale, Vec4 baseColor, float metallic, float roughness,
TransparentShadowCast shadowCast )
{
AppendCube( ToRelativeFrame( transform ), scale, baseColor, metallic, roughness, shadowCast );
}
void DrawCube( b3WorldTransform transform, b3Vec3 scale, Vec4 baseColor )
{
DrawCubeEx( transform, scale, baseColor, DEFAULT_METALLIC, DEFAULT_ROUGHNESS, TRANSPARENT_SHADOW_FULL );
}
void DrawSphereEx( b3Transform transform, float radius, Vec4 baseColor, float metallic, float roughness,
void DrawSphereEx( b3WorldTransform transform, float radius, Vec4 baseColor, float metallic, float roughness,
TransparentShadowCast shadowCast )
{
AppendSphere( transform, radius, baseColor, metallic, roughness, shadowCast );
AppendSphere( ToRelativeFrame( transform ), radius, baseColor, metallic, roughness, shadowCast );
}
void DrawSphere( b3Transform transform, float radius, Vec4 baseColor )
void DrawSphere( b3WorldTransform transform, float radius, Vec4 baseColor )
{
DrawSphereEx( transform, radius, baseColor, DEFAULT_METALLIC, DEFAULT_ROUGHNESS, TRANSPARENT_SHADOW_FULL );
}
void DrawCapsuleEx( b3Transform transform, float halfLength, float radius, Vec4 baseColor, float metallic, float roughness,
void DrawCapsuleEx( b3WorldTransform transform, float halfLength, float radius, Vec4 baseColor, float metallic, float roughness,
TransparentShadowCast shadowCast )
{
AppendCapsule( transform, halfLength, radius, baseColor, metallic, roughness, shadowCast );
AppendCapsule( ToRelativeFrame( transform ), halfLength, radius, baseColor, metallic, roughness, shadowCast );
}
void DrawCapsule( b3Transform transform, float halfLength, float radius, Vec4 baseColor )
void DrawCapsule( b3WorldTransform transform, float halfLength, float radius, Vec4 baseColor )
{
DrawCapsuleEx( transform, halfLength, radius, baseColor, DEFAULT_METALLIC, DEFAULT_ROUGHNESS, TRANSPARENT_SHADOW_FULL );
}
void DrawSolidSphere( b3Transform transform, b3Sphere sphere, Vec4 color )
void DrawSolidSphere( b3WorldTransform transform, b3Sphere sphere, Vec4 color )
{
b3Transform world = { b3TransformPoint( transform, sphere.center ), transform.q };
DrawSphere( world, sphere.radius, color );
b3Transform rel = ToRelativeFrame( transform );
b3Transform world = { b3TransformPoint( rel, sphere.center ), rel.q };
AppendSphere( world, sphere.radius, color, DEFAULT_METALLIC, DEFAULT_ROUGHNESS, TRANSPARENT_SHADOW_FULL );
}
void DrawSolidCapsule( b3Transform transform, b3Capsule capsule, Vec4 color )
void DrawSolidCapsule( b3WorldTransform transform, b3Capsule capsule, Vec4 color )
{
b3Vec3 c1 = b3TransformPoint( transform, capsule.center1 );
b3Vec3 c2 = b3TransformPoint( transform, capsule.center2 );
b3Transform rel = ToRelativeFrame( transform );
b3Vec3 c1 = b3TransformPoint( rel, capsule.center1 );
b3Vec3 c2 = b3TransformPoint( rel, capsule.center2 );
b3Vec3 axis = b3Sub( c2, c1 );
float length = b3Length( axis );
// Impostor cylinder runs along local +X, so align +X with the capsule axis.
b3Quat q = transform.q;
b3Quat q = rel.q;
if ( length > 1e-6f )
{
q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, b3MulSV( 1.0f / length, axis ) );
}
b3Transform world = { b3MulSV( 0.5f, b3Add( c1, c2 ) ), q };
DrawCapsule( world, 0.5f * length, capsule.radius, color );
AppendCapsule( world, 0.5f * length, capsule.radius, color, DEFAULT_METALLIC, DEFAULT_ROUGHNESS, TRANSPARENT_SHADOW_FULL );
}
void DrawHull( b3Transform transform, const b3HullData* hull, Vec4 color )
void DrawHull( b3WorldTransform transform, const b3HullData* hull, Vec4 color )
{
b3Transform rel = ToRelativeFrame( transform );
const b3Vec3* points = b3GetHullPoints( hull );
const b3HullHalfEdge* edges = b3GetHullEdges( hull );
@ -79,55 +110,59 @@ void DrawHull( b3Transform transform, const b3HullData* hull, Vec4 color )
{
continue;
}
b3Vec3 p1 = b3TransformPoint( transform, points[edges[i].origin] );
b3Vec3 p2 = b3TransformPoint( transform, points[edges[edges[i].twin].origin] );
DrawLine( p1, p2, color );
b3Vec3 p1 = b3TransformPoint( rel, points[edges[i].origin] );
b3Vec3 p2 = b3TransformPoint( rel, points[edges[edges[i].twin].origin] );
OverlayAppendLine( p1, p2, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
}
}
void DrawPlane( b3Vec3 normal, b3Vec3 point, Vec4 color )
void DrawPlane( b3Vec3 normal, b3Pos point, Vec4 color )
{
b3Vec3 c = ToRelative( point );
b3Vec3 perp1 = b3Perp( normal );
b3Vec3 perp2 = b3Cross( perp1, normal );
b3Vec3 p1 = b3Add( point, b3Add( perp1, perp2 ) );
b3Vec3 p2 = b3Add( point, b3Sub( perp2, perp1 ) );
b3Vec3 p3 = b3Sub( point, b3Add( perp1, perp2 ) );
b3Vec3 p4 = b3Add( point, b3Sub( perp1, perp2 ) );
DrawLine( p1, p2, color );
DrawLine( p2, p3, color );
DrawLine( p3, p4, color );
DrawLine( p4, p1, color );
DrawLine( point, b3Add( point, b3MulSV( 0.5f, normal ) ), color );
DrawPoint( point, 10.0f, color );
b3Vec3 p1 = b3Add( c, b3Add( perp1, perp2 ) );
b3Vec3 p2 = b3Add( c, b3Sub( perp2, perp1 ) );
b3Vec3 p3 = b3Sub( c, b3Add( perp1, perp2 ) );
b3Vec3 p4 = b3Add( c, b3Sub( perp1, perp2 ) );
float th = DEFAULT_LINE_THICKNESS_PX;
OverlayAppendLine( p1, p2, color, th, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
OverlayAppendLine( p2, p3, color, th, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
OverlayAppendLine( p3, p4, color, th, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
OverlayAppendLine( p4, p1, color, th, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
OverlayAppendLine( c, b3Add( c, b3MulSV( 0.5f, normal ) ), color, th, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
OverlayAppendPoint( c, color, 10.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM );
}
void DrawLineEx( b3Vec3 a, b3Vec3 b, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
void DrawLineEx( b3Pos a, b3Pos b, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode )
{
OverlayAppendLine( a, b, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( ToRelative( a ), ToRelative( b ), color, thickness, thicknessUnit, occlusionMode );
}
void DrawLine( b3Vec3 a, b3Vec3 b, Vec4 color )
void DrawLine( b3Pos a, b3Pos b, Vec4 color )
{
DrawLineEx( a, b, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
}
void DrawPointEx( b3Vec3 p, Vec4 color, float size, OverlayThicknessUnit sizeUnit, OverlayOcclusionMode occlusionMode )
void DrawPointEx( b3Pos p, Vec4 color, float size, OverlayThicknessUnit sizeUnit, OverlayOcclusionMode occlusionMode )
{
OverlayAppendPoint( p, color, size, sizeUnit, occlusionMode );
OverlayAppendPoint( ToRelative( p ), color, size, sizeUnit, occlusionMode );
}
void DrawPoint( b3Vec3 p, float size, Vec4 color )
void DrawPoint( b3Pos p, float size, Vec4 color )
{
DrawPointEx( p, color, size, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM );
}
void DrawArrowEx( b3Vec3 a, b3Vec3 b, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
void DrawArrowEx( b3Pos a, b3Pos b, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode, float headLengthFrac )
{
DrawLineEx( a, b, color, thickness, thicknessUnit, occlusionMode );
b3Vec3 tail = ToRelative( a );
b3Vec3 tip = ToRelative( b );
OverlayAppendLine( tail, tip, color, thickness, thicknessUnit, occlusionMode );
b3Vec3 shaft = b3Sub( b, a );
b3Vec3 shaft = b3Sub( tip, tail );
float shaftLen = b3Length( shaft );
if ( shaftLen < 1e-6f )
{
@ -139,31 +174,32 @@ void DrawArrowEx( b3Vec3 a, b3Vec3 b, Vec4 color, float thickness, OverlayThickn
b3Vec3 backFromTip = b3MulSV( -headLen, dir );
b3Vec3 sideStep = b3MulSV( headLen * 0.5f, perp );
b3Vec3 tip1 = b3Add( b, b3Add( backFromTip, sideStep ) );
b3Vec3 tip2 = b3Add( b, b3Sub( backFromTip, sideStep ) );
DrawLineEx( b, tip1, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( b, tip2, color, thickness, thicknessUnit, occlusionMode );
b3Vec3 tip1 = b3Add( tip, b3Add( backFromTip, sideStep ) );
b3Vec3 tip2 = b3Add( tip, b3Sub( backFromTip, sideStep ) );
OverlayAppendLine( tip, tip1, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( tip, tip2, color, thickness, thicknessUnit, occlusionMode );
}
void DrawArrow( b3Vec3 a, b3Vec3 b, Vec4 color )
void DrawArrow( b3Pos a, b3Pos b, Vec4 color )
{
DrawArrowEx( a, b, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE,
DEFAULT_ARROW_HEAD_FRAC );
}
void DrawCrossEx( b3Vec3 center, float size, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
void DrawCrossEx( b3Pos center, float size, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode )
{
b3Vec3 c = ToRelative( center );
float h = size * 0.5f;
DrawLineEx( (b3Vec3){ center.x - h, center.y, center.z }, (b3Vec3){ center.x + h, center.y, center.z }, color, thickness,
thicknessUnit, occlusionMode );
DrawLineEx( (b3Vec3){ center.x, center.y - h, center.z }, (b3Vec3){ center.x, center.y + h, center.z }, color, thickness,
thicknessUnit, occlusionMode );
DrawLineEx( (b3Vec3){ center.x, center.y, center.z - h }, (b3Vec3){ center.x, center.y, center.z + h }, color, thickness,
thicknessUnit, occlusionMode );
OverlayAppendLine( (b3Vec3){ c.x - h, c.y, c.z }, (b3Vec3){ c.x + h, c.y, c.z }, color, thickness, thicknessUnit,
occlusionMode );
OverlayAppendLine( (b3Vec3){ c.x, c.y - h, c.z }, (b3Vec3){ c.x, c.y + h, c.z }, color, thickness, thicknessUnit,
occlusionMode );
OverlayAppendLine( (b3Vec3){ c.x, c.y, c.z - h }, (b3Vec3){ c.x, c.y, c.z + h }, color, thickness, thicknessUnit,
occlusionMode );
}
void DrawCross( b3Vec3 center, float size, Vec4 color )
void DrawCross( b3Pos center, float size, Vec4 color )
{
DrawCrossEx( center, size, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
}
@ -171,31 +207,32 @@ void DrawCross( b3Vec3 center, float size, Vec4 color )
void DrawAabbEx( b3Vec3 mn, b3Vec3 mx, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode )
{
// 8 corners of the box.
b3Vec3 c000 = { mn.x, mn.y, mn.z };
b3Vec3 c100 = { mx.x, mn.y, mn.z };
b3Vec3 c010 = { mn.x, mx.y, mn.z };
b3Vec3 c110 = { mx.x, mx.y, mn.z };
b3Vec3 c001 = { mn.x, mn.y, mx.z };
b3Vec3 c101 = { mx.x, mn.y, mx.z };
b3Vec3 c011 = { mn.x, mx.y, mx.z };
b3Vec3 c111 = { mx.x, mx.y, mx.z };
// The box is float world space, so promote each corner and difference against the origin in
// double before it demotes. Far from the origin a plain narrow could clip a corner.
b3Vec3 c000 = ToRelative( b3ToPos( (b3Vec3){ mn.x, mn.y, mn.z } ) );
b3Vec3 c100 = ToRelative( b3ToPos( (b3Vec3){ mx.x, mn.y, mn.z } ) );
b3Vec3 c010 = ToRelative( b3ToPos( (b3Vec3){ mn.x, mx.y, mn.z } ) );
b3Vec3 c110 = ToRelative( b3ToPos( (b3Vec3){ mx.x, mx.y, mn.z } ) );
b3Vec3 c001 = ToRelative( b3ToPos( (b3Vec3){ mn.x, mn.y, mx.z } ) );
b3Vec3 c101 = ToRelative( b3ToPos( (b3Vec3){ mx.x, mn.y, mx.z } ) );
b3Vec3 c011 = ToRelative( b3ToPos( (b3Vec3){ mn.x, mx.y, mx.z } ) );
b3Vec3 c111 = ToRelative( b3ToPos( (b3Vec3){ mx.x, mx.y, mx.z } ) );
// 4 bottom edges (y = mn.y).
DrawLineEx( c000, c100, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( c100, c101, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( c101, c001, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( c001, c000, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c000, c100, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c100, c101, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c101, c001, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c001, c000, color, thickness, thicknessUnit, occlusionMode );
// 4 top edges (y = mx.y).
DrawLineEx( c010, c110, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( c110, c111, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( c111, c011, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( c011, c010, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c010, c110, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c110, c111, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c111, c011, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c011, c010, color, thickness, thicknessUnit, occlusionMode );
// 4 vertical edges.
DrawLineEx( c000, c010, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( c100, c110, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( c101, c111, color, thickness, thicknessUnit, occlusionMode );
DrawLineEx( c001, c011, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c000, c010, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c100, c110, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c101, c111, color, thickness, thicknessUnit, occlusionMode );
OverlayAppendLine( c001, c011, color, thickness, thicknessUnit, occlusionMode );
}
void DrawAabb( b3Vec3 mn, b3Vec3 mx, Vec4 color )
@ -209,30 +246,33 @@ void DrawBounds( b3AABB bounds, float extension, Vec4 color )
DrawAabb( b3Sub( bounds.lowerBound, e ), b3Add( bounds.upperBound, e ), color );
}
void DrawAxesEx( b3Transform transform, float size, float thickness, OverlayThicknessUnit thicknessUnit,
void DrawAxesEx( b3WorldTransform transform, float size, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode )
{
b3Vec3 origin = transform.p;
b3Matrix3 basis = b3MakeMatrixFromQuat( transform.q );
DrawLineEx( origin, b3MulAdd( origin, size, basis.cx ), MakeVec4( 1.0f, 0.0f, 0.0f, 1.0f ), thickness, thicknessUnit,
occlusionMode );
DrawLineEx( origin, b3MulAdd( origin, size, basis.cy ), MakeVec4( 0.0f, 1.0f, 0.0f, 1.0f ), thickness, thicknessUnit,
occlusionMode );
DrawLineEx( origin, b3MulAdd( origin, size, basis.cz ), MakeVec4( 0.0f, 0.0f, 1.0f, 1.0f ), thickness, thicknessUnit,
occlusionMode );
b3Transform rel = ToRelativeFrame( transform );
b3Vec3 origin = rel.p;
b3Matrix3 basis = b3MakeMatrixFromQuat( rel.q );
OverlayAppendLine( origin, b3MulAdd( origin, size, basis.cx ), MakeVec4( 1.0f, 0.0f, 0.0f, 1.0f ), thickness, thicknessUnit,
occlusionMode );
OverlayAppendLine( origin, b3MulAdd( origin, size, basis.cy ), MakeVec4( 0.0f, 1.0f, 0.0f, 1.0f ), thickness, thicknessUnit,
occlusionMode );
OverlayAppendLine( origin, b3MulAdd( origin, size, basis.cz ), MakeVec4( 0.0f, 0.0f, 1.0f, 1.0f ), thickness, thicknessUnit,
occlusionMode );
}
void DrawAxes( b3Transform transform, float size )
void DrawAxes( b3WorldTransform transform, float size )
{
DrawAxesEx( transform, size, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
}
void DrawGrid( b3Vec3 center, b3Vec3 normal, float halfExtent, int divisions, Vec4 color )
void DrawGrid( b3Pos center, b3Vec3 normal, float halfExtent, int divisions, Vec4 color )
{
if ( divisions < 1 || halfExtent <= 0.0f )
{
return;
}
b3Vec3 c = ToRelative( center );
// Orthonormal in-plane axes from the normal.
b3Vec3 n = b3Normalize( normal );
b3Vec3 u = b3Normalize( b3Perp( n ) );
@ -243,29 +283,36 @@ void DrawGrid( b3Vec3 center, b3Vec3 normal, float halfExtent, int divisions, Ve
{
const float o = -halfExtent + (float)i * step;
// Line spanning u at this offset along v.
b3Vec3 ua = b3Add( center, b3Add( b3MulSV( -halfExtent, u ), b3MulSV( o, v ) ) );
b3Vec3 ub = b3Add( center, b3Add( b3MulSV( halfExtent, u ), b3MulSV( o, v ) ) );
DrawLineEx( ua, ub, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
b3Vec3 ua = b3Add( c, b3Add( b3MulSV( -halfExtent, u ), b3MulSV( o, v ) ) );
b3Vec3 ub = b3Add( c, b3Add( b3MulSV( halfExtent, u ), b3MulSV( o, v ) ) );
OverlayAppendLine( ua, ub, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
// Line spanning v at this offset along u.
b3Vec3 va = b3Add( center, b3Add( b3MulSV( o, u ), b3MulSV( -halfExtent, v ) ) );
b3Vec3 vb = b3Add( center, b3Add( b3MulSV( o, u ), b3MulSV( halfExtent, v ) ) );
DrawLineEx( va, vb, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
b3Vec3 va = b3Add( c, b3Add( b3MulSV( o, u ), b3MulSV( -halfExtent, v ) ) );
b3Vec3 vb = b3Add( c, b3Add( b3MulSV( o, u ), b3MulSV( halfExtent, v ) ) );
OverlayAppendLine( va, vb, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
}
}
void DrawGroundGrid( int size )
{
Vec4 color = MakeVec4( 0.3f, 0.3f, 0.3f, 1.0f );
DrawGrid( b3Vec3_zero, b3Vec3_axisY, (float)size, size, color );
DrawGrid( b3Pos_zero, b3Vec3_axisY, (float)size, size, color );
}
void DrawTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, Vec4 color )
void DrawTriangle( b3WorldTransform transform, b3Vec3 a, b3Vec3 b, b3Vec3 c, Vec4 color )
{
DrawLineEx( a, b, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
DrawLineEx( b, c, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
DrawLineEx( c, a, color, DEFAULT_LINE_THICKNESS_PX, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
b3Transform rel = ToRelativeFrame( transform );
b3Vec3 ra = b3TransformPoint( rel, a );
b3Vec3 rb = b3TransformPoint( rel, b );
b3Vec3 rc = b3TransformPoint( rel, c );
float th = DEFAULT_LINE_THICKNESS_PX;
OverlayAppendLine( ra, rb, color, th, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
OverlayAppendLine( rb, rc, color, th, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
OverlayAppendLine( rc, ra, color, th, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_HIDE );
}
// Disc of `segments` lines in the plane through `center` (relative frame) with the given normal.
static void DrawDisc( b3Vec3 center, b3Vec3 normal, float radius, int segments, Vec4 color )
{
b3Vec3 tangent1 = b3Perp( normal );
@ -284,7 +331,7 @@ static void DrawDisc( b3Vec3 center, b3Vec3 normal, float radius, int segments,
float y2 = sine * x1 + cosine * y1;
b3Vec3 vertex2 = b3Add( center, b3Blend2( x2, tangent1, y2, tangent2 ) );
DrawLineEx( vertex1, vertex2, color, 2.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM );
OverlayAppendLine( vertex1, vertex2, color, 2.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DIM );
x1 = x2;
y1 = y2;
@ -292,6 +339,7 @@ static void DrawDisc( b3Vec3 center, b3Vec3 normal, float radius, int segments,
}
}
// Arc swept `maxDegrees` from `start` about `normal`, in the relative frame.
static void DrawArc( b3Vec3 center, b3Vec3 normal, float radius, b3Vec3 start, float maxDegrees, int segments, Vec4 color )
{
b3Vec3 tangent1 = b3Normalize( start );
@ -309,7 +357,7 @@ static void DrawArc( b3Vec3 center, b3Vec3 normal, float radius, b3Vec3 start, f
float y2 = cs.sine * x1 + cs.cosine * y1;
b3Vec3 vertex2 = b3Add( center, b3Blend2( x2, tangent1, y2, tangent2 ) );
DrawLineEx( vertex1, vertex2, color, 2.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
OverlayAppendLine( vertex1, vertex2, color, 2.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
x1 = x2;
y1 = y2;
@ -317,38 +365,40 @@ static void DrawArc( b3Vec3 center, b3Vec3 normal, float radius, b3Vec3 start, f
}
}
void DrawWireSphere( b3Transform transform, const b3Sphere* sphere, int segments, Vec4 color )
void DrawWireSphere( b3WorldTransform transform, const b3Sphere* sphere, int segments, Vec4 color )
{
b3Vec3 center = b3TransformPoint( transform, sphere->center );
b3Transform rel = ToRelativeFrame( transform );
b3Vec3 center = b3TransformPoint( rel, sphere->center );
float radius = sphere->radius;
b3Vec3 axisX = b3RotateVector( transform.q, (b3Vec3){ 1.0f, 0.0f, 0.0f } );
b3Vec3 axisY = b3RotateVector( transform.q, (b3Vec3){ 0.0f, 1.0f, 0.0f } );
b3Vec3 axisZ = b3RotateVector( transform.q, (b3Vec3){ 0.0f, 0.0f, 1.0f } );
b3Vec3 axisX = b3RotateVector( rel.q, (b3Vec3){ 1.0f, 0.0f, 0.0f } );
b3Vec3 axisY = b3RotateVector( rel.q, (b3Vec3){ 0.0f, 1.0f, 0.0f } );
b3Vec3 axisZ = b3RotateVector( rel.q, (b3Vec3){ 0.0f, 0.0f, 1.0f } );
DrawDisc( center, axisX, radius, segments, color );
DrawDisc( center, axisY, radius, segments, color );
DrawDisc( center, axisZ, radius, segments, color );
}
void DrawWireCapsule( b3Transform transform, const b3Capsule* capsule, int segments, Vec4 color )
void DrawWireCapsule( b3WorldTransform transform, const b3Capsule* capsule, int segments, Vec4 color )
{
b3Vec3 center1 = b3TransformPoint( transform, capsule->center1 );
b3Vec3 center2 = b3TransformPoint( transform, capsule->center2 );
b3Transform rel = ToRelativeFrame( transform );
b3Vec3 center1 = b3TransformPoint( rel, capsule->center1 );
b3Vec3 center2 = b3TransformPoint( rel, capsule->center2 );
float radius = capsule->radius;
b3Vec3 normal = b3Normalize( b3Sub( center2, center1 ) );
b3Vec3 tangent1 = b3Perp( normal );
b3Vec3 tangent2 = b3Cross( normal, tangent1 );
DrawLineEx( b3MulAdd( center1, radius, tangent1 ), b3MulAdd( center2, radius, tangent1 ), color, 2.0f,
OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
DrawLineEx( b3MulAdd( center1, radius, tangent2 ), b3MulAdd( center2, radius, tangent2 ), color, 2.0f,
OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
DrawLineEx( b3MulSub( center1, radius, tangent1 ), b3MulSub( center2, radius, tangent1 ), color, 2.0f,
OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
DrawLineEx( b3MulSub( center1, radius, tangent2 ), b3MulSub( center2, radius, tangent2 ), color, 2.0f,
OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
OverlayAppendLine( b3MulAdd( center1, radius, tangent1 ), b3MulAdd( center2, radius, tangent1 ), color, 2.0f,
OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
OverlayAppendLine( b3MulAdd( center1, radius, tangent2 ), b3MulAdd( center2, radius, tangent2 ), color, 2.0f,
OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
OverlayAppendLine( b3MulSub( center1, radius, tangent1 ), b3MulSub( center2, radius, tangent1 ), color, 2.0f,
OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
OverlayAppendLine( b3MulSub( center1, radius, tangent2 ), b3MulSub( center2, radius, tangent2 ), color, 2.0f,
OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED );
DrawArc( center1, b3Neg( tangent1 ), radius, tangent2, 180.0f, segments / 2, color );
DrawArc( center1, tangent2, radius, tangent1, 180.0f, segments / 2, color );
@ -359,12 +409,12 @@ void DrawWireCapsule( b3Transform transform, const b3Capsule* capsule, int segme
DrawDisc( center2, normal, radius, segments, color );
}
void DrawWorldString( b3Vec3 point, Vec4 color, const char* format, ... )
void DrawString3D( b3Pos point, Vec4 color, const char* format, ... )
{
va_list args;
va_start( args, format );
char buffer[256];
vsnprintf( buffer, sizeof( buffer ), format, args );
va_end( args );
DrawString( point, color, buffer );
DrawString( ToRelative( point ), color, buffer );
}

View file

@ -24,16 +24,26 @@ extern "C"
{
#endif
// World space draw API. Every call takes absolute world coordinates and demotes against the
// draw origin (set to the camera focus each frame, SetDrawOrigin) before handing float
// coordinates to the renderer. The shift happens in double far from the origin, so the
// overlay and impostor primitives only ever see small coordinates. Identity in float mode
// where b3Pos aliases b3Vec3 and the origin is zero.
void SetDrawOrigin( b3Pos origin );
// The draw origin the host and engine debug callbacks demote against (the camera focus).
b3Pos GetDrawOrigin( void );
// Cube. transform places and orients the cube. scale is the cube's full side
// lengths (it multiplies the unit cube proxy, which spans [-0.5, 0.5]).
void DrawCube( b3Transform transform, b3Vec3 scale, Vec4 baseColor );
void DrawCubeEx( b3Transform transform, b3Vec3 scale, Vec4 baseColor, float metallic, float roughness,
void DrawCube( b3WorldTransform transform, b3Vec3 scale, Vec4 baseColor );
void DrawCubeEx( b3WorldTransform transform, b3Vec3 scale, Vec4 baseColor, float metallic, float roughness,
TransparentShadowCast shadowCast );
// Sphere impostor. transform supplies the orientation (rotation drives the
// surface pattern so spin is visible) and the center (translation).
void DrawSphere( b3Transform transform, float radius, Vec4 baseColor );
void DrawSphereEx( b3Transform transform, float radius, Vec4 baseColor, float metallic, float roughness,
void DrawSphere( b3WorldTransform transform, float radius, Vec4 baseColor );
void DrawSphereEx( b3WorldTransform transform, float radius, Vec4 baseColor, float metallic, float roughness,
TransparentShadowCast shadowCast );
// Capsule impostor. transform supplies the orientation (the capsule's local +X
@ -41,47 +51,48 @@ void DrawSphereEx( b3Transform transform, float radius, Vec4 baseColor, float me
// cylinder/cap surface pattern so roll is visible) and the center
// (translation). halfLength is the distance from center to either cap
// center along local +X. radius is the cap radius.
void DrawCapsule( b3Transform transform, float halfLength, float radius, Vec4 baseColor );
void DrawCapsuleEx( b3Transform transform, float halfLength, float radius, Vec4 baseColor, float metallic, float roughness,
void DrawCapsule( b3WorldTransform transform, float halfLength, float radius, Vec4 baseColor );
void DrawCapsuleEx( b3WorldTransform transform, float halfLength, float radius, Vec4 baseColor, float metallic, float roughness,
TransparentShadowCast shadowCast );
// Solid shape wrappers over the impostor primitives. They take Box3D shape
// types and fold the shape-local frame into the transform, so samples draw
// straight from collision data.
void DrawSolidSphere( b3Transform transform, b3Sphere sphere, Vec4 color );
void DrawSolidCapsule( b3Transform transform, b3Capsule capsule, Vec4 color );
void DrawSolidSphere( b3WorldTransform transform, b3Sphere sphere, Vec4 color );
void DrawSolidCapsule( b3WorldTransform transform, b3Capsule capsule, Vec4 color );
// Convex hull wireframe. Each undirected edge once, in world space.
void DrawHull( b3Transform transform, const b3HullData* hull, Vec4 color );
void DrawHull( b3WorldTransform transform, const b3HullData* hull, Vec4 color );
// Plane as a unit wireframe quad through the point, plus a short normal and a dot.
void DrawPlane( b3Vec3 normal, b3Vec3 point, Vec4 color );
void DrawPlane( b3Vec3 normal, b3Pos point, Vec4 color );
// All overlay submissions land in the renderer's per-frame overlay arenas
// and are drawn into the MSAA HDR scene target after opaque + sky, before
// the AgX tone map pass. Colors are straight (non-premultiplied) linear RGBA.
void DrawLine( b3Vec3 a, b3Vec3 b, Vec4 color );
void DrawLineEx( b3Vec3 a, b3Vec3 b, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
void DrawLine( b3Pos a, b3Pos b, Vec4 color );
void DrawLineEx( b3Pos a, b3Pos b, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode );
// Debug point, size in pixels. Dimmed rather than hidden when occluded so
// contact and witness points stay visible through geometry.
void DrawPoint( b3Vec3 p, float size, Vec4 color );
void DrawPointEx( b3Vec3 p, Vec4 color, float size, OverlayThicknessUnit sizeUnit, OverlayOcclusionMode occlusionMode );
void DrawPoint( b3Pos p, float size, Vec4 color );
void DrawPointEx( b3Pos p, Vec4 color, float size, OverlayThicknessUnit sizeUnit, OverlayOcclusionMode occlusionMode );
// Single-arrow shaft from a to b with two short angled head segments at b.
// headLengthFrac is the head length as a fraction of the shaft length
void DrawArrow( b3Vec3 a, b3Vec3 b, Vec4 color );
void DrawArrowEx( b3Vec3 a, b3Vec3 b, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
void DrawArrow( b3Pos a, b3Pos b, Vec4 color );
void DrawArrowEx( b3Pos a, b3Pos b, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode, float headLengthFrac );
// Three orthogonal lines through center, total length size per axis, uniform color.
void DrawCross( b3Vec3 center, float size, Vec4 color );
void DrawCrossEx( b3Vec3 center, float size, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
void DrawCross( b3Pos center, float size, Vec4 color );
void DrawCrossEx( b3Pos center, float size, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode );
// Wireframe AABB (12 edges) between world-space min and max corners.
// Wireframe AABB (12 edges) between world-space min and max corners. The AABB stays float
// world space, so the corners demote against the draw origin in double before they render.
void DrawAabb( b3Vec3 min, b3Vec3 max, Vec4 color );
void DrawAabbEx( b3Vec3 min, b3Vec3 max, Vec4 color, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode );
@ -90,27 +101,27 @@ void DrawAabbEx( b3Vec3 min, b3Vec3 max, Vec4 color, float thickness, OverlayThi
void DrawBounds( b3AABB bounds, float extension, Vec4 color );
// Draw RGB axes
void DrawAxes( b3Transform transform, float size );
void DrawAxesEx( b3Transform transform, float size, float thickness, OverlayThicknessUnit thicknessUnit,
void DrawAxes( b3WorldTransform transform, float size );
void DrawAxesEx( b3WorldTransform transform, float size, float thickness, OverlayThicknessUnit thicknessUnit,
OverlayOcclusionMode occlusionMode );
// Planar overlay grid centered at `center`, lying in the plane defined by
// `normal`. `halfExtent` is the half-width per in-plane axis, `divisions` the
// cell count per axis. Emits 2 * (divisions + 1) lines along an orthonormal
// basis derived from the normal, so it is not pinned to a Y-up ground plane.
void DrawGrid( b3Vec3 center, b3Vec3 normal, float halfExtent, int divisions, Vec4 color );
void DrawGrid( b3Pos center, b3Vec3 normal, float halfExtent, int divisions, Vec4 color );
// Gray ground grid in the XZ plane, `size` meters half-extent with 1 m cells.
void DrawGroundGrid( int size );
// Wireframe triangle: three overlay lines a -> b -> c -> a.
void DrawTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, Vec4 color );
void DrawTriangle( b3WorldTransform transform, b3Vec3 a, b3Vec3 b, b3Vec3 c, Vec4 color );
void DrawWireSphere( b3Transform transform, const b3Sphere* sphere, int segments, Vec4 color );
void DrawWireCapsule( b3Transform transform, const b3Capsule* capsule, int segments, Vec4 color );
void DrawWireSphere( b3WorldTransform transform, const b3Sphere* sphere, int segments, Vec4 color );
void DrawWireCapsule( b3WorldTransform transform, const b3Capsule* capsule, int segments, Vec4 color );
// printf-style world-space text label. The renderer projects with its latched camera.
void DrawWorldString( b3Vec3 point, Vec4 color, const char* format, ... );
// printf-style positioned text label. The renderer projects with its latched camera.
void DrawString3D( b3Pos point, Vec4 color, const char* format, ... );
#ifdef __cplusplus
} // extern "C"

View file

@ -10,7 +10,7 @@
#include <stdlib.h>
#include <string.h>
#define MAX_REGISTRY_ENTRIES 256
#define MAX_REGISTRY_ENTRIES 4096
#define MAX_GEOM_INSTANCES_GLOBAL 65536
#define INITIAL_PER_ENTRY_CAPACITY 8

View file

@ -1,11 +1,7 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
// Named key / modifier / mouse-button aliases over sokol_app's codes, so
// host Sample code can read input without including sokol_app.h directly.
// render3d's own code uses SAPP_* directly; this header is for the port-side
// host shell. The aliases are the codes actually exercised by the samples
// (character + joint movers, scene hotkeys, picking).
// todo get rid of this file
#pragma once

View file

@ -1,67 +0,0 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
// Pick-ray helper.
//
// Build a world-space ray from a screen-pixel coordinate using the latched
// camera state set each frame by RenderFrame / RenderFrameOffscreen.
// Pure-C / header-only so the demo app and the Box3D adapter share one
// implementation without dragging the rest of the renderer in.
//
// Conventions match the rest of the renderer:
// * Column-major matrices, multiplied as M * v
// * Reverse-Z clip space (near maps to clip-Z = +w, far to 0)
// * Mouse (x, y) in framebuffer pixels with origin at the top-left
// (Y grows downward, matches ImGui and sokol_app)
//
// Output matches Box3D's `b3World_CastRayClosest(origin, translation, ...)`
// convention: `*outOrigin` is the world point on the near plane along the
// pick ray, and `*outTranslation` is the unnormalized vector pointing from
// the near to the far plane along that ray. Pass them straight through.
//
// Returns false (and leaves outputs untouched) when the matrices are
// degenerate or the camera state hasn't been initialized (viewportW/H 0).
#pragma once
#include "gfx/renderer.h"
#include "gfx/utility.h"
#include <stdbool.h>
static inline bool BuildPickRay( float mouseX, float mouseY, b3Vec3* outOrigin, b3Vec3* outTranslation )
{
CameraState s = GetCameraState();
if ( s.viewportW <= 0 || s.viewportH <= 0 )
return false;
// Pixel -> NDC. Origin top-left, Y down. NDC has Y up, X right.
const float ndcX = ( 2.0f * mouseX / (float)s.viewportW ) - 1.0f;
const float ndcY = 1.0f - ( 2.0f * mouseY / (float)s.viewportH );
// Reverse-Z: near plane z = 1, far plane z = 0 in NDC. We pick those
// two clip-space points (w = 1) and run them through the inverse VP to
// recover world-space. inv(P*V) = inv(V)*inv(P) = viewInv * projInv,
// both produced by the camera alongside their forward matrices, no
// runtime matrix inversion.
const Mat4 invVP = MulMM4( s.viewInv, s.projInv );
const Vec4 clipN = MakeVec4( ndcX, ndcY, 1.0f, 1.0f );
const Vec4 clipF = MakeVec4( ndcX, ndcY, 0.0f, 1.0f );
const Vec4 wN = MulMV4( invVP, clipN );
const Vec4 wF = MulMV4( invVP, clipF );
if ( wN.w == 0.0f || wF.w == 0.0f )
return false;
const float invWN = 1.0f / wN.w;
const float invWF = 1.0f / wF.w;
b3Vec3 nearWorld = { wN.x * invWN, wN.y * invWN, wN.z * invWN };
b3Vec3 farWorld = { wF.x * invWF, wF.y * invWF, wF.z * invWF };
*outOrigin = nearWorld;
*outTranslation = b3Sub( farWorld, nearWorld );
return true;
}

View file

@ -1865,6 +1865,8 @@ static void DrawScene( int targetWidth, int targetHeight, const FrameInput* fram
gup.sun_color = sunColor;
gup.flags[0] = frame->debugMode;
gup.camera_pos = MakeVec4( frame->cameraPosition.x, frame->cameraPosition.y, frame->cameraPosition.z, 0.0f );
// .xy = origin wrapped to the grid period (grid lines), .zw = full origin (origin axes).
gup.grid_offset = MakeVec4( frame->gridWrap.x, frame->gridWrap.y, frame->drawOrigin.x, frame->drawOrigin.z );
gup.view = frame->view;
gup.cascade_far_view_z = cascadeFarViewZ;
for ( int i = 0; i < 3; ++i )
@ -2362,6 +2364,8 @@ static void DrawTransparentIntoResolve( int width, int height, const FrameInput*
geomPass.sun_color = sunColor;
geomPass.flags[0] = frame->debugMode;
geomPass.camera_pos = MakeVec4( frame->cameraPosition.x, frame->cameraPosition.y, frame->cameraPosition.z, 0.0f );
// .xy = origin wrapped to the grid period (grid lines), .zw = full origin (origin axes).
geomPass.grid_offset = MakeVec4( frame->gridWrap.x, frame->gridWrap.y, frame->drawOrigin.x, frame->drawOrigin.z );
geomPass.view = frame->view;
geomPass.cascade_far_view_z = cascadeFarViewZ;
for ( int i = 0; i < 3; ++i )

View file

@ -24,6 +24,19 @@ typedef struct FrameInput
Mat4 projInv;
b3Vec3 cameraPosition;
// World-space position of the relative render frame's origin (the draw
// origin the host shifts to the camera each frame). Shapes render relative
// to it, so the ground grid adds it back to recover absolute world XZ and
// stay locked to the world instead of sliding with the camera. Used full
// precision for the origin axes, which fade out far from the world origin.
b3Vec3 drawOrigin;
// Draw origin XZ wrapped to the grid period (10 cells) in double before it
// narrows. The grid pattern repeats over this period, so the wrapped offset
// reproduces the same lines while staying small enough for a float to
// resolve a 1 m cell out at planetary distance.
b3Vec2 gridWrap;
// seconds since InitRenderer
float time;

View file

@ -1,9 +1,8 @@
#include "camera.h"
#include "gfx/picking.h"
#include "sokol_app.h"
#include "box3d/math_functions.h"
#include "sokol_app.h"
#include <math.h>
@ -52,14 +51,15 @@ static void RebuildBasisAndView( Camera& c )
const b3Vec3 upUnnorm = b3Sub( worldUp, b3MulSV( b3Dot( worldUp, forward ), forward ) );
const b3Vec3 up = b3Normalize( upUnnorm );
const b3Vec3 right = b3Normalize( b3Cross( up, forward ) );
const b3Vec3 position = { c.m_pivot.x + forward.x * c.m_radius, c.m_pivot.y + forward.y * c.m_radius,
c.m_pivot.z + forward.z * c.m_radius };
c.m_position = position;
// Eye in world space is the draw origin. Building the view in that relative frame puts the eye
// at the origin, so the view matrix carries no translation and stays exact far from the origin.
c.m_worldEye = b3OffsetPos( c.m_pivot, b3MulSV( c.m_radius, forward ) );
c.m_position = b3Vec3_zero;
c.m_right = right;
c.m_up = up;
c.m_forward = forward;
MakeViewAndInverse( &c.m_view, &c.m_viewInv, right, up, forward, position );
MakeViewAndInverse( &c.m_view, &c.m_viewInv, right, up, forward, c.m_position );
}
Camera::Camera()
@ -75,6 +75,7 @@ Camera::Camera()
, m_height( 0 )
, m_thirdPerson( false )
, m_position{ 0.0f, 0.0f, 0.0f }
, m_worldEye{ 0.0f, 0.0f, 0.0f }
, m_right{ 1.0f, 0.0f, 0.0f }
, m_up{ 0.0f, 1.0f, 0.0f }
, m_forward{ 0.0f, 0.0f, 1.0f }
@ -103,7 +104,7 @@ void Camera::Frame( b3AABB aabb, float aspect, float padding )
b3Vec3 center = b3AABB_Center( aabb );
b3Vec3 ext = b3AABB_Extents( aabb ); // half-extents
float r = sqrtf( ext.x * ext.x + ext.y * ext.y + ext.z * ext.z );
m_pivot = center;
m_pivot = b3ToPos( center );
if ( r < 1.0e-6f )
{
// Point-like; preserve current radius, just retarget. The cached
@ -140,10 +141,8 @@ void Camera::SetOrbit( float yawRadians, float pitchRadians, float radius )
RebuildBasisAndView( *this );
}
void Camera::SetView( float yawDegrees, float pitchDegrees, float radius, b3Vec3 pivot )
void Camera::SetView( float yawDegrees, float pitchDegrees, float radius, b3Pos pivot )
{
// Box3D feeds degrees; render3d stores radians. Pivot first so SetOrbit's
// basis rebuild uses the new pivot.
SetPivot( pivot );
SetOrbit( yawDegrees * DEG_TO_RAD, pitchDegrees * DEG_TO_RAD, radius );
}
@ -160,14 +159,47 @@ b3Vec3 Camera::Position() const
PickRay Camera::BuildPickRay( float x, float y ) const
{
// Defined zero ray so callers never read uninitialized values when the
// camera has no size yet or the unprojection is degenerate.
PickRay ray;
if ( !::BuildPickRay( x, y, &ray.origin, &ray.translation ) )
ray.origin = b3Pos_zero;
ray.translation = b3Vec3_zero;
if ( m_width <= 0 || m_height <= 0 )
{
// Camera state not latched yet, or degenerate matrices: defined zero
// ray so callers never read uninitialized values.
ray.origin = m_position;
ray.translation = b3Vec3_zero;
return ray;
}
// Pixel -> NDC. Origin top-left, Y down. NDC has Y up, X right.
const float ndcX = ( 2.0f * x / (float)m_width ) - 1.0f;
const float ndcY = 1.0f - ( 2.0f * y / (float)m_height );
// Reverse-Z: near maps to clip-Z = +w, far to 0. Unproject both clip points
// through inv(P*V) = viewInv * projInv, no runtime matrix inversion. The
// matrices are eye-relative, so the recovered points are too.
const Mat4 invVP = MulMM4( m_viewInv, m_projInv );
const Vec4 clipN = MakeVec4( ndcX, ndcY, 1.0f, 1.0f );
const Vec4 clipF = MakeVec4( ndcX, ndcY, 0.0f, 1.0f );
const Vec4 wN = MulMV4( invVP, clipN );
const Vec4 wF = MulMV4( invVP, clipF );
if ( wN.w == 0.0f || wF.w == 0.0f )
{
return ray;
}
const float invWN = 1.0f / wN.w;
const float invWF = 1.0f / wF.w;
b3Vec3 nearWorld = { wN.x * invWN, wN.y * invWN, wN.z * invWN };
b3Vec3 farWorld = { wF.x * invWF, wF.y * invWF, wF.z * invWF };
// Lift the near point to absolute world with the double eye. Translation is
// a delta so it stays eye-relative and needs no offset.
ray.origin = b3OffsetPos( m_worldEye, nearWorld );
ray.translation = b3Sub( farWorld, nearWorld );
return ray;
}
@ -321,13 +353,12 @@ void Camera::Update( float dt, int width, int height )
{
// Snapshot eye BEFORE rotating so yaw/pitch pivot around the eye
// (FPS) instead of around m_pivot (orbit). After rotation we
// back-derive m_pivot from this preserved eye so Position()
// returns the same point regardless of look angle.
const b3Vec3 eyeBefore = Position();
// back-derive m_pivot from this preserved eye so the eye stays put
// regardless of look angle.
const b3Pos eyeBefore = b3OffsetPos( m_pivot, b3MulSV( m_radius, ForwardFromAngles( m_yaw, m_pitch ) ) );
// FPS look: drag right -> yaw decreases (turn head right, scene
// shifts left); drag down -> pitch increases (look down). Box3D's
// signs - opposite of render3d's orbit pitch sign on purpose.
// shifts left); drag down -> pitch increases (look down).
if ( m_orbitDX != 0.0f || m_orbitDY != 0.0f )
{
m_yaw -= m_orbitDX * FLY_LOOK_SENS;
@ -351,7 +382,7 @@ void Camera::Update( float dt, int width, int height )
if ( m_aDown )
wasdR -= 1.0f;
b3Vec3 eye = eyeBefore;
b3Pos eye = eyeBefore;
if ( wasdF != 0.0f || wasdR != 0.0f )
{
// Right = normalize(worldUp x forward), matching Box3D's

View file

@ -36,7 +36,7 @@ struct sapp_event;
// translation spans near -> far. Feeds straight into b3World_CastRay*.
struct PickRay
{
b3Vec3 origin;
b3Pos origin;
b3Vec3 translation;
};
@ -57,14 +57,17 @@ public:
{
return m_view;
}
Mat4 ViewInverse() const
{
return m_viewInv;
}
Mat4 Proj() const
{
return m_proj;
}
Mat4 ProjInverse() const
{
return m_projInv;
@ -72,20 +75,22 @@ public:
b3Vec3 Position() const;
// Cached basis accessors (Box3D parity). Forward is +view-Z (pivot -> eye),
// so the look direction is -GetForward().
// Forward is +view-Z (pivot -> eye), so the look direction is -GetForward().
b3Vec3 GetPosition() const
{
return m_position;
}
b3Vec3 GetForward() const
{
return m_forward;
}
b3Vec3 GetRight() const
{
return m_right;
}
b3Vec3 GetUp() const
{
return m_up;
@ -97,14 +102,14 @@ public:
// Box3D's sample signature: angles in degrees, pivot folded in. Mirrors
// the ~140 sample call sites exactly so they need no edits.
void SetView( float yawDegrees, float pitchDegrees, float radius, b3Vec3 pivot = b3Vec3_zero );
void SetView( float yawDegrees, float pitchDegrees, float radius, b3Pos pivot );
void SetPivot( b3Vec3 pivot )
void SetPivot( b3Pos pivot )
{
m_pivot = pivot;
}
// Forwarder for existing render3d call sites.
void SetTarget( b3Vec3 target )
void SetTarget( b3Pos target )
{
m_pivot = target;
}
@ -132,13 +137,13 @@ public:
// point for sample code that mutates m_pivot directly (third-person follow).
void UpdateTransform();
// World-space pick ray from a framebuffer-pixel coordinate, using the
// latched camera state. Wraps the free function in gfx/picking.h; hands
// back a zero-translation ray (origin at the eye) if that state is not
// ready yet, so callers never read uninitialized values.
// World-space pick ray from a framebuffer-pixel coordinate. Unprojects
// through the camera's own eye-relative matrices and framebuffer size, then
// lifts the origin to absolute world with the double eye. Hands back a zero
// ray if the camera has no size yet, so callers never read uninitialized values.
PickRay BuildPickRay( float x, float y ) const;
b3Vec3 m_pivot;
b3Pos m_pivot; // look-at point, world space. Double precision so it stays exact far from origin.
float m_yaw; // radians, around Y
float m_pitch; // radians, around camera-frame X
float m_radius; // meters from pivot
@ -155,14 +160,19 @@ public:
int m_width;
int m_height;
// Locks input to wheel-zoom; the sample drives m_pivot to a followed body
// Locks input to wheel-zoom. The sample drives m_pivot to a followed body
// and calls UpdateTransform. Eye placement stays the orbit formula.
bool m_thirdPerson;
// Cached basis (Box3D-style: forward is +view-Z, i.e. pivot->eye), refreshed
// Cached basis: forward is +view-Z, i.e. pivot->eye) refreshed
// alongside m_view / m_viewInv whenever yaw/pitch/pivot/radius change.
// These let consumers (picking, shadow frustum) skip re-inverting m_view.
//
// The view is built in the eye-relative frame, so m_position (the eye in that
// frame) is always zero and the view matrix is translation free. m_worldEye is
// the eye in world space, the draw origin the sample renders and picks against.
b3Vec3 m_position;
b3Pos m_worldEye;
b3Vec3 m_right;
b3Vec3 m_up;
b3Vec3 m_forward;

View file

@ -1,19 +1,6 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
// Box3D samples host: a sokol_app shell driving render3d's renderer. The four
// sokol_app callbacks own the window and input; everything below the entry
// points (InitRenderer, RenderFrame, the Draw* API, the b3DebugDraw adapter) is
// host-agnostic. See render3d's HOST_INTEGRATION.md for the contract this fills.
//
// OnInit: InitRenderer -> InitUI -> InitAdapter -> Load -> SelectSample
// OnEvent: Esc quits; ImGui gate; else feed camera + dispatch to the sample
// OnFrame: ResetFrameArena -> Step -> Render -> RenderFrame -> UI -> commit
// OnCleanup: destroy sample + Save -> ShutdownUI -> ShutdownRenderer
//
// --frames N runs N frames then exits with status = sokol validation-error
// count, the automated regression net for the port.
#include "gfx/debug_adapter.h"
#include "gfx/keycodes.h"
#include "gfx/renderer.h"
@ -24,6 +11,7 @@
#include "box3d/box3d.h"
#include "box3d/math_functions.h"
#include "gfx/draw.h"
#include <stdio.h>
#include <stdlib.h>
@ -303,6 +291,11 @@ static void OnFrame( void )
Camera& camera = s_context.camera;
camera.Update( dt, W, H );
// Sync the draw origin to the camera eye once per frame, before any drawing. This must hold even
// for samples that drive their own Step without calling Sample::Step. Render re-syncs after Step
// because a third person follow moves the eye while stepping.
SetDrawOrigin( camera.m_worldEye );
ResetFrameArena();
// Apply the per-frame draw state the UI owns, then advance the sample. Step
@ -327,6 +320,17 @@ static void OnFrame( void )
fi.proj = camera.Proj();
fi.projInv = camera.ProjInverse();
fi.cameraPosition = camera.Position();
// Read back the origin Render actually shifted to. A follow-cam sample can
// move it during Step/Render, so reuse the live value rather than the eye.
b3Pos drawOrigin = GetDrawOrigin();
fi.drawOrigin = b3ToVec3( drawOrigin );
// Wrap the origin to the grid period in double, before it narrows to float.
// A float can't resolve a 1 m cell at 1e7 m, so feeding the raw origin to
// the grid would shatter the lines. The pattern repeats every 10 cells, so
// the wrapped offset draws identical lines at any distance.
double gridPeriod = 10.0 * BOX3D_GROUND_GRID_CELL_SIZE;
fi.gridWrap.x = (float)fmod( (double)drawOrigin.x, gridPeriod );
fi.gridWrap.y = (float)fmod( (double)drawOrigin.z, gridPeriod );
fi.time = (float)sapp_frame_count() / 60.0f;
fi.debugMode = s_context.debugView;
fi.disableShadows = !s_context.enableShadows;
@ -399,7 +403,12 @@ sapp_desc sokol_main( int argc, char** argv )
// No swap-chain MSAA. The renderer runs MSAA in its own scene target.
desc.sample_count = 1;
desc.window_title = "Box3D Samples";
// Static so the pointer outlives sokol_main; sokol keeps it for the window lifetime.
b3Version version = b3GetVersion();
static char title[64];
snprintf( title, sizeof( title ), "Box3D %d.%d.%d - %s precision", version.major, version.minor,
version.revision, b3IsDoublePrecision() ? "double" : "single" );
desc.window_title = title;
// Vsync off: the software limiter in OnFrame owns the cadence. A hard 60 Hz
// cap under vsync would beat against a non-60 display and pace to the wrong rate.

View file

@ -29,7 +29,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <thread>
// #include <thread>
#define INFO_PANEL_WIDTH 16.0f
@ -426,18 +426,21 @@ void Sample::Step()
SetHoveredBody( hovered );
}
}
void Sample::Render()
{
// The frame latched the origin before Step, but a third person follow moves the eye while
// stepping, so refresh it here. Shapes come back demoted to float against this origin, the same
// relative frame the translation free view renders, so everything stays precise far from the origin.
SetDrawOrigin( m_camera->m_worldEye );
b3DebugDraw debugDraw;
MakeDebugDraw( &debugDraw );
// Generous visible volume around the eye. Box3D uses this to decide which
// shapes enter the draw set and lazily fire createDebugShape.
b3Vec3 position = m_camera->GetPosition();
// Generous visible volume around the eye. Box3D uses this to decide which shapes enter the
// draw set and lazily fire createDebugShape. The cull bounds live in absolute world space to
// match the broad-phase tree; the draw origin is the world eye.
b3Vec3 r = { 1000.0f, 1000.0f, 1000.0f };
debugDraw.drawingBounds = { position - r, position + r };
b3AABB bounds = b3OffsetAABB( { b3Neg( r ), r }, m_camera->m_worldEye );
debugDraw.drawingBounds = bounds;
ApplyGuiFlags( &debugDraw );
@ -1049,7 +1052,8 @@ void Sample::MouseDown( b3Vec2 p, int button, int modifiers )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() );
b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation,
b3DefaultQueryFilter() );
if ( result.hit )
{
@ -1117,6 +1121,7 @@ void Sample::MouseDown( b3Vec2 p, int button, int modifiers )
bodyDef.type = b3_dynamicBody;
bodyDef.position = pickRay.origin + 2.0f * direction;
bodyDef.linearVelocity = ( 10.0f * m_launchSpeedScale ) * direction;
bodyDef.isBullet = true;
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3HullData* hull = b3CreateCylinder( 2.0f, 0.15f, 0.0f, 6 );
@ -1125,11 +1130,11 @@ void Sample::MouseDown( b3Vec2 p, int button, int modifiers )
}
else if ( modifiers & MOD_ALT )
{
b3Vec3 position = pickRay.origin + 2.0f * direction;
b3Pos position = pickRay.origin + 2.0f * direction;
Human human = {};
CreateHuman( &human, m_worldId, position, 1.0f, 1.0f, 1.0f, 0, nullptr, true );
Human_SetBullet( &human, true );
Human_SetVelocity( &human, ( 5.0f * m_launchSpeedScale ) * direction );
Human_SetVelocity( &human, ( 10.0f * m_launchSpeedScale ) * direction );
}
else
{
@ -1757,7 +1762,7 @@ static void DrawInfoPanel( SampleContext* context )
ImGui::TextColored( HexColor( b3_colorSeaGreen ), "step %d", context->sample->m_stepCount );
ImGui::Separator();
b3Vec3 p = context->camera.m_pivot;
b3Pos p = context->camera.m_pivot;
ImGui::TextColored( HexColor( b3_colorSeaGreen ), "pivot (%.1f, %.1f, %.1f)", p.x, p.y, p.z );
float yawDeg = B3_RAD_TO_DEG * context->camera.m_yaw;
float pitchDeg = B3_RAD_TO_DEG * context->camera.m_pitch;
@ -1849,7 +1854,7 @@ void Sample::ToggleThirdPerson()
}
}
float CastClosestCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex,
float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex,
int childIndex, void* context )
{
CastClosestContext* rayContext = (CastClosestContext*)context;
@ -1878,7 +1883,7 @@ static bool MoverFilterCallback( b3ShapeId shapeId, void* context )
return true;
}
void CharacterMover::Initialize( Sample* sample, b3Vec3 position )
void CharacterMover::Initialize( Sample* sample, b3Pos position )
{
m_sample = sample;
m_transform.p = position;
@ -1924,7 +1929,7 @@ static bool PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* planeResults
.clipVelocity = clipVelocity,
};
self->m_planeExtras[self->m_planeCount] = {
.point = planeResults[i].point,
.point = b3OffsetPos( self->m_transform.p, planeResults[i].point ),
.shapeId = shapeId,
};
self->m_planeCount += 1;
@ -1990,7 +1995,7 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
float pogoRestLength = 3.0f * m_capsule.radius;
float rayLength = pogoRestLength + m_capsule.radius;
b3Vec3 rayOrigin = b3TransformPoint( m_transform, m_capsule.center1 );
b3Pos rayOrigin = b3TransformWorldPoint( m_transform, m_capsule.center1 );
b3Vec3 rayTranslation = -rayLength * b3Vec3_axisY;
b3QueryFilter skipTeamFilter = { 1, ~2u };
b3RayResult rayResult = b3World_CastRayClosest( worldId, rayOrigin, rayTranslation, skipTeamFilter );
@ -2000,7 +2005,7 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
m_onGround = false;
m_pogoVelocity = 0.0f;
DrawLine( rayOrigin, rayOrigin + rayTranslation, MakeColor( b3_colorGray ) );
DrawLine( rayOrigin, b3OffsetPos( rayOrigin, rayTranslation ), MakeColor( b3_colorGray ) );
}
else
{
@ -2017,8 +2022,8 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
DrawLine( rayOrigin, rayResult.point, MakeColor( b3_colorGreen ) );
}
b3Vec3 startPosition = m_transform.p;
b3Vec3 target = m_transform.p + timeStep * m_velocity + timeStep * m_pogoVelocity * b3Vec3_axisY;
b3Pos startPosition = m_transform.p;
b3Pos target = m_transform.p + timeStep * m_velocity + timeStep * m_pogoVelocity * b3Vec3_axisY;
// Want the mover to collide with allies
b3QueryFilter moverFilter = { .categoryBits = 1, .maskBits = ~0u };
@ -2034,11 +2039,11 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
m_planeCount = 0;
b3Capsule mover;
mover.center1 = b3TransformPoint( m_transform, m_capsule.center1 );
mover.center2 = b3TransformPoint( m_transform, m_capsule.center2 );
mover.center1 = m_capsule.center1;
mover.center2 = m_capsule.center2;
mover.radius = m_capsule.radius;
b3World_CollideMover( worldId, &mover, moverFilter, PlaneResultFcn, this );
b3World_CollideMover( worldId, m_transform.p, &mover, moverFilter, PlaneResultFcn, this );
b3Vec3 targetDelta = target - m_transform.p;
b3PlaneSolverResult result = b3SolvePlanes( targetDelta, m_planes, m_planeCount );
@ -2047,10 +2052,10 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
b3Vec3 delta = result.delta;
float fraction = b3World_CastMover( worldId, &mover, delta, castFilter, MoverFilterCallback, this );
float fraction = b3World_CastMover( worldId, m_transform.p, &mover, delta, castFilter, MoverFilterCallback, this );
delta *= fraction;
m_transform.p += delta;
m_transform.p = m_transform.p + delta;
if ( b3LengthSquared( delta ) < tolerance * tolerance )
{
@ -2067,15 +2072,15 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
continue;
}
b3Vec3 point = m_planeExtras[i].point;
b3Pos point = m_planeExtras[i].point;
b3Vec3 normal = b3Neg( m_planes[i].plane.normal );
float invMassA = 0.0f;
float invMassB = b3Body_GetInverseMass( bodyId );
b3Matrix3 invIB = b3Body_GetWorldInverseRotationalInertia( bodyId );
b3Vec3 pB = b3Body_GetWorldCenterOfMass( bodyId );
b3Vec3 rB = point - pB;
b3Pos pB = b3Body_GetWorldCenterOfMass( bodyId );
b3Vec3 rB = b3SubPos( point, pB );
b3Vec3 rnB = b3Cross( rB, normal );
float kNormal = invMassA + invMassB + b3Dot( rnB, b3MulMV( invIB, rnB ) );
@ -2161,31 +2166,30 @@ void CharacterMover::Step( b3ShapeId* ignoreShapes, int ignoreCount, bool clipVe
SolveMove( timeStep, forward, right, throttle, clipVelocity );
b3Pos position = m_transform.p;
// Follow the mover and latch the draw origin before drawing, so the overlays below demote
// against the same eye the view renders from.
if ( m_sample->m_camera->m_thirdPerson )
{
m_sample->m_camera->m_pivot = position;
m_sample->m_camera->UpdateTransform();
}
SetDrawOrigin( m_sample->m_camera->m_worldEye );
int count = m_planeCount;
for ( int i = 0; i < count; ++i )
{
b3Plane plane = m_planes[i].plane;
b3Vec3 p1 = m_transform.p + ( plane.offset - m_capsule.radius ) * plane.normal;
b3Vec3 p2 = p1 + 0.1f * plane.normal;
b3Pos p1 = position + ( plane.offset - m_capsule.radius ) * plane.normal;
b3Pos p2 = p1 + 0.1f * plane.normal;
DrawPoint( p1, 5.0f, MakeColor( b3_colorYellow ) );
DrawLine( p1, p2, MakeColor( b3_colorYellow ) );
}
DrawSolidCapsule( m_transform, m_capsule, MakeColor( b3_colorBlue ) );
DrawLine( m_transform.p, m_transform.p + m_velocity, MakeColor( b3_colorPurple ) );
b3Vec3 p = m_transform.p;
// m_sample->DrawTextLine( "position %.2f %.2f %.2f", p.x, p.y, p.z );
// m_sample->DrawTextLine( "forward %.2f %.2f %.2f", forward.x, forward.y, forward.z );
// m_sample->DrawTextLine( "right %.2f %.2f %.2f", right.x, right.y, right.z );
// m_sample->DrawTextLine( "velocity %.2f %.2f %.2f", m_velocity.x, m_velocity.y, m_velocity.z );
// m_sample->DrawTextLine( "iterations %d", m_totalIterations );
if ( m_sample->m_camera->m_thirdPerson )
{
m_sample->m_camera->m_pivot = p;
m_sample->m_camera->UpdateTransform();
}
DrawLine( position, position + m_velocity, MakeColor( b3_colorPurple ) );
m_ignoreShapeIds = nullptr;
m_ignoreCount = 0;

View file

@ -92,7 +92,7 @@ public:
// Update and render are split to support pausing the simulation
virtual void Step();
virtual void Render();
virtual void Render() {}
// Draw sample controls into the shared info panel. Return true if any widget
// was drawn so the panel can add a separator.
@ -139,7 +139,8 @@ public:
Camera* m_camera;
b3WorldId m_worldId;
b3Vec3 m_mousePoint;
b3Pos m_mousePoint;
b3BodyId m_mouseBodyId;
b3JointId m_mouseJointId;
float m_mouseFraction;
@ -190,7 +191,7 @@ void DrawUI( SampleContext* context );
struct CastClosestContext
{
b3ShapeId shapeId;
b3Vec3 point;
b3Pos point;
b3Vec3 normal;
float fraction;
uint64_t materialId;
@ -199,7 +200,7 @@ struct CastClosestContext
bool hit;
};
float CastClosestCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex,
float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex,
int childIndex, void* context );
struct MoverShapeUserData
@ -210,7 +211,7 @@ struct MoverShapeUserData
struct PlaneExtra
{
b3Vec3 point;
b3Pos point;
b3ShapeId shapeId;
};
@ -225,12 +226,12 @@ struct CharacterMover
static constexpr float m_friction = 4.0f;
static constexpr float m_gravity = 15.0f;
void Initialize( Sample* sample, b3Vec3 position );
void Initialize( Sample* sample, b3Pos position );
void SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, bool clipVelocity );
void Step( b3ShapeId* ignoreShapes, int ignoreCount, bool clipVelocity );
Sample* m_sample;
b3Transform m_transform;
b3WorldTransform m_transform;
b3Vec3 m_velocity;
b3Capsule m_capsule;
b3CollisionPlane m_planes[m_planeCapacity] = {};

View file

@ -108,7 +108,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 25.0f, 10.0f, 70.0f, b3Vec3_zero );
m_camera->SetView( 25.0f, 10.0f, 70.0f, b3Pos_zero );
GetGuiDraw()->drawJoints = false;
}
@ -141,8 +141,8 @@ public:
void Render() override
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 2.0f );
b3Transform t = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity };
DrawAxes( b3MakeWorldTransform( t ), 2.0f );
}
static Sample* Create( SampleContext* context )
@ -220,8 +220,8 @@ public:
void Render() override
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 4.0f );
b3Transform t = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity };
DrawAxes( b3MakeWorldTransform( t ), 4.0f );
// transform.p.y = 0.0f;
// transform.p.z = -5.0f;
@ -301,7 +301,7 @@ public:
if ( context->restart == false )
{
float radius = m_isDebug ? 20.0f : 70.0f;
m_camera->SetView( 45.0f, 20.0f, radius, b3Vec3_zero );
m_camera->SetView( 45.0f, 20.0f, radius, b3Pos_zero );
}
AddGroundBox( 60.0f );
@ -497,13 +497,13 @@ class BenchmarkHeightField : public Sample
public:
struct Context
{
b3Vec3 point;
b3Pos point;
b3Vec3 normal;
float fraction;
bool hit;
};
static float CastCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t userMaterialId,
static float CastCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId,
int triangleIndex, int childIndex, void* context )
{
(void)shapeId;
@ -524,7 +524,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 20.0f, 50.0f );
m_camera->SetView( 0.0f, 20.0f, 50.0f, b3Pos_zero );
}
m_columnCount = 50;
@ -555,9 +555,9 @@ public:
{
Sample::Render();
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisX, MakeColor( b3_colorRed ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisY, MakeColor( b3_colorGreen ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisZ, MakeColor( b3_colorBlue ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisX ), MakeColor( b3_colorRed ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisY ), MakeColor( b3_colorGreen ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisZ ), MakeColor( b3_colorBlue ) );
int hitCount = 0;
int iterationCount = 0;
@ -571,16 +571,14 @@ public:
uint64_t startTick = b3GetTicks();
b3Vec3 rayTranslation = { 80000.0f, -80000.0f, 8.0f };
// b3Vec3 rayTranslation = { 0.0f, -16.0f, 0.0f };
b3Sphere sphere = { b3Vec3_zero, m_radius };
int castCount = 0;
for ( float x = -spanX; x <= spanX; x += delta )
{
for ( float z = -spanZ; z <= spanZ; z += delta )
{
b3Vec3 rayOrigin = { x, 2.0f, z };
b3Pos rayOrigin = { x, 2.0f, z };
b3RayResult result = {};
if ( m_radius == 0.0f )
@ -590,8 +588,9 @@ public:
else
{
Context context = {};
b3ShapeProxy proxy = { &rayOrigin, 1, m_radius };
b3World_CastShape( m_worldId, &proxy, rayTranslation, b3DefaultQueryFilter(), CastCallback, &context );
b3ShapeProxy proxy = { &b3Vec3_zero, 1, m_radius };
b3World_CastShape( m_worldId, rayOrigin, &proxy, rayTranslation, b3DefaultQueryFilter(), CastCallback,
&context );
if ( context.hit )
{
@ -609,19 +608,19 @@ public:
hitCount += 1;
if ( m_isDebug )
{
b3Vec3 point = result.point;
b3Pos point = result.point;
DrawLine( point, point + 0.5f * result.normal, MakeColor( b3_colorGreen ) );
DrawPoint( point, 10.0f, MakeColor( b3_colorGreen ) );
if ( m_radius > 0.0f )
{
b3Transform transform = b3Transform_identity;
transform.p = rayOrigin + result.fraction * rayTranslation;
DrawSphereEx( transform, m_radius, MakeColorAlpha( b3_colorPurple, 0.5f ), 0.0f, 0.5f,
b3WorldTransform t = b3WorldTransform_identity;
t.p = rayOrigin + result.fraction * rayTranslation;
DrawSphereEx( t, m_radius, MakeColorAlpha( b3_colorPurple, 0.5f ), 0.0f, 0.5f,
TRANSPARENT_SHADOW_NONE );
}
b3Vec3 rayEnd = rayOrigin + result.fraction * rayTranslation;
b3Pos rayEnd = rayOrigin + result.fraction * rayTranslation;
DrawLine( rayOrigin, rayEnd, MakeColor( b3_colorYellow ) );
DrawPoint( rayOrigin, 2.0f, MakeColor( b3_colorRed ) );
DrawPoint( rayEnd, 2.0f, MakeColor( b3_colorRed ) );
@ -629,7 +628,7 @@ public:
}
else if ( m_isDebug )
{
b3Vec3 rayEnd = rayOrigin + rayTranslation;
b3Pos rayEnd = rayOrigin + rayTranslation;
DrawLine( rayOrigin, rayEnd, MakeColor( b3_colorYellow ) );
DrawPoint( rayOrigin, 2.0f, MakeColor( b3_colorRed ) );
DrawPoint( rayEnd, 2.0f, MakeColor( b3_colorRed ) );
@ -1003,7 +1002,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 10.0f, 250.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 10.0f, 250.0f, b3Pos_zero );
}
b3Capacity capacity = {};
@ -1035,7 +1034,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero );
}
g_randomSeed = 42;
@ -1059,11 +1058,11 @@ public:
void Render() override
{
b3Transform transform1 = { { -2.0f, 0.0f, 0.0f }, b3Quat_identity };
b3Transform transform2 = { { 2.0f, 0.0f, 0.0f }, b3Quat_identity };
b3Transform t1 = { { -2.0f, 0.0f, 0.0f }, b3Quat_identity };
b3Transform t2 = { { 2.0f, 0.0f, 0.0f }, b3Quat_identity };
DrawHull( transform1, m_hull, MakeColor( b3_colorGreen ) );
DrawHull( transform2, m_transformedHull, MakeColor( b3_colorYellow ) );
DrawHull( b3MakeWorldTransform( t1 ), m_hull, MakeColor( b3_colorGreen ) );
DrawHull( b3MakeWorldTransform( t2 ), m_transformedHull, MakeColor( b3_colorYellow ) );
Sample::Render();
}
@ -1383,12 +1382,12 @@ public:
DrawTextLine( "destroy = %.2f ms", m_destroyMilliseconds );
float r = m_explosionDef.radius;
b3Sphere sphere1 = { m_explosionDef.position, r };
DrawWireSphere( b3Transform_identity, &sphere1, 24, MakeColor( b3_colorAqua ) );
b3Sphere sphere1 = { b3Vec3_zero, r };
DrawWireSphere( { m_explosionDef.position, b3Quat_identity }, &sphere1, 24, MakeColor( b3_colorAqua ) );
float rf = r + m_explosionDef.falloff;
b3Sphere sphere2 = { m_explosionDef.position, rf };
DrawWireSphere( b3Transform_identity, &sphere2, 24, MakeColor( b3_colorCornsilk ) );
b3Sphere sphere2 = { b3Vec3_zero, rf };
DrawWireSphere( { m_explosionDef.position, b3Quat_identity }, &sphere2, 24, MakeColor( b3_colorCornsilk ) );
}
void Step() override
@ -1430,7 +1429,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 125.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 125.0f, b3Pos_zero );
GetGuiDraw()->drawJoints = false;
}
@ -1441,7 +1440,11 @@ public:
void Step() override
{
StepJunkyard( m_worldId, m_stepCount );
if ( m_context->pause == false || m_context->singleStep == 0 )
{
StepJunkyard( m_worldId, m_stepCount );
}
Sample::Step();
}

View file

@ -1,13 +1,12 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "gfx/draw.h"
#include "gfx/keycodes.h"
#include "imgui.h"
#include "sample.h"
#include "gfx/keycodes.h"
#include "box3d/box3d.h"
#include "gfx/draw.h"
class BodyType : public Sample
{
@ -71,7 +70,7 @@ public:
b3CreateHullShape( m_platformId, &shapeDef, &box.base );
b3RevoluteJointDef revoluteDef = b3DefaultRevoluteJointDef();
b3Vec3 pivot = { -2.0f, 5.0f, 0.0f };
b3Pos pivot = { -2.0f, 5.0f, 0.0f };
revoluteDef.base.bodyIdA = m_attachmentId;
revoluteDef.base.bodyIdB = m_platformId;
revoluteDef.base.localFrameA.p = b3Body_GetLocalPoint( m_attachmentId, pivot );
@ -90,7 +89,7 @@ public:
b3CreateRevoluteJoint( m_worldId, &revoluteDef );
b3PrismaticJointDef prismaticDef = b3DefaultPrismaticJointDef();
b3Vec3 anchor = { 0.0f, 5.0f, 0.0f };
b3Pos anchor = { 0.0f, 5.0f, 0.0f };
prismaticDef.base.bodyIdA = groundId;
prismaticDef.base.bodyIdB = m_platformId;
prismaticDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, anchor );
@ -238,7 +237,7 @@ public:
// Drive the kinematic body.
if ( m_type == b3_kinematicBody )
{
b3Vec3 p = b3Body_GetPosition( m_platformId );
b3Pos p = b3Body_GetPosition( m_platformId );
b3Vec3 v = b3Body_GetLinearVelocity( m_platformId );
if ( ( p.x < -14.0f && v.x < 0.0f ) || ( p.x > 6.0f && v.x > 0.0f ) )
@ -364,7 +363,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 25.0f, 25.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 25.0f, 25.0f, b3Pos_zero );
}
AddGroundBox( 30.0f );
@ -385,7 +384,7 @@ public:
b3Vec3 offset = { 0.0f, -1.5f, 0.0f };
// See: https://en.wikipedia.org/wiki/Parallel_axis_theorem
inertiaTensor = b3AddMM(inertiaTensor, b3Steiner( mass, offset ));
inertiaTensor = b3AddMM( inertiaTensor, b3Steiner( mass, offset ) );
b3MassData massData = {
.mass = mass,
@ -429,12 +428,12 @@ public:
{
Sample::Step();
b3Sphere sphere = { m_explosionPosition, m_explosionRadius };
DrawWireSphere(b3Transform_identity, &sphere, 64, MakeColor(b3_colorAzure) );
b3Sphere sphere = { b3Vec3_zero, m_explosionRadius };
DrawWireSphere( { m_explosionPosition, b3Quat_identity }, &sphere, 64, MakeColor( b3_colorAzure ) );
// This shows how to get the velocity of a point on a body
b3Vec3 localPoint = { 0.0f, 2.0f, 0.0f };
b3Vec3 worldPoint = b3Body_GetWorldPoint( m_weebleId, localPoint );
b3Pos worldPoint = b3Body_GetWorldPoint( m_weebleId, localPoint );
b3Vec3 v1 = b3Body_GetLocalPointVelocity( m_weebleId, localPoint );
b3Vec3 v2 = b3Body_GetWorldPointVelocity( m_weebleId, worldPoint );
@ -450,7 +449,7 @@ public:
}
b3BodyId m_weebleId;
b3Vec3 m_explosionPosition;
b3Pos m_explosionPosition;
float m_explosionRadius;
float m_explosionMagnitude;
};
@ -470,7 +469,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 25.0f, 10.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 25.0f, 10.0f, b3Pos_zero );
}
AddGroundBox( 20.0f );
@ -485,7 +484,7 @@ public:
b3BodyId parentId = {};
for ( int link = 0; link < e_count; ++link )
{
bodyDef.position = { 0.0f, (float( e_count ) - link) * linkLength + 1.0f, 0.0f };
bodyDef.position = { 0.0f, ( float( e_count ) - link ) * linkLength + 1.0f, 0.0f };
bodyDef.type = B3_IS_NULL( parentId ) ? b3_kinematicBody : b3_dynamicBody;
b3BodyId childId = b3CreateBody( m_worldId, &bodyDef );
b3CreateCapsuleShape( childId, &shapeDef, &capsule );
@ -591,10 +590,10 @@ public:
m_transform.p = { -10.0f, 2.0f, 0.0f };
m_transform.q = b3MakeQuatFromAxisAngle( b3Normalize( { 1.0f, -2.0f, 3.0f } ), 0.75f * B3_PI );
m_baseTranslation = b3Vec3_zero;
m_baseTranslation = b3Pos_zero;
m_baseX = 0;
m_baseY = 0;
m_origin = b3Vec3_zero;
m_origin = b3Pos_zero;
m_tracking = false;
}
@ -605,7 +604,7 @@ public:
void MouseDown( b3Vec2 p, int button, int modifiers ) override
{
if ( button == 0 && modifiers == MOD_ALT )
if ( button == 0 && modifiers == MOD_SHIFT )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
m_origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
@ -618,13 +617,21 @@ public:
}
}
void MouseUp( b3Vec2 p, int button ) override
{
if ( button == 0 )
{
m_tracking = false;
}
}
void MouseMove( b3Vec2 p ) override
{
if ( m_tracking )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transform.p = m_baseTranslation + origin - m_origin;
b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transform.p = m_baseTranslation + b3SubPos(origin, m_origin);
}
}
@ -633,8 +640,8 @@ public:
Sample::Render();
DrawGroundGrid( 10 );
b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 4.0f );
b3WorldTransform t = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity };
DrawAxes( t, 4.0f );
DrawHull( m_transform, m_cylinder, MakeColor( b3_colorBlue ) );
}
@ -643,81 +650,86 @@ public:
{
// Cast ray
{
b3BodyRayCastInput input = {};
input.origin = { -9.75f, 3.0f, -4.0f };
input.translation = { 0.0f, 0.0f, 8.0f };
input.filter = b3DefaultQueryFilter();
input.maxFraction = 1.0f;
b3BodyCastResult result = b3Body_CastRay( m_bodyId, &input, m_transform );
b3Pos origin = { -9.75f, 3.0f, -4.0f };
b3Vec3 translation = { 0.0f, 0.0f, 8.0f };
b3QueryFilter filter = b3DefaultQueryFilter();
float maxFraction = 1.0f;
b3BodyCastResult result = b3Body_CastRay( m_bodyId, origin, translation, filter, maxFraction, m_transform );
DrawLine( input.origin, input.origin + input.maxFraction * input.translation, MakeColor( b3_colorCyan ) );
DrawLine( origin, origin + maxFraction * translation, MakeColor( b3_colorCyan ) );
if ( result.hit )
{
DrawLine( result.point, result.point + 0.2f * result.normal, MakeColor( b3_colorYellow ) );
DrawPoint( result.point, 10.0f, MakeColor( b3_colorYellow ) );
b3Pos hitPoint = result.point;
DrawLine( hitPoint, hitPoint + 0.2f * result.normal, MakeColor( b3_colorYellow ) );
DrawPoint( hitPoint, 10.0f, MakeColor( b3_colorYellow ) );
}
DrawPoint( input.origin, 10.0f, MakeColor( b3_colorGreen ) );
DrawPoint( input.origin + input.translation, 10.0f, MakeColor( b3_colorRed ) );
DrawPoint( origin, 10.0f, MakeColor( b3_colorGreen ) );
DrawPoint( origin + translation, 10.0f, MakeColor( b3_colorRed ) );
}
// Cast sphere
{
b3Sphere sphere = { { -14.5f, 2.5f, 0.5f }, 0.2f };
b3Pos origin = { -14.5f, 2.5f, 0.5f };
b3Sphere sphere = { b3Vec3_zero, 0.2f };
b3BodyShapeCastInput input = {};
input.proxy = { &sphere.center, 1, sphere.radius };
input.translation = { 8.0f, 0.0f, 0.0f };
input.filter = b3DefaultQueryFilter();
input.maxFraction = 1.0f;
input.canEncroach = true;
b3BodyCastResult result = b3Body_CastShape( m_bodyId, &input, m_transform );
b3ShapeProxy proxy = { &sphere.center, 1, sphere.radius };
b3Vec3 translation = { 8.0f, 0.0f, 0.0f };
b3QueryFilter filter = b3DefaultQueryFilter();
float maxFraction = 1.0f;
bool canEncroach = true;
b3BodyCastResult result =
b3Body_CastShape( m_bodyId, origin, &proxy, translation, filter, maxFraction, canEncroach, m_transform );
b3Pos sphereCenter = b3OffsetPos( origin, sphere.center );
if ( result.hit )
{
b3Transform transform = { result.fraction * input.translation, b3Quat_identity };
DrawSolidSphere( transform, sphere, MakeColor( b3_colorGreen ) );
DrawLine( result.point, result.point + 0.2f * result.normal, MakeColor( b3_colorYellow ) );
b3WorldTransform t = { origin + result.fraction * translation, b3Quat_identity };
DrawSolidSphere( t, sphere, MakeColor( b3_colorGreen ) );
b3Pos hitPoint = result.point;
DrawLine( hitPoint, hitPoint + 0.2f * result.normal, MakeColor( b3_colorYellow ) );
}
else
{
b3Transform transform = { input.maxFraction * input.translation, b3Quat_identity };
DrawSolidSphere( transform, sphere, MakeColor( b3_colorWhite ) );
b3WorldTransform t = { origin + maxFraction * translation, b3Quat_identity };
DrawSolidSphere( t, sphere, MakeColor( b3_colorWhite ) );
}
DrawLine( sphere.center, sphere.center + input.maxFraction * input.translation, MakeColor( b3_colorWhite ) );
DrawPoint( sphere.center, 10.0f, MakeColor( b3_colorGreen ) );
DrawPoint( sphere.center + input.maxFraction * input.translation, 10.0f, MakeColor( b3_colorRed ) );
DrawLine( sphereCenter, sphereCenter + maxFraction * translation, MakeColor( b3_colorWhite ) );
DrawPoint( sphereCenter, 10.0f, MakeColor( b3_colorGreen ) );
DrawPoint( sphereCenter + maxFraction * translation, 10.0f, MakeColor( b3_colorRed ) );
}
// Overlap capsule
{
b3Capsule capsule = { { -10.5f, 2.0f, 0.5f }, { -9.5f, 1.0f, 0.5f }, 0.5f };
b3Pos origin = { -10.0f, 1.0f, 0.5f };
b3Capsule capsule = { { -.5f, 1.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.5f };
b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius };
bool overlaps = b3Body_OverlapShape( m_bodyId, &proxy, b3DefaultQueryFilter(), m_transform );
bool overlaps = b3Body_OverlapShape( m_bodyId, origin, &proxy, b3DefaultQueryFilter(), m_transform );
if ( overlaps )
{
DrawSolidCapsule( b3Transform_identity, capsule, MakeColor( b3_colorGreen ) );
DrawSolidCapsule( { origin, b3Quat_identity }, capsule, MakeColor( b3_colorGreen ) );
}
else
{
DrawSolidCapsule( b3Transform_identity, capsule, MakeColor( b3_colorGray ) );
DrawSolidCapsule( { origin, b3Quat_identity }, capsule, MakeColor( b3_colorGray ) );
}
}
// Collide capsule
{
b3Capsule capsule = { { -10.25f, 2.0f, -0.75f }, { -10.25f, 3.0f, -0.75f }, 0.3f };
b3Pos origin = { -10.0f, 2.0f, -0.75f };
b3Capsule capsule = { { -0.25f, 0.0f, 0.0f }, { 0.25f, 1.0f, 0.0f }, 0.3f };
b3BodyPlaneResult bodyPlanes[4];
int count = b3Body_CollideMover( m_bodyId, bodyPlanes, 4, &capsule, b3DefaultQueryFilter(), m_transform );
DrawSolidCapsule( b3Transform_identity, capsule, MakeColor( b3_colorPurple ) );
int count = b3Body_CollideMover( m_bodyId, bodyPlanes, 4, origin, &capsule, b3DefaultQueryFilter(), m_transform );
DrawSolidCapsule( { origin, b3Quat_identity }, capsule, MakeColor( b3_colorPurple ) );
for ( int i = 0; i < count; ++i )
{
b3PlaneResult result = bodyPlanes[i].result;
DrawPlane( result.plane.normal, result.point, MakeColor( b3_colorOrange ) );
DrawPlane( result.plane.normal, b3ToPos( result.point ), MakeColor( b3_colorOrange ) );
}
}
}
@ -729,10 +741,10 @@ public:
b3HullData* m_cylinder;
b3BodyId m_bodyId;
b3Transform m_transform;
b3WorldTransform m_transform;
b3Vec3 m_baseTranslation;
b3Vec3 m_origin;
b3Pos m_baseTranslation;
b3Pos m_origin;
int m_baseX;
int m_baseY;
bool m_tracking;
@ -751,7 +763,7 @@ public:
{
m_camera->SetView( 0.0f, 30.0f, 10.0f, { 0.0f, 1.5f, 0.0f } );
}
AddGroundBox( 20.0f );
m_amplitude = 2.0f;
@ -786,7 +798,7 @@ public:
{
float t = m_time - delay;
b3Vec3 point;
b3Pos point;
point.x = 2.0f * m_amplitude * cosf( t );
point.y = m_amplitude * ( sinf( 2.0f * t ) + 1.0f ) + 1.0f;
point.z = 0.0f;
@ -824,7 +836,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Pos_zero );
}
AddGroundBox( 20.0f );
@ -907,7 +919,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 10.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 10.0f, b3Pos_zero );
}
AddGroundBox( 20.0f );

View file

@ -1,12 +1,10 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "sample.h"
#include "mesh_loader.h"
#include "gfx/draw.h"
#include "gfx/keycodes.h"
#include "mesh_loader.h"
#include "sample.h"
#include "box3d/box3d.h"
@ -39,10 +37,10 @@ public:
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
b3CreateHullShape( body, &shapeDef, &box.base );
m_baseTranslation = b3Vec3_zero;
m_baseTranslation = b3Pos_zero;
m_baseX = 0;
m_baseY = 0;
m_origin = b3Vec3_zero;
m_origin = b3Pos_zero;
m_tracking = false;
m_planeCount = 0;
}
@ -50,7 +48,7 @@ public:
void Solve()
{
b3PlaneSolverResult result = b3SolvePlanes( b3Vec3_zero, m_planes, m_planeCount );
m_transform.p += result.delta;
m_transform.p = m_transform.p + result.delta;
}
void Render() override
@ -58,9 +56,9 @@ public:
Sample::Render();
DrawGroundGrid( 10 );
DrawLine( b3Vec3_zero, 2.0f * b3Vec3_axisX, MakeColor( b3_colorRed ) );
DrawLine( b3Vec3_zero, 2.0f * b3Vec3_axisY, MakeColor( b3_colorGreen ) );
DrawLine( b3Vec3_zero, 2.0f * b3Vec3_axisZ, MakeColor( b3_colorBlue ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 2.0f * b3Vec3_axisX ), MakeColor( b3_colorRed ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 2.0f * b3Vec3_axisY ), MakeColor( b3_colorGreen ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 2.0f * b3Vec3_axisZ ), MakeColor( b3_colorBlue ) );
}
bool DrawControls() override
@ -105,8 +103,8 @@ public:
if ( m_tracking )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transform.p = m_baseTranslation + origin - m_origin;
b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transform.p = m_baseTranslation + b3SubPos( origin, m_origin );
}
}
@ -117,14 +115,14 @@ public:
DrawSolidCapsule( m_transform, m_capsule, MakeColor( b3_colorGreen ) );
b3QueryFilter filter = b3DefaultQueryFilter();
b3Capsule capsule = { m_capsule.center1 + m_transform.p, m_capsule.center2 + m_transform.p, m_capsule.radius };
b3World_CollideMover( m_worldId, &capsule, filter, PlaneResultFcn, this );
b3Capsule capsule = { m_capsule.center1, m_capsule.center2, m_capsule.radius };
b3World_CollideMover( m_worldId, m_transform.p, &capsule, filter, PlaneResultFcn, this );
for ( int i = 0; i < m_planeCount; ++i )
{
b3Plane plane = m_planes[i].plane;
b3Vec3 p1 = m_transform.p + ( plane.offset - m_capsule.radius ) * plane.normal;
b3Vec3 p2 = p1 + 0.1f * plane.normal;
b3Pos p1 = m_transform.p + ( plane.offset - m_capsule.radius ) * plane.normal;
b3Pos p2 = p1 + 0.1f * plane.normal;
DrawPoint( p1, 5.0f, MakeColor( b3_colorYellow ) );
DrawLine( p1, p2, MakeColor( b3_colorYellow ) );
}
@ -132,13 +130,13 @@ public:
static constexpr int m_planeCapacity = 3;
b3Transform m_transform;
b3WorldTransform m_transform;
b3Capsule m_capsule;
b3CollisionPlane m_planes[m_planeCapacity] = {};
int m_planeCount;
b3Vec3 m_baseTranslation;
b3Vec3 m_origin;
b3Pos m_baseTranslation;
b3Pos m_origin;
int m_baseX;
int m_baseY;
bool m_tracking;
@ -200,8 +198,8 @@ public:
b3CreateHullShape( body, &shapeDef, &box.base );
}
m_baseTranslation = b3Vec3_zero;
m_origin = b3Vec3_zero;
m_baseTranslation = b3Pos_zero;
m_origin = b3Pos_zero;
m_tracking = false;
m_planeCount = 0;
m_zeroNormalCount = 0;
@ -245,20 +243,20 @@ public:
if ( m_tracking )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transform.p = m_baseTranslation + origin - m_origin;
b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transform.p = m_baseTranslation + b3SubPos( origin, m_origin );
}
}
void Step() override
{
// Build the mover in world space and query all overlapping shapes.
b3Capsule worldMover = { m_capsule.center1 + m_transform.p, m_capsule.center2 + m_transform.p, m_capsule.radius };
// Query overlapping shapes. The mover capsule is relative to the body origin.
b3Capsule mover = { m_capsule.center1, m_capsule.center2, m_capsule.radius };
m_planeCount = 0;
m_zeroNormalCount = 0;
b3QueryFilter filter = b3DefaultQueryFilter();
b3World_CollideMover( m_worldId, &worldMover, filter, PlaneResultFcn, this );
b3World_CollideMover( m_worldId, m_transform.p, &mover, filter, PlaneResultFcn, this );
// Mover at the queried position.
DrawSolidCapsule( m_transform, m_capsule, MakeColor( b3_colorYellow ) );
@ -271,8 +269,9 @@ public:
b3PlaneResult r = m_results[i];
bool valid = b3IsNormalized( r.plane.normal );
b3HexColor color = valid ? b3_colorLimeGreen : b3_colorRed;
DrawPoint( r.point, 6.0f, MakeColor( color ) );
DrawArrow( r.point, r.point + 0.5f * r.plane.normal, MakeColor( color ) );
b3Pos rp = b3OffsetPos( m_transform.p, r.point );
DrawPoint( rp, 6.0f, MakeColor( color ) );
DrawArrow( rp, b3OffsetPos( rp, 0.5f * r.plane.normal ), MakeColor( color ) );
if ( valid == false )
{
m_zeroNormalCount += 1;
@ -282,7 +281,7 @@ public:
// Solve the planes and show the pushed-out capsule pose.
b3PlaneSolverResult solved = b3SolvePlanes( b3Vec3_zero, solverPlanes, m_planeCount );
b3Transform pushed = { m_transform.p + solved.delta, m_transform.q };
b3WorldTransform pushed = { m_transform.p + solved.delta, m_transform.q };
DrawSolidCapsule( pushed, m_capsule, MakeColor( b3_colorCyan ) );
DrawTextLine( "drag the capsule with the left mouse to push it into the shapes" );
@ -299,14 +298,14 @@ public:
static constexpr int m_planeCapacity = 32;
b3Transform m_transform;
b3WorldTransform m_transform;
b3Capsule m_capsule;
b3PlaneResult m_results[m_planeCapacity] = {};
int m_planeCount;
int m_zeroNormalCount;
b3Vec3 m_baseTranslation;
b3Vec3 m_origin;
b3Pos m_baseTranslation;
b3Pos m_origin;
bool m_tracking;
};
@ -318,7 +317,7 @@ public:
explicit BasicMover( SampleContext* context )
: Sample( context )
{
b3Vec3 moverPosition = { 7.5f, 0.75f, 9.0f };
b3Pos moverPosition = { 7.5f, 0.75f, 9.0f };
if ( m_context->restart == false )
{
@ -417,7 +416,7 @@ public:
0.3f,
};
//shapeDef.filter = { 2u, ~0u, 0 };
// shapeDef.filter = { 2u, ~0u, 0 };
shapeDef.userData = &m_enemyShape;
shapeDef.baseMaterial.customColor = b3_colorMediumVioletRed;
b3BodyId body = b3CreateBody( m_worldId, &bodyDef );
@ -488,7 +487,7 @@ public:
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = bodyId;
jointDef.base.localFrameA.p = b3Add( bodyDef.position, offset );
jointDef.base.localFrameA.p = b3ToVec3( bodyDef.position + offset );
jointDef.base.localFrameA.q = axisQuat;
jointDef.base.localFrameB.p = offset;
jointDef.base.localFrameB.q = axisQuat;
@ -521,9 +520,9 @@ public:
m_clipVelocity = true;
// sapp_lock_mouse( true );
//m_haveMouseLast = false;
//m_mouseLast = { 0.0f, 0.0f };
//m_mouseDelta = { 0.0f, 0.0f };
// m_haveMouseLast = false;
// m_mouseLast = { 0.0f, 0.0f };
// m_mouseDelta = { 0.0f, 0.0f };
}
~BasicMover() override
@ -565,7 +564,7 @@ public:
void Step() override
{
m_mover.Step(&m_ignoreShapeId, 1, m_clipVelocity);
m_mover.Step( &m_ignoreShapeId, 1, m_clipVelocity );
DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson );
DrawTextLine( "deltaX = %g, deltaY = %g", m_mouseDelta.x, m_mouseDelta.y );
@ -596,13 +595,13 @@ struct ClosestShapeCastContext
int ignoreCount;
float closestFraction;
b3Vec3 closestNormal;
b3Vec3 closestPoint;
b3Pos closestPoint;
b3ShapeId closestShape;
bool hit;
bool startedSolid;
};
static float ClosestShapeCastCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t userMaterialId,
static float ClosestShapeCastCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId,
int triangleIndex, int childIndex, void* context )
{
auto* ctx = static_cast<ClosestShapeCastContext*>( context );
@ -637,9 +636,9 @@ static float ClosestShapeCastCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 n
struct TraceResult
{
b3Vec3 endPosition;
b3Pos endPosition;
b3Vec3 normal;
b3Vec3 hitPoint;
b3Pos hitPoint;
float fraction;
bool hit;
bool startedSolid;
@ -692,15 +691,15 @@ struct RigidbodyCharacter
// Step-up state: position to restore after physics step
bool m_didStep;
b3Vec3 m_stepPosition;
b3Pos m_stepPosition;
// Debug readouts
b3Vec3 m_lastWishVelocity;
b3Vec3 m_massCenterWorld;
b3Pos m_massCenterWorld;
Sample* m_sample;
void Initialize( Sample* sample, b3Vec3 position )
void Initialize( Sample* sample, b3Pos position )
{
m_sample = sample;
m_onGround = false;
@ -709,14 +708,14 @@ struct RigidbodyCharacter
m_groundNormal = b3Vec3_axisY;
m_groundVelocity = b3Vec3_zero;
m_lastWishVelocity = b3Vec3_zero;
m_massCenterWorld = position;
m_didStep = false;
m_stepPosition = b3Vec3_zero;
m_stepPosition = b3Pos_zero;
// Create dynamic body with all rotation locked
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = position;
m_massCenterWorld = bodyDef.position;
bodyDef.motionLocks.angularX = true;
bodyDef.motionLocks.angularY = true;
bodyDef.motionLocks.angularZ = true;
@ -784,7 +783,7 @@ struct RigidbodyCharacter
// --- TraceBody: box shape cast matching s&box's TraceBody ---
// Casts a box from `from` to `to` with given radius and height scale.
TraceResult TraceBody( b3Vec3 from, b3Vec3 to, float radiusScale = 1.0f, float heightScale = 1.0f ) const
TraceResult TraceBody( b3Pos from, b3Pos to, float radiusScale = 1.0f, float heightScale = 1.0f ) const
{
TraceResult result = {};
result.endPosition = to;
@ -819,7 +818,7 @@ struct RigidbodyCharacter
float sx = ( i & 1 ) ? halfW : -halfW;
float sy = ( i & 2 ) ? halfH : -halfH;
float sz = ( i & 4 ) ? halfD : -halfD;
points[i] = { from.x + sx, from.y + boxCenterY + sy, from.z + sz };
points[i] = { sx, boxCenterY + sy, sz };
}
b3ShapeProxy proxy;
@ -839,7 +838,7 @@ struct RigidbodyCharacter
ctx.closestShape = b3_nullShapeId;
b3QueryFilter filter = b3DefaultQueryFilter();
b3World_CastShape( m_sample->m_worldId, &proxy, translation, filter, ClosestShapeCastCallback, &ctx );
b3World_CastShape( m_sample->m_worldId, from, &proxy, translation, filter, ClosestShapeCastCallback, &ctx );
result.startedSolid = ctx.startedSolid;
if ( ctx.hit )
@ -861,26 +860,20 @@ struct RigidbodyCharacter
}
// Get feet position from body center position
b3Vec3 GetFeetPosition() const
b3Pos GetFeetPosition() const
{
b3Vec3 pos = b3Body_GetPosition( m_bodyId );
b3Pos pos = b3Body_GetPosition( m_bodyId );
return { pos.x, pos.y - m_totalHeight * 0.5f, pos.z };
}
// Convert feet position back to body center for SetTransform
b3Vec3 FeetToCenter( b3Vec3 feetPos ) const
{
return { feetPos.x, feetPos.y + m_totalHeight * 0.5f, feetPos.z };
}
// --- CategorizeGround: s&box-style box cast with radius shrinking ---
void CategorizeGround()
{
b3Vec3 feet = GetFeetPosition();
b3Pos feet = GetFeetPosition();
// s&box: from = WorldPosition + Up*4, to = WorldPosition + Down*2 (Source units)
b3Vec3 from = { feet.x, feet.y + 4.0f * SRC, feet.z };
b3Vec3 to = { feet.x, feet.y - 2.0f * SRC, feet.z };
b3Pos from = { feet.x, feet.y + 4.0f * SRC, feet.z };
b3Pos to = { feet.x, feet.y - 2.0f * SRC, feet.z };
float radiusScale = 1.0f;
TraceResult tr = TraceBody( from, to, radiusScale, 0.5f );
@ -929,10 +922,10 @@ struct RigidbodyCharacter
return;
}
b3Vec3 pos = b3Body_GetPosition( m_bodyId );
b3Pos pos = b3Body_GetPosition( m_bodyId );
b3Vec3 from = { pos.x, pos.y + 0.05f, pos.z };
b3Vec3 to = { pos.x, pos.y - stepSize, pos.z };
b3Pos from = { pos.x, pos.y + 0.05f, pos.z };
b3Pos to = { pos.x, pos.y - stepSize, pos.z };
float radiusScale = 1.0f;
TraceResult tr = TraceBody( from, to, radiusScale, 0.5f );
@ -949,7 +942,7 @@ struct RigidbodyCharacter
if ( tr.hit )
{
b3Vec3 targetPos = { tr.endPosition.x, tr.endPosition.y + 0.01f, tr.endPosition.z };
b3Pos targetPos = { tr.endPosition.x, tr.endPosition.y + 0.01f, tr.endPosition.z };
float deltaY = targetPos.y - pos.y;
b3Quat rot = b3Body_GetRotation( m_bodyId );
@ -971,7 +964,7 @@ struct RigidbodyCharacter
// Returns true if a step was taken and m_stepPosition was set.
bool TryStep( float maxStepHeight )
{
b3Vec3 pos = b3Body_GetPosition( m_bodyId );
b3Pos pos = b3Body_GetPosition( m_bodyId );
b3Vec3 vel = b3Body_GetLinearVelocity( m_bodyId );
// Only step when on ground and moving
@ -992,8 +985,8 @@ struct RigidbodyCharacter
// Phase 1 — FORWARD: trace body forward in velocity direction
// Start slightly behind (offset by skin)
float forwardDist = hSpeed * ( 1.0f / 60.0f ) + m_bodyRadius; // one frame of movement + radius
b3Vec3 forwardFrom = pos - m_skin * moveDir;
b3Vec3 forwardTo = pos + forwardDist * moveDir;
b3Pos forwardFrom = pos - m_skin * moveDir;
b3Pos forwardTo = pos + forwardDist * moveDir;
float radiusScale = 1.0f;
TraceResult trForward = TraceBody( forwardFrom, forwardTo, radiusScale );
@ -1018,11 +1011,11 @@ struct RigidbodyCharacter
DrawLine( forwardFrom, trForward.endPosition, MakeColor( b3_colorYellow ) );
// Remaining velocity direction after hit
b3Vec3 hitPos = trForward.endPosition;
b3Pos hitPos = trForward.endPosition;
// Phase 2 — UP: trace straight up from hit position
b3Vec3 upFrom = hitPos;
b3Vec3 upTo = { hitPos.x, hitPos.y + maxStepHeight, hitPos.z };
b3Pos upFrom = hitPos;
b3Pos upTo = { hitPos.x, hitPos.y + maxStepHeight, hitPos.z };
TraceResult trUp = TraceBody( upFrom, upTo, radiusScale );
if ( trUp.startedSolid )
@ -1031,7 +1024,7 @@ struct RigidbodyCharacter
return false;
}
b3Vec3 topPos = trUp.hit ? trUp.endPosition : upTo;
b3Pos topPos = trUp.hit ? trUp.endPosition : upTo;
float upDistance = topPos.y - upFrom.y;
if ( upDistance < 0.005f )
{
@ -1044,8 +1037,8 @@ struct RigidbodyCharacter
// Phase 3 — ACROSS: from top position, trace in move direction
float acrossDist = forwardDist * ( 1.0f - trForward.fraction ) + m_bodyRadius * 0.5f;
b3Vec3 acrossFrom = topPos;
b3Vec3 acrossTo = topPos + acrossDist * moveDir;
b3Pos acrossFrom = topPos;
b3Pos acrossTo = topPos + acrossDist * moveDir;
TraceResult trAcross = TraceBody( acrossFrom, acrossTo, radiusScale );
if ( trAcross.startedSolid )
@ -1054,12 +1047,12 @@ struct RigidbodyCharacter
return false;
}
b3Vec3 acrossPos = trAcross.hit ? trAcross.endPosition : acrossTo;
b3Pos acrossPos = trAcross.hit ? trAcross.endPosition : acrossTo;
DrawLine( acrossFrom, acrossPos, MakeColor( b3_colorYellow ) );
// Phase 4 — DOWN: from across position, trace straight down
b3Vec3 downFrom = acrossPos;
b3Vec3 downTo = { acrossPos.x, acrossPos.y - maxStepHeight, acrossPos.z };
b3Pos downFrom = acrossPos;
b3Pos downTo = { acrossPos.x, acrossPos.y - maxStepHeight, acrossPos.z };
TraceResult trDown = TraceBody( downFrom, downTo, radiusScale );
if ( !trDown.hit )
@ -1085,7 +1078,7 @@ struct RigidbodyCharacter
DrawPoint( trDown.endPosition, 8.0f, MakeColor( b3_colorYellow ) );
// Teleport body to step position
b3Vec3 stepPos = { trDown.endPosition.x, trDown.endPosition.y + 0.01f, trDown.endPosition.z };
b3Pos stepPos = { trDown.endPosition.x, trDown.endPosition.y + 0.01f, trDown.endPosition.z };
b3Quat rot = b3Body_GetRotation( m_bodyId );
b3Body_SetTransform( m_bodyId, stepPos, rot );
@ -1296,14 +1289,14 @@ struct RigidbodyCharacter
void DrawDebug() const
{
b3Vec3 pos = b3Body_GetPosition( m_bodyId );
b3Pos pos = b3Body_GetPosition( m_bodyId );
b3Vec3 vel = b3Body_GetLinearVelocity( m_bodyId );
// Draw velocity vector (purple)
DrawLine( pos, pos + vel, MakeColor( b3_colorPurple ) );
DrawLine( pos, b3OffsetPos( pos, vel ), MakeColor( b3_colorPurple ) );
// Draw wish velocity (orange)
DrawLine( pos, pos + m_lastWishVelocity, MakeColor( b3_colorOrange ) );
DrawLine( pos, b3OffsetPos( pos, m_lastWishVelocity ), MakeColor( b3_colorOrange ) );
// Draw mass center (yellow dot)
DrawPoint( m_massCenterWorld, 8.0f, MakeColor( b3_colorYellow ) );
@ -1311,8 +1304,8 @@ struct RigidbodyCharacter
// Draw ground indicator
if ( m_onGround )
{
b3Vec3 bottom = { pos.x, pos.y - m_totalHeight * 0.5f, pos.z };
DrawLine( bottom, bottom + 0.3f * m_groundNormal, MakeColor( b3_colorGreen ) );
b3Pos bottom = { pos.x, pos.y - m_totalHeight * 0.5f, pos.z };
DrawLine( bottom, b3OffsetPos( bottom, 0.3f * m_groundNormal ), MakeColor( b3_colorGreen ) );
}
}
};
@ -1323,7 +1316,7 @@ public:
explicit RigidBodyCharacter( SampleContext* context )
: Sample( context )
{
b3Vec3 startPosition = { 7.5f, 2.0f, 9.0f };
b3Pos startPosition = { 7.5f, 2.0f, 9.0f };
if ( m_context->restart == false )
{
@ -1575,45 +1568,44 @@ public:
// Post-step: re-categorize ground and apply corrections
m_character.LateStep( timeStep );
// Update camera pivot to follow character
b3Vec3 pos = b3Body_GetPosition( m_character.m_bodyId );
// Follow the character with the camera.
b3Pos charPos = b3Body_GetPosition( m_character.m_bodyId );
b3Pos pos = charPos;
if ( m_camera->m_thirdPerson )
{
m_camera->m_pivot = pos;
m_camera->m_pivot = charPos;
m_camera->UpdateTransform();
// Sphere collision: cast a sphere from pivot toward camera position
// to prevent clipping through geometry
// Keep the eye from clipping through geometry. Cast from the character toward the eye
// and, on a hit, shorten the boom for this frame only. Restoring the radius lets the
// next frame start from the user's chosen distance instead of ratcheting inward.
float cameraRadius = 0.15f;
b3Vec3 desiredPos = m_camera->m_position;
b3Vec3 translation = desiredPos - pos;
b3Vec3 translation = b3SubPos( m_camera->m_worldEye, charPos );
float desiredDist = b3Length( translation );
if ( desiredDist > 0.01f )
{
// Single-point sphere proxy (sphere = 1 point + radius)
b3Vec3 sphereCenter = pos;
b3ShapeProxy proxy;
proxy.points = &sphereCenter;
proxy.count = 1;
proxy.radius = cameraRadius;
b3QueryFilter filter = b3DefaultQueryFilter();
b3RayResult rayResult = b3World_CastRayClosest( m_worldId, pos, translation, filter );
b3RayResult rayResult = b3World_CastRayClosest( m_worldId, charPos, translation, filter );
if ( rayResult.hit )
{
// Pull camera in front of the hit point
float clampedDist = rayResult.fraction * desiredDist - cameraRadius;
if ( clampedDist < 0.1f )
clampedDist = 0.1f;
b3Vec3 dir = { translation.x / desiredDist, translation.y / desiredDist, translation.z / desiredDist };
m_camera->m_position = pos + clampedDist * dir;
float savedRadius = m_camera->m_radius;
m_camera->m_radius = b3MinFloat( savedRadius, clampedDist );
m_camera->UpdateTransform();
m_camera->m_radius = savedRadius;
}
}
}
// Latch the draw origin to the followed eye so the debug overlays demote against the same
// point the view renders from.
SetDrawOrigin(m_camera->m_worldEye);
// Debug visualization
if ( m_showDebug )
{
@ -1654,8 +1646,8 @@ public:
ImGui::Text( "Speed: %.2f m/s", hSpeed );
ImGui::Text( "Vertical: %.2f m/s", vel.y );
b3Vec3 mc = m_character.m_massCenterWorld;
b3Vec3 pos = b3Body_GetPosition( m_character.m_bodyId );
b3Pos mc = m_character.m_massCenterWorld;
b3Pos pos = b3Body_GetPosition( m_character.m_bodyId );
ImGui::Text( "Mass center offset: %.2f", mc.y - pos.y );
return true;

View file

@ -1,13 +1,11 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "sample.h"
#include "gfx/draw.h"
#include "utils.h"
#include "gfx/debug_adapter.h"
#include "gfx/draw.h"
#include "gfx/keycodes.h"
#include "sample.h"
#include "utils.h"
#include "box3d/box3d.h"
@ -26,7 +24,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 20.0f );
m_camera->SetView( 45.0f, 30.0f, 20.0f, b3Pos_zero );
}
b3BoxHull box = b3MakeBoxHull( 0.6f, 0.6f, 0.6f );
@ -75,26 +73,25 @@ public:
Sample::Render();
DrawGroundGrid( 10 );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisX, MakeColor( b3_colorRed ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisY, MakeColor( b3_colorGreen ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisZ, MakeColor( b3_colorBlue ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisX ), MakeColor( b3_colorRed ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisY ), MakeColor( b3_colorGreen ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisZ ), MakeColor( b3_colorBlue ) );
for ( float x = -8.0f; x <= 8.0f; x += 0.1f )
{
b3Vec3 rayOrigin = { x, 8.0f, m_offset };
b3Vec3 rayTranslation = b3Vec3{ x, 0.0f, m_offset } - rayOrigin;
b3Pos rayOrigin = { x, 8.0f, m_offset };
b3Pos rayEnd = { x, 0.0f, m_offset };
b3Vec3 rayTranslation = b3SubPos( rayEnd, rayOrigin );
b3RayResult result = b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, b3DefaultQueryFilter() );
if ( result.hit )
{
b3Vec3 point = result.point;
DrawLine( point, point + 0.5f * result.normal, MakeColor( b3_colorGreen ) );
DrawLine( result.point, b3OffsetPos( result.point, 0.5f * result.normal ), MakeColor( b3_colorGreen ) );
}
DrawPoint( rayOrigin, 4.0f, MakeColor( b3_colorGreen ) );
DrawPoint( rayOrigin + rayTranslation, 4.0f, MakeColor( b3_colorRed ) );
DrawLine( rayOrigin, rayOrigin + rayTranslation, MakeColor( b3_colorYellow ) );
DrawPoint( b3OffsetPos( rayOrigin, rayTranslation ), 4.0f, MakeColor( b3_colorRed ) );
DrawLine( rayOrigin, b3OffsetPos( rayOrigin, rayTranslation ), MakeColor( b3_colorYellow ) );
}
if ( m_offset > 2.0f )
@ -123,7 +120,7 @@ static int sampleRayCurtain = RegisterSample( "Collision", "Ray Curtain", RayCur
// Context for ray cast callbacks. Do what you want with this.
struct CastContext
{
b3Vec3 points[3];
b3Pos points[3];
b3Vec3 normals[3];
float fractions[3];
uint64_t materialIds[3];
@ -134,7 +131,7 @@ struct CastContext
};
// This callback finds the closest hit. This is the most common callback used in games.
static float RayCastClosestCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId,
static float RayCastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId,
int triangleIndex, int childIndex, void* context )
{
CastContext* rayContext = (CastContext*)context;
@ -173,7 +170,7 @@ static float RayCastClosestCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 nor
// This callback finds any hit. For this type of query we are usually just checking for obstruction,
// so the hit data is not relevant.
// NOTE: shape hits are not ordered, so this may not return the closest hit
static float RayCastAnyCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId,
static float RayCastAnyCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId,
int triangleIndex, int childIndex, void* context )
{
CastContext* rayContext = (CastContext*)context;
@ -213,7 +210,7 @@ static float RayCastAnyCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal,
// NOTE: shape hits are not ordered, so this may return hits in any order. This means that
// if you limit the number of results, you may discard the closest hit. You can see this
// behavior in the sample.
static float RayCastMultipleCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId,
static float RayCastMultipleCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId,
int triangleIndex, int childIndex, void* context )
{
CastContext* rayContext = (CastContext*)context;
@ -257,7 +254,7 @@ static float RayCastMultipleCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 no
}
// This ray cast collects multiple hits along the ray and sorts them.
static float RayCastSortedCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t materialId,
static float RayCastSortedCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId,
int triangleIndex, int childIndex, void* context )
{
CastContext* rayContext = (CastContext*)context;
@ -362,7 +359,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 20.0f );
m_camera->SetView( 45.0f, 30.0f, 20.0f, b3Pos_zero );
}
m_sphere = { b3Vec3_zero, 0.9f };
@ -425,12 +422,12 @@ public:
}
// Heightfield must be a static body
if (shapeType == b3_heightShape)
if ( shapeType == b3_heightShape )
{
bodyDef.type = b3_staticBody;
}
bodyDef.position = RandomVec3Uniform( -20.0f, 20.0f );
bodyDef.position = b3OffsetPos( b3Pos_zero, RandomVec3Uniform( -20.0f, 20.0f ) );
b3Vec3 axis = RandomVec3Uniform( -1.0f, 1.0f );
axis = b3Normalize( axis );
@ -620,7 +617,7 @@ public:
}
DrawGroundGrid( 10 );
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
Sample::Render();
}
@ -646,8 +643,8 @@ public:
m_castContext.fractions[1] = FLT_MAX;
m_castContext.fractions[2] = FLT_MAX;
b3Sphere sphere = { m_origin, m_castRadius };
b3Capsule capsule = { m_origin, m_origin + b3Vec3{ 0.0f, 1.0f, 0.0f }, m_castRadius };
b3Sphere sphere = { b3Vec3_zero, m_castRadius };
b3Capsule capsule = { b3Vec3_zero, b3Vec3{ 0.0f, 1.0f, 0.0f }, m_castRadius };
b3BoxHull box = {};
b3Vec3 pointBuffer[2];
@ -677,7 +674,8 @@ public:
case e_boxCast:
{
b3Vec3 extent = { m_castRadius, 0.5f * m_castRadius, 0.25f * m_castRadius };
box = b3MakeTransformedBoxHull( extent.x, extent.y, extent.z, { m_origin, b3Quat_identity } );
b3Transform boxXf = { b3Vec3_zero, b3Quat_identity };
box = b3MakeTransformedBoxHull( extent.x, extent.y, extent.z, boxXf );
proxy.points = box.boxPoints;
proxy.count = box.base.vertexCount;
}
@ -694,23 +692,23 @@ public:
}
else
{
b3World_CastShape( m_worldId, &proxy, m_translation, b3DefaultQueryFilter(), modeFcn, &m_castContext );
b3World_CastShape( m_worldId, m_origin, &proxy, m_translation, b3DefaultQueryFilter(), modeFcn, &m_castContext );
}
if ( m_castContext.count > 0 )
{
assert( m_castContext.count <= 3 );
b3HexColor colors[3] = { b3_colorRed, b3_colorGreen, b3_colorBlue };
DrawLine( m_origin, m_origin + m_translation, MakeColor( color2 ) );
DrawLine( m_origin, b3OffsetPos( m_origin, m_translation ), MakeColor( color2 ) );
for ( int i = 0; i < m_castContext.count; ++i )
{
b3Vec3 point = m_castContext.points[i];
b3Pos point = m_castContext.points[i];
b3Vec3 normal = m_castContext.normals[i];
DrawPoint( point, 10.0f, MakeColor( colors[i] ) );
b3Vec3 head = point + 0.5f * normal;
b3Pos head = b3OffsetPos( point, 0.5f * normal );
b3Transform transform = { m_castContext.fractions[i] * m_translation, b3Quat_identity };
b3WorldTransform transform = { m_origin + m_castContext.fractions[i] * m_translation, b3Quat_identity };
if ( m_castType == e_rayCast )
{
@ -735,9 +733,9 @@ public:
}
else
{
DrawLine( m_origin, m_origin + m_translation, MakeColor( color2 ) );
DrawLine( m_origin, b3OffsetPos( m_origin, m_translation ), MakeColor( color2 ) );
b3Transform transform = { m_translation, b3Quat_identity };
b3WorldTransform transform = { m_origin + m_translation, b3Quat_identity };
if ( m_castType == e_sphereCast )
{
@ -772,7 +770,7 @@ public:
float m_castRadius;
bool m_initialOverlap;
b3Vec3 m_origin;
b3Pos m_origin;
b3Vec3 m_translation;
CastContext m_castContext;
@ -793,7 +791,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 20.0f );
m_camera->SetView( 45.0f, 30.0f, 20.0f, b3Pos_zero );
}
m_mesh = b3CreateBoxMesh( { 0.0f, 0.0f, 0.0f }, { 0.5f, 0.5f, 0.5f }, true );
@ -832,8 +830,10 @@ public:
b3Shape_SetMesh( m_meshShapeId, m_mesh, m_scale );
}
ImGui::SliderFloat( "Start Y", &m_start.y, -2.0f, 2.0f, "%.1f" );
ImGui::SliderFloat( "Start Z", &m_start.z, -2.0f, 2.0f, "%.1f" );
b3Vec3 delta = b3SubPos( m_start, b3Pos_zero );
ImGui::SliderFloat( "Start Y", &delta.y, -2.0f, 2.0f, "%.1f" );
ImGui::SliderFloat( "Start Z", &delta.z, -2.0f, 2.0f, "%.1f" );
m_start = b3OffsetPos( b3Pos_zero, delta );
ImGui::Checkbox( "sphere Cast", &m_sphereCast );
@ -842,33 +842,34 @@ public:
void Step() override
{
b3Vec3 rayOrigin = m_start;
b3Pos rayOrigin = m_start;
b3Vec3 rayTranslation = { 4.0f, 0.0f, 0.0f };
DrawPoint( rayOrigin, 8.0f, MakeColor( b3_colorGreen ) );
DrawPoint( rayOrigin + rayTranslation, 8.0f, MakeColor( b3_colorRed ) );
DrawLine( rayOrigin, rayOrigin + rayTranslation, MakeColor( b3_colorWhite ) );
DrawPoint( b3OffsetPos( rayOrigin, rayTranslation ), 8.0f, MakeColor( b3_colorRed ) );
DrawLine( rayOrigin, b3OffsetPos( rayOrigin, rayTranslation ), MakeColor( b3_colorWhite ) );
if ( m_sphereCast )
{
b3Sphere sphere = { rayOrigin, 0.25f };
b3Sphere sphere = { b3Vec3_zero, 0.25f };
CastContext context = {};
b3ShapeProxy proxy = { &sphere.center, 1, sphere.radius };
b3World_CastShape( m_worldId, &proxy, rayTranslation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
b3World_CastShape( m_worldId, m_start, &proxy, rayTranslation, b3DefaultQueryFilter(), RayCastClosestCallback,
&context );
if ( context.count > 0 )
{
b3Transform transform = { context.fractions[0] * rayTranslation, b3Quat_identity };
b3WorldTransform transform = b3MakeWorldTransform( { context.fractions[0] * rayTranslation, b3Quat_identity } );
DrawSolidSphere( transform, sphere, MakeColor( b3_colorYellow ) );
b3Vec3 point = context.points[0];
DrawLine( point, point + 0.5f * context.normals[0], MakeColor( b3_colorGreen ) );
b3Pos point = context.points[0];
DrawLine( point, b3OffsetPos( point, 0.5f * context.normals[0] ), MakeColor( b3_colorGreen ) );
DrawPoint( point, 5.0f, MakeColor( b3_colorYellow ) );
}
else
{
b3Transform transform = { rayTranslation, b3Quat_identity };
b3WorldTransform transform = b3MakeWorldTransform( { rayTranslation, b3Quat_identity } );
DrawSolidSphere( transform, sphere, MakeColor( b3_colorGray ) );
}
}
@ -877,9 +878,8 @@ public:
b3RayResult result = b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, b3DefaultQueryFilter() );
if ( result.hit )
{
b3Vec3 point = result.point;
DrawLine( point, point + 0.5f * result.normal, MakeColor( b3_colorGreen ) );
DrawPoint( point, 5.0f, MakeColor( b3_colorYellow ) );
DrawLine( result.point, b3OffsetPos( result.point, 0.5f * result.normal ), MakeColor( b3_colorGreen ) );
DrawPoint( result.point, 5.0f, MakeColor( b3_colorYellow ) );
}
}
}
@ -888,7 +888,7 @@ public:
b3MeshData* m_mesh;
b3BodyId m_meshBodyId;
b3ShapeId m_meshShapeId;
b3Vec3 m_start;
b3Pos m_start;
bool m_sphereCast;
};
@ -962,25 +962,27 @@ public:
CastContext context = {};
context.initialOverlap = m_initialOverlap;
b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
b3World_CastShape( m_worldId, b3Pos_zero, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback,
&context );
DrawSolidSphere( b3Transform_identity, sphere, MakeColor( b3_colorGreen ) );
DrawSolidSphere( b3WorldTransform_identity, sphere, MakeColor( b3_colorGreen ) );
if ( context.count > 0 )
{
b3Vec3 point = context.points[0];
b3Pos point = context.points[0];
b3Vec3 normal = context.normals[0];
float fraction = context.fractions[0];
// final position with overlap resolution
DrawSolidSphere( { fraction * translation, b3Quat_identity }, sphere, MakeColor( b3_colorRed ) );
DrawSolidSphere( b3MakeWorldTransform( { fraction * translation, b3Quat_identity } ), sphere,
MakeColor( b3_colorRed ) );
DrawPoint( point, 2.0f, MakeColor( b3_colorRed ) );
DrawLine( point, point + 0.2f * normal, MakeColor( b3_colorYellow ) );
DrawLine( point, b3OffsetPos( point, 0.2f * normal ), MakeColor( b3_colorYellow ) );
}
else
{
DrawSolidSphere( { translation, b3Quat_identity }, sphere, MakeColor( b3_colorGray ) );
DrawSolidSphere( b3MakeWorldTransform( { translation, b3Quat_identity } ), sphere, MakeColor( b3_colorGray ) );
}
}
}
@ -999,24 +1001,26 @@ public:
CastContext context = {};
context.initialOverlap = m_initialOverlap;
b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
b3World_CastShape( m_worldId, b3Pos_zero, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback,
&context );
DrawSolidCapsule( b3Transform_identity, capsule, MakeColor( b3_colorGreen ) );
DrawSolidCapsule( b3WorldTransform_identity, capsule, MakeColor( b3_colorGreen ) );
if ( context.count > 0 )
{
b3Vec3 point = context.points[0];
b3Pos point = context.points[0];
b3Vec3 normal = context.normals[0];
float fraction = context.fractions[0];
DrawSolidCapsule( { fraction * translation, b3Quat_identity }, capsule, MakeColor( b3_colorRed ) );
DrawSolidCapsule( b3MakeWorldTransform( { fraction * translation, b3Quat_identity } ), capsule,
MakeColor( b3_colorRed ) );
DrawPoint( point, 2.0f, MakeColor( b3_colorRed ) );
DrawLine( point, point + 0.2f * normal, MakeColor( b3_colorYellow ) );
DrawLine( point, b3OffsetPos( point, 0.2f * normal ), MakeColor( b3_colorYellow ) );
}
else
{
DrawSolidCapsule( { translation, b3Quat_identity }, capsule, MakeColor( b3_colorGray ) );
DrawSolidCapsule( b3MakeWorldTransform( { translation, b3Quat_identity } ), capsule, MakeColor( b3_colorGray ) );
}
}
}
@ -1042,25 +1046,27 @@ public:
CastContext context = {};
context.initialOverlap = m_initialOverlap;
b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
b3World_CastShape( m_worldId, b3Pos_zero, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback,
&context );
DrawHull( b3Transform_identity, &box.base, MakeColor( b3_colorGreen ) );
DrawHull( b3WorldTransform_identity, &box.base, MakeColor( b3_colorGreen ) );
if ( context.count > 0 )
{
b3Vec3 point = context.points[0];
b3Pos point = context.points[0];
b3Vec3 normal = context.normals[0];
float fraction = context.fractions[0];
// final position with overlap resolution
DrawHull( { fraction * translation, b3Quat_identity }, &box.base, MakeColor( b3_colorRed ) );
DrawHull( b3MakeWorldTransform( { fraction * translation, b3Quat_identity } ), &box.base,
MakeColor( b3_colorRed ) );
DrawPoint( point, 2.0f, MakeColor( b3_colorRed ) );
DrawLine( point, point + 0.2f * normal, MakeColor( b3_colorYellow ) );
DrawLine( point, b3OffsetPos( point, 0.2f * normal ), MakeColor( b3_colorYellow ) );
}
else
{
DrawHull( { translation, b3Quat_identity }, &box.base, MakeColor( b3_colorGray ) );
DrawHull( b3MakeWorldTransform( { translation, b3Quat_identity } ), &box.base, MakeColor( b3_colorGray ) );
}
}
}
@ -1083,7 +1089,7 @@ public:
DrawTextLine( "Shift + LMB and drag to shift start position" );
DrawGroundGrid( 10 );
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
}
void MouseDown( b3Vec2 p, int button, int modifiers ) override
@ -1246,8 +1252,8 @@ public:
b3Sphere sphere = { { -6.0f + 3.0f * i, 3.0f, -5.0f + m_castOffset }, 0.3f };
b3ShapeProxy proxy = { &sphere.center, 1, sphere.radius };
b3HexColor color = b3_colorGreen;
b3World_OverlapShape( m_worldId, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color );
DrawSolidSphere( b3Transform_identity, sphere, MakeColor( color ) );
b3World_OverlapShape( m_worldId, b3Pos_zero, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color );
DrawSolidSphere( b3WorldTransform_identity, sphere, MakeColor( color ) );
}
}
@ -1262,8 +1268,8 @@ public:
capsule.center2 += offset;
b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius };
b3HexColor color = b3_colorGreen;
b3World_OverlapShape( m_worldId, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color );
DrawSolidCapsule( b3Transform_identity, capsule, MakeColor( color ) );
b3World_OverlapShape( m_worldId, b3Pos_zero, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color );
DrawSolidCapsule( b3WorldTransform_identity, capsule, MakeColor( color ) );
}
}
@ -1275,8 +1281,8 @@ public:
b3BoxHull box = b3MakeTransformedBoxHull( 0.3f, 0.3f, 0.3f, transform );
b3ShapeProxy proxy = { box.boxPoints, box.base.vertexCount, 0.0f };
b3HexColor color = b3_colorGreen;
b3World_OverlapShape( m_worldId, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color );
DrawHull( b3Transform_identity, &box.base, MakeColor( color ) );
b3World_OverlapShape( m_worldId, b3Pos_zero, &proxy, b3DefaultQueryFilter(), OverlapResultFcn, &color );
DrawHull( b3WorldTransform_identity, &box.base, MakeColor( color ) );
}
}
@ -1287,9 +1293,9 @@ public:
DrawTextLine( "Shift + LMB and drag to move shapes" );
DrawGroundGrid( 10 );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisX, MakeColor( b3_colorRed ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisY, MakeColor( b3_colorGreen ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisZ, MakeColor( b3_colorBlue ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisX ), MakeColor( b3_colorRed ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisY ), MakeColor( b3_colorGreen ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisZ ), MakeColor( b3_colorBlue ) );
}
void MouseDown( b3Vec2 p, int button, int modifiers ) override
@ -1382,7 +1388,7 @@ public:
{
Sample::Render();
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
}
bool HasSolverControls() const override
@ -1416,19 +1422,19 @@ public:
b3ShapeProxy proxy = { &capsule.center1, 2, capsule.radius };
DrawSolidCapsule( b3Transform_identity, capsule, MakeColor( b3_colorGreen ) );
DrawSolidCapsule( b3WorldTransform_identity, capsule, MakeColor( b3_colorGreen ) );
b3World_CastShape( m_worldId, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
b3World_CastShape( m_worldId, b3Pos_zero, &proxy, translation, b3DefaultQueryFilter(), RayCastClosestCallback, &context );
float fraction = context.count > 0 ? context.fractions[0] : 1.0f;
b3Transform shapeEnd = { fraction * translation, b3Quat_identity };
b3WorldTransform shapeEnd = b3MakeWorldTransform( { fraction * translation, b3Quat_identity } );
DrawSolidCapsule( shapeEnd, capsule, MakeColor( context.count > 0 ? b3_colorRed : b3_colorGreen ) );
if ( context.count > 0 )
{
b3Vec3 p = context.points[0];
b3Pos p = context.points[0];
b3Vec3 n = context.normals[0];
DrawLine( p, p + 0.5f * n, MakeColor( b3_colorAliceBlue ) );
DrawLine( p, b3OffsetPos( p, 0.5f * n ), MakeColor( b3_colorAliceBlue ) );
DrawPoint( p, 8.0f, MakeColor( b3_colorAliceBlue ) );
}
}
@ -1510,35 +1516,34 @@ public:
Sample::Render();
DrawGroundGrid( 10 );
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
b3ShapeCastPairInput input;
input.proxyA = { m_triangle, 3, 0.0f };
// input.proxyB = { m_points, 8, 0.0f };
input.proxyB = { &m_capsule.center1, 2, m_capsule.radius };
input.transformA = b3Transform_identity;
input.transformB = m_transform;
input.transform = m_transform;
input.translationB = m_translation;
input.maxFraction = 0.970617533;
input.canEncroach = false;
b3CastOutput output = b3ShapeCast( &input );
DrawTriangle( m_triangle[0], m_triangle[1], m_triangle[2], MakeColor( b3_colorCyan ) );
DrawTriangle( b3WorldTransform_identity, m_triangle[0], m_triangle[1], m_triangle[2], MakeColor( b3_colorCyan ) );
// DrawHull( m_scene, m_transform, m_box, b3_colorGreen, false );
DrawSolidCapsule( m_transform, m_capsule, MakeColor( b3_colorGreen ) );
DrawSolidCapsule( b3MakeWorldTransform( m_transform ), m_capsule, MakeColor( b3_colorGreen ) );
if ( output.hit )
{
// final position with overlap resolution
b3Vec3 shapeEnd = m_transform.p + output.fraction * m_translation;
// DrawHull( m_scene, { shapeEnd, m_transform.q }, m_box, b3_colorRed, false );
DrawSolidCapsule( { shapeEnd, m_transform.q }, m_capsule, MakeColor( b3_colorRed ) );
DrawSolidCapsule( b3MakeWorldTransform( { shapeEnd, m_transform.q } ), m_capsule, MakeColor( b3_colorRed ) );
}
b3Vec3 shapeEnd = m_transform.p + m_translation;
// DrawHull( m_scene, { shapeEnd, m_transform.q }, m_box, b3_colorGray, false );
DrawSolidCapsule( { shapeEnd, m_transform.q }, m_capsule, MakeColor( b3_colorGray ) );
DrawSolidCapsule( b3MakeWorldTransform( { shapeEnd, m_transform.q } ), m_capsule, MakeColor( b3_colorGray ) );
}
static Sample* Create( SampleContext* context )
@ -1596,6 +1601,7 @@ public:
// .q = b3Quat_identity,
// };
m_transformA = b3WorldTransform_identity;
m_transformB = {
.p = { -1.64657831e-06, 1.00989532471, 0.0f },
.q = { { 0.00000000, 0.00000000, 0.00494779600 }, 0.999987781 },
@ -1619,7 +1625,7 @@ public:
Sample::Render();
DrawGroundGrid( 10 );
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
}
bool HasSolverControls() const override
@ -1702,8 +1708,7 @@ public:
b3DistanceInput input;
input.proxyA = { m_boxA.boxPoints, 8, 0.0f };
input.proxyB = { m_boxB.boxPoints, 8, 0.0f };
input.transformA = b3Transform_identity;
input.transformB = m_transformB;
input.transform = b3InvMulWorldTransforms( m_transformA, m_transformB );
input.useRadii = false;
b3SimplexCache cache = {};
@ -1713,21 +1718,23 @@ public:
// DrawFace( m_scene, m_triangle[0], m_triangle[1], m_triangle[2], b3_colorCyan );
// DrawHull( m_scene, m_transformB, m_box, b3_colorGreen, false );
DrawHull( b3Transform_identity, &m_boxA.base, MakeColor( b3_colorGreen ) );
DrawHull( m_transformA, &m_boxA.base, MakeColor( b3_colorGreen ) );
DrawHull( m_transformB, &m_boxB.base, MakeColor( b3_colorCyan ) );
for ( int i = 0; i < m_boxB.base.vertexCount; ++i )
{
b3Vec3 p = b3TransformPoint( m_transformB, m_boxB.boxPoints[i] );
DrawWorldString( p, MakeColor( b3_colorAliceBlue ), " %d", i );
b3Pos p = b3TransformWorldPoint( m_transformB, m_boxB.boxPoints[i] );
DrawString3D( p, MakeColor( b3_colorAliceBlue ), " %d", i );
}
DrawPoint( output.pointA, 5.0f, MakeColor( b3_colorWhite ) );
DrawPoint( output.pointB, 5.0f, MakeColor( b3_colorWhite ) );
DrawLine( output.pointA, output.pointA + 1.0f * output.normal, MakeColor( b3_colorWhite ) );
b3Pos pA = b3TransformWorldPoint( m_transformA, output.pointA );
b3Pos pB = b3TransformWorldPoint( m_transformA, output.pointB );
DrawPoint( pA, 5.0f, MakeColor( b3_colorWhite ) );
DrawPoint( pB, 5.0f, MakeColor( b3_colorWhite ) );
b3Vec3 normal = b3RotateVector( m_transformA.q, output.normal );
DrawLine( pA, b3OffsetPos( pA, 1.0f * normal ), MakeColor( b3_colorWhite ) );
b3Vec3 n = output.normal;
DrawTextLine( "distance = %g, normal = %g, %g, %g", output.distance, n.x, n.y, n.z );
DrawTextLine( "distance = %g, normal = %g, %g, %g", output.distance, normal.x, normal.y, normal.z );
B3_ASSERT( output.distance > 0.0f && b3IsNormalized( output.normal ) );
@ -1738,15 +1745,17 @@ public:
b3Vec3 v1, v2;
BuildWitnessPoints( simplex, v1, v2 );
DrawPoint( v1, 10.0f, MakeColor( b3_colorGreen ) );
DrawPoint( v2, 10.0f, MakeColor( b3_colorGreen ) );
b3Pos wv1 = b3TransformWorldPoint( m_transformA, v1 );
b3Pos wv2 = b3TransformWorldPoint( m_transformA, v2 );
DrawPoint( wv1, 10.0f, MakeColor( b3_colorGreen ) );
DrawPoint( wv2, 10.0f, MakeColor( b3_colorGreen ) );
for ( int i = 0; i < simplex->count; ++i )
{
v1 = simplex->vertices[i].wA;
v2 = simplex->vertices[i].wB;
DrawPoint( v1, 5.0f, MakeColor( b3_colorRed ) );
DrawPoint( v2, 5.0f, MakeColor( b3_colorRed ) );
b3Pos wA = b3TransformWorldPoint( m_transformA, simplex->vertices[i].wA );
b3Pos wB = b3TransformWorldPoint( m_transformA, simplex->vertices[i].wB );
DrawPoint( wA, 5.0f, MakeColor( b3_colorRed ) );
DrawPoint( wB, 5.0f, MakeColor( b3_colorRed ) );
}
b3Vec3 p = GetClosestPoint( simplex );
@ -1795,7 +1804,8 @@ public:
return new DistanceDebug( context );
}
b3Transform m_transformB;
b3WorldTransform m_transformA;
b3WorldTransform m_transformB;
b3HullData* m_box;
b3BoxHull m_boxA;
b3BoxHull m_boxB;
@ -1824,28 +1834,20 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( -90.0f, 0.0f, 10.0f, { 0.0f, 0.0f, 0.0f } );
m_camera->SetView( -45.0f, 10.0f, 5.0f, { 0.0f, 0.0f, 0.0f } );
}
m_point = b3Vec3_zero;
m_segment[0] = { -0.5f, 0.0f };
m_segment[1] = { 0.5f, 0.0f };
m_segment[0] = { -0.5f, 0.0f, 0.0f };
m_segment[1] = { 0.5f, 0.0f, 0.0f };
m_triangle[0] = { -0.5f, 0.0f };
m_triangle[1] = { 0.5f, 0.0f };
m_triangle[2] = { 0.0f, 1.0f };
m_box = b3MakeBoxHull( 0.25f, 0.5f, 1.0f );
m_triangle[0] = { -1.5f, 0.0f, 0.0f };
m_triangle[1] = { 1.5f, 0.0f, 0.0f };
m_triangle[2] = { 0.0f, 0.0f, 2.0f };
m_box = b3MakeBoxHull( 0.125f, 0.25f, 0.5f );
m_triangle[0] = { 0.0f, 0.0f, -4.0f };
m_triangle[1] = { 0.0f, 0.0f, -8.0f };
m_triangle[2] = { -4.0f, 0.0f, -8.0f };
m_box = b3MakeBoxHull( 0.02f, 0.2f, 0.04f );
m_transform = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity };
m_transform = {
.p = { 0.00000000, 0.796873450, -4.50000000 },
.q = { { -0.964603841, -0.0856034234, 0.248849452 }, 0.0168983955 },
};
m_transformA = b3WorldTransform_identity;
m_transformB = { { 0.0f, 1.0f, 0.0f }, b3Quat_identity };
m_cache = {};
m_simplexCount = 0;
@ -1903,7 +1905,7 @@ public:
return proxy;
}
void DrawShape( ShapeType type, b3Transform transform, float radius, b3HexColor color )
void DrawShape( ShapeType type, b3WorldTransform transform, float radius, b3HexColor color )
{
switch ( type )
{
@ -1919,7 +1921,7 @@ public:
}
else
{
b3Vec3 p = b3TransformPoint( transform, m_point );
b3Pos p = b3TransformWorldPoint( transform, m_point );
DrawPoint( p, 5.0f, MakeColor( color ) );
}
}
@ -1939,8 +1941,8 @@ public:
}
else
{
b3Vec3 p1 = b3TransformPoint( transform, m_segment[0] );
b3Vec3 p2 = b3TransformPoint( transform, m_segment[1] );
b3Pos p1 = b3TransformWorldPoint( transform, m_segment[0] );
b3Pos p2 = b3TransformWorldPoint( transform, m_segment[1] );
DrawLine( p1, p2, MakeColor( color ) );
}
}
@ -1948,9 +1950,9 @@ public:
case e_triangle:
{
b3Vec3 p1 = b3TransformPoint( transform, m_triangle[0] );
b3Vec3 p2 = b3TransformPoint( transform, m_triangle[1] );
b3Vec3 p3 = b3TransformPoint( transform, m_triangle[2] );
b3Pos p1 = b3TransformWorldPoint( transform, m_triangle[0] );
b3Pos p2 = b3TransformWorldPoint( transform, m_triangle[1] );
b3Pos p3 = b3TransformWorldPoint( transform, m_triangle[2] );
DrawLine( p1, p2, MakeColor( color ) );
DrawLine( p2, p3, MakeColor( color ) );
DrawLine( p3, p1, MakeColor( color ) );
@ -2030,18 +2032,18 @@ public:
b3Vec3 d = b3Normalize( ray.translation );
// Intersect with plane going through origin
// p = c + alpha * f
// dot(p, f) = 0
// dot(c, f) + alpha = 0
// p = c - dot(c, f) * f
m_dragStart = b3MulSub( ray.origin, b3Dot( ray.origin, d ), d );
m_basePosition = m_transform.p;
// p = c + alpha * d
// dot(p, d) = 0
// dot(c, d) + alpha = 0
// p = c - dot(c, d) * d
m_dragStart = ray.origin - b3Dot( b3SubPos( ray.origin, b3Pos_zero ), d ) * d;
m_basePosition = m_transformB.p;
}
else if ( mods == MOD_SHIFT && m_dragging == false )
{
m_rotating = true;
m_rotateStart = ps.x;
m_baseQuat = m_transform.q;
m_baseQuat = m_transformB.q;
}
}
}
@ -2061,8 +2063,8 @@ public:
{
PickRay ray = m_camera->BuildPickRay( ps.x, ps.y );
b3Vec3 d = b3Normalize( ray.translation );
b3Vec3 p = b3MulSub( ray.origin, b3Dot( ray.origin, d ), d );
m_transform.p = m_basePosition + ( p - m_dragStart );
b3Pos p = ray.origin - b3Dot( b3SubPos( ray.origin, b3Pos_zero ), d ) * d;
m_transformB.p = m_basePosition + ( p - m_dragStart );
}
else if ( m_rotating && m_camera->m_width > 0.0f )
{
@ -2070,20 +2072,10 @@ public:
float angle = b3ClampFloat( 2.0f * dx, -B3_PI, B3_PI );
b3Vec3 axis = m_camera->GetForward();
b3Quat deltaQuat = b3MakeQuatFromAxisAngle( axis, angle );
m_transform.q = b3MulQuat( deltaQuat, m_baseQuat );
m_transformB.q = b3MulQuat( deltaQuat, m_baseQuat );
}
}
static b3Vec3 Weight2( float a1, b3Vec3 w1, float a2, b3Vec3 w2 )
{
return { a1 * w1.x + a2 * w2.x, a1 * w1.y + a2 * w2.y };
}
static b3Vec3 Weight3( float a1, b3Vec3 w1, float a2, b3Vec3 w2, float a3, b3Vec3 w3 )
{
return { a1 * w1.x + a2 * w2.x + a3 * w3.x, a1 * w1.y + a2 * w2.y + a3 * w3.y };
}
static void ComputeWitnessPoints( b3Vec3* a, b3Vec3* b, const b3Simplex* s )
{
const b3SimplexVertex* vs = s->vertices;
@ -2120,7 +2112,7 @@ public:
void Render() override
{
DrawAxes( b3Transform_identity, 0.5f );
DrawAxes( b3WorldTransform_identity, 0.5f );
Sample::Render();
}
@ -2129,8 +2121,7 @@ public:
b3DistanceInput input;
input.proxyA = m_proxyA;
input.proxyB = m_proxyB;
input.transformA = b3Transform_identity;
input.transformB = m_transform;
input.transform = b3InvMulWorldTransforms( m_transformA, m_transformB );
input.useRadii = m_radiusA > 0.0f || m_radiusB > 0.0f;
if ( m_useCache == false )
@ -2154,8 +2145,8 @@ public:
m_simplexCount = output.simplexCount;
DrawShape( m_typeA, b3Transform_identity, m_radiusA, b3_colorCyan );
DrawShape( m_typeB, m_transform, m_radiusB, b3_colorBisque );
DrawShape( m_typeA, b3WorldTransform_identity, m_radiusA, b3_colorCyan );
DrawShape( m_typeB, m_transformB, m_radiusB, b3_colorBisque );
if ( m_drawSimplex )
{
@ -2168,9 +2159,11 @@ public:
b3Vec3 pointA, pointB;
ComputeWitnessPoints( &pointA, &pointB, simplex );
DrawLine( pointA, pointB, MakeColor( b3_colorWhite ) );
DrawPoint( pointA, 10.0f, MakeColor( b3_colorLightGreen ) );
DrawPoint( pointB, 10.0f, MakeColor( b3_colorLightBlue ) );
b3Pos pA = b3TransformWorldPoint( m_transformA, pointA );
b3Pos pB = b3TransformWorldPoint( m_transformA, pointB );
DrawLine( pA, pB, MakeColor( b3_colorWhite ) );
DrawPoint( pA, 10.0f, MakeColor( b3_colorLightGreen ) );
DrawPoint( pB, 10.0f, MakeColor( b3_colorLightBlue ) );
}
b3HexColor colors[3] = { b3_colorRed, b3_colorGreen, b3_colorBlue };
@ -2178,30 +2171,36 @@ public:
for ( int i = 0; i < simplex->count; ++i )
{
b3SimplexVertex* vertex = vertices + i;
DrawPoint( vertex->wA, 10.0f, MakeColor( colors[i] ) );
DrawPoint( vertex->wB, 10.0f, MakeColor( colors[i] ) );
b3Pos wA = b3TransformWorldPoint( m_transformA, vertex->wA );
b3Pos wB = b3TransformWorldPoint( m_transformA, vertex->wB );
DrawPoint( wA, 10.0f, MakeColor( colors[i] ) );
DrawPoint( wB, 10.0f, MakeColor( colors[i] ) );
}
}
else
{
DrawLine( output.pointA, output.pointB, MakeColor( b3_colorDimGray ) );
DrawPoint( output.pointA, 10.0f, MakeColor( b3_colorLightGreen ) );
DrawPoint( output.pointB, 10.0f, MakeColor( b3_colorLightBlue ) );
DrawLine( output.pointA, output.pointA + 0.5f * output.normal, MakeColor( b3_colorYellow ) );
b3Pos pA = b3TransformWorldPoint( m_transformA, output.pointA );
b3Pos pB = b3TransformWorldPoint( m_transformA, output.pointB );
DrawLine( pA, pB, MakeColor( b3_colorDimGray ) );
DrawPoint( pA, 10.0f, MakeColor( b3_colorLightGreen ) );
DrawPoint( pB, 10.0f, MakeColor( b3_colorLightBlue ) );
b3Vec3 normal = b3RotateVector( m_transformA.q, output.normal );
DrawLine( pA, b3OffsetPos( pA, 0.5f * normal ), MakeColor( b3_colorYellow ) );
}
if ( m_showIndices )
{
for ( int i = 0; i < m_proxyA.count; ++i )
{
b3Vec3 p = m_proxyA.points[i];
DrawWorldString( p, MakeColor( b3_colorWhite ), " %d", i );
b3Pos p = b3TransformWorldPoint( m_transformA, m_proxyA.points[i] );
DrawString3D( p, MakeColor( b3_colorWhite ), " %d", i );
}
for ( int i = 0; i < m_proxyB.count; ++i )
{
b3Vec3 p = b3TransformPoint( m_transform, m_proxyB.points[i] );
DrawWorldString( p, MakeColor( b3_colorWhite ), " %d", i );
b3Pos p = b3TransformWorldPoint( m_transformB, m_proxyB.points[i] );
DrawString3D( p, MakeColor( b3_colorWhite ), " %d", i );
}
}
@ -2249,10 +2248,11 @@ public:
int m_simplexCount;
int m_simplexIndex;
b3Transform m_transform;
b3WorldTransform m_transformA;
b3WorldTransform m_transformB;
b3Vec3 m_basePosition;
b3Vec3 m_dragStart;
b3Pos m_basePosition;
b3Pos m_dragStart;
b3Quat m_baseQuat;
float m_rotateStart;
@ -2336,7 +2336,7 @@ public:
return proxy;
}
void DrawShape( ShapeType type, b3Transform transform, b3HexColor color )
void DrawShape( ShapeType type, b3WorldTransform transform, b3HexColor color )
{
switch ( type )
{
@ -2344,9 +2344,8 @@ public:
{
DrawSolidCapsule( transform, m_capsule, MakeColor( color ) );
b3Vec3 center = b3Lerp( m_capsule.center1, m_capsule.center2, 0.5f );
center = b3TransformPoint( transform, center );
b3Transform xf = {
.p = center,
b3WorldTransform xf = {
.p = b3TransformWorldPoint( transform, center ),
.q = transform.q,
};
DrawAxes( xf, 0.025f );
@ -2355,15 +2354,15 @@ public:
case e_triangle:
{
b3Vec3 p1 = b3TransformPoint( transform, m_triangle[0] );
b3Vec3 p2 = b3TransformPoint( transform, m_triangle[1] );
b3Vec3 p3 = b3TransformPoint( transform, m_triangle[2] );
b3Pos p1 = b3TransformWorldPoint( transform, m_triangle[0] );
b3Pos p2 = b3TransformWorldPoint( transform, m_triangle[1] );
b3Pos p3 = b3TransformWorldPoint( transform, m_triangle[2] );
DrawLine( p1, p2, MakeColor( color ) );
DrawLine( p2, p3, MakeColor( color ) );
DrawLine( p3, p1, MakeColor( color ) );
DrawWorldString( p1, MakeColor( b3_colorWhite ), "0" );
DrawWorldString( p2, MakeColor( b3_colorWhite ), "1" );
DrawWorldString( p3, MakeColor( b3_colorWhite ), "2" );
DrawString3D( p1, MakeColor( b3_colorWhite ), "0" );
DrawString3D( p2, MakeColor( b3_colorWhite ), "1" );
DrawString3D( p3, MakeColor( b3_colorWhite ), "2" );
}
break;
@ -2403,7 +2402,7 @@ public:
void Render() override
{
DrawAxes( b3Transform_identity, 0.5f );
DrawAxes( b3WorldTransform_identity, 0.5f );
Sample::Render();
}
@ -2418,7 +2417,7 @@ public:
b3TOIOutput output = b3TimeOfImpact( &input );
DrawShape( m_typeA, b3Transform_identity, b3_colorCyan );
DrawShape( m_typeA, b3WorldTransform_identity, b3_colorCyan );
b3Transform transform1 = b3GetSweepTransform( &m_sweepB, 0.0f );
b3Transform transform2 = b3GetSweepTransform( &m_sweepB, 1.0f );
@ -2429,19 +2428,20 @@ public:
(void)b3GetAxisAngle( &angle, qr );
DrawTextLine( "angle = %g", 180.0f * angle / B3_PI );
DrawShape( m_typeB, transform1, b3_colorLightGreen );
DrawShape( m_typeB, transform2, b3_colorLightCoral );
DrawShape( m_typeB, b3MakeWorldTransform( transform1 ), b3_colorLightGreen );
DrawShape( m_typeB, b3MakeWorldTransform( transform2 ), b3_colorLightCoral );
if ( output.fraction < 1.0f )
{
b3Transform transformHit = b3GetSweepTransform( &m_sweepB, output.fraction );
DrawShape( m_typeB, transformHit, b3_colorLightCyan );
DrawShape( m_typeB, b3MakeWorldTransform( transformHit ), b3_colorLightCyan );
}
if ( output.state == b3_toiStateHit || output.state == b3_toiStateFailed )
{
DrawLine( output.point, output.point + 0.5f * output.normal, MakeColor( b3_colorDimGray ) );
DrawPoint( output.point, 10.0f, MakeColor( b3_colorLightGreen ) );
b3Pos p = b3ToPos( output.point );
DrawLine( p, b3OffsetPos( p, 0.5f * output.normal ), MakeColor( b3_colorDimGray ) );
DrawPoint( p, 10.0f, MakeColor( b3_colorLightGreen ) );
}
switch ( output.state )
@ -2538,21 +2538,22 @@ public:
Sample::Render();
DrawGroundGrid( 10 );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisX, MakeColor( b3_colorRed ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisY, MakeColor( b3_colorGreen ) );
DrawLine( b3Vec3_zero, 0.4f * b3Vec3_axisZ, MakeColor( b3_colorBlue ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisX ), MakeColor( b3_colorRed ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisY ), MakeColor( b3_colorGreen ) );
DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 0.4f * b3Vec3_axisZ ), MakeColor( b3_colorBlue ) );
b3BodyRayCastInput input;
input.origin = { -1.0f, 0.5f, 0.0f };
input.translation = { 2.0f, 0.0f, 0.0f };
input.filter = b3DefaultQueryFilter();
input.maxFraction = 1.0f;
b3Pos origin = { -1.0f, 0.5f, 0.0f };
b3Vec3 translation = { 2.0f, 0.0f, 0.0f };
b3QueryFilter filter = b3DefaultQueryFilter();
float maxFraction = 1.0f;
b3WorldTransform bodyTransform = b3WorldTransform_identity;
b3BodyCastResult result = b3Body_CastRay( m_bodyId, &input, b3Transform_identity );
b3BodyCastResult result = b3Body_CastRay( m_bodyId, origin, translation, filter, maxFraction, bodyTransform );
DrawLine( input.origin, input.origin + input.translation, MakeColor( b3_colorGray ) );
DrawPoint( input.origin, 4.0f, MakeColor( b3_colorGreen ) );
DrawPoint( input.origin + input.translation, 4.0f, MakeColor( b3_colorRed ) );
b3Pos rayEnd = origin + translation;
DrawLine( origin, rayEnd, MakeColor( b3_colorGray ) );
DrawPoint( origin, 4.0f, MakeColor( b3_colorGreen ) );
DrawPoint( rayEnd, 4.0f, MakeColor( b3_colorRed ) );
if ( result.hit )
{

View file

@ -1,10 +1,10 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "gfx/draw.h"
#include "human.h"
#include "mesh_loader.h"
#include "sample.h"
#include "gfx/draw.h"
#include "utils.h"
#include "box3d/box3d.h"
@ -19,7 +19,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Pos_zero );
}
{
@ -30,7 +30,7 @@ public:
b3Transform hullTransform;
hullTransform.p = { 1.0f, -0.125f * a, 0.0f };
hullTransform.q = b3MakeQuatFromAxisAngle( b3Normalize({ 1.0f, 0.0f, 1.0f }), 0.0f * B3_PI );
hullTransform.q = b3MakeQuatFromAxisAngle( b3Normalize( { 1.0f, 0.0f, 1.0f } ), 0.0f * B3_PI );
b3CompoundHullDef hullDef = {
.hull = &box.base,
@ -89,7 +89,7 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 1.0f );
DrawAxes( b3MakeWorldTransform( transform ), 1.0f );
int height = b3DynamicTree_GetHeight( &m_compound->tree );
DrawTextLine( "compound tree height = %d", height );
@ -113,7 +113,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Pos_zero );
}
float h = 10.0f;
@ -149,7 +149,7 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 1.0f );
DrawAxes( b3MakeWorldTransform( transform ), 1.0f );
int height = b3DynamicTree_GetHeight( &m_compound->tree );
DrawTextLine( "compound tree height = %d", height );
@ -174,7 +174,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Pos_zero );
}
float h = 10.0f;
@ -223,7 +223,7 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 1.0f );
DrawAxes( b3MakeWorldTransform( transform ), 1.0f );
int height = b3DynamicTree_GetHeight( &m_compound->tree );
DrawTextLine( "compound tree height = %d", height );
@ -248,7 +248,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Pos_zero );
}
{
@ -302,7 +302,7 @@ public:
hulls = nullptr;
}
//b3World_SetContactRecycleDistance( m_worldId, 0.0f );
// b3World_SetContactRecycleDistance( m_worldId, 0.0f );
{
b3BodyDef bodyDef = b3DefaultBodyDef();
@ -338,7 +338,7 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 1.0f );
DrawAxes( b3MakeWorldTransform( transform ), 1.0f );
DrawTextLine( "compound hull count = %d, mesh count = %d", m_compound->hullCount, m_compound->meshCount );
DrawTextLine( "compound byte count = %d", m_compound->byteCount );
@ -366,7 +366,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 45.0f, b3Pos_zero );
}
{
@ -455,7 +455,7 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 1.0f );
DrawAxes( b3MakeWorldTransform( transform ), 1.0f );
DrawTextLine( "compound instance count = %d, byte count = %d", m_compound->meshCount, m_compound->byteCount );
@ -492,7 +492,7 @@ public:
constexpr float a = 4.0f;
m_worldWidth = 2.0f * gridCount * a;
b3Vec3 position = { 0.0f, 10.0f, 0.0f };
b3Pos position = { 0.0f, 10.0f, 0.0f };
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 10.0f, 5.0f, position );
@ -645,7 +645,7 @@ public:
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = { -1.0f, -0.5f, 2.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( {0.0f, 1.0f, 0.0f}, -1.15f * B3_PI );
bodyDef.rotation = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, -1.15f * B3_PI );
b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
@ -686,7 +686,7 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 4.0f );
DrawAxes( b3MakeWorldTransform( transform ), 4.0f );
DrawTextLine( "surface type = %d", m_userMaterialId );
DrawTextLine( "compound capsules/hulls/meshes/sphere = %d / %d / %d / %d", m_compound->capsuleCount,
@ -710,11 +710,11 @@ public:
CastClosestContext context = {};
(void)b3World_CastRay( m_worldId, m_rayOrigin, translation, filter, CastClosestCallback, &context );
DrawLine( m_rayOrigin, m_rayOrigin + translation, MakeColor( b3_colorAliceBlue ) );
DrawLine( m_rayOrigin, b3OffsetPos( m_rayOrigin, translation ), MakeColor( b3_colorAliceBlue ) );
if ( context.hit )
{
b3Vec3 p1 = context.point;
b3Vec3 p2 = b3MulAdd( p1, 0.5f, context.normal );
b3Pos p1 = context.point;
b3Pos p2 = p1 + 0.5f * context.normal;
DrawLine( p1, p2, MakeColor( b3_colorYellow ) );
DrawPoint( p1, 8.0f, MakeColor( b3_colorLightCoral ) );
DrawTextLine( "ray hit triangle/child/material = %d / %d / %d", context.triangleIndex, context.childIndex,
@ -728,20 +728,20 @@ public:
{
CastClosestContext context = {};
b3Vec3 origin = b3Sub( m_rayOrigin, { 1.0f, 0.0f, 1.0f } );
b3ShapeProxy proxy = { &origin, 1, 0.25f };
b3World_CastShape( m_worldId, &proxy, translation, filter, CastClosestCallback, &context );
b3Pos origin = m_rayOrigin - b3Vec3{ 1.0f, 0.0f, 1.0f };
b3ShapeProxy proxy = { &b3Vec3_zero, 1, 0.25f };
b3World_CastShape( m_worldId, origin, &proxy, translation, filter, CastClosestCallback, &context );
DrawLine( origin, origin + translation, MakeColor( b3_colorAliceBlue ) );
DrawLine( origin, b3OffsetPos( origin, translation ), MakeColor( b3_colorAliceBlue ) );
if ( context.hit )
{
b3Vec3 position = b3MulAdd( origin, context.fraction, translation );
b3Vec3 p1 = context.point;
b3Vec3 p2 = b3MulAdd( p1, 0.5f, context.normal );
b3Pos position = b3OffsetPos( origin, context.fraction * translation );
b3Pos p1 = context.point;
b3Pos p2 = p1 + 0.5f * context.normal;
DrawLine( p1, p2, MakeColor( b3_colorYellow ) );
DrawPoint( p1, 8.0f, MakeColor( b3_colorLightCoral ) );
b3Sphere sphere = { position, 0.25f };
DrawSolidSphere( b3Transform_identity, sphere, MakeColor( b3_colorOrchid ) );
b3Sphere sphere = { b3Vec3_zero, 0.25f };
DrawSolidSphere( { position, b3Quat_identity }, sphere, MakeColor( b3_colorOrchid ) );
DrawTextLine( "shape hit triangle/child/material = %d / %d / %d", context.triangleIndex, context.childIndex,
context.materialId );
}
@ -753,13 +753,13 @@ public:
{
bool overlap = false;
b3Vec3 origin = { m_rayOrigin.x - 1.0f, 2.0f, m_rayOrigin.z - 1.0f };
b3ShapeProxy proxy = { &origin, 1, 0.3f };
b3World_OverlapShape( m_worldId, &proxy, filter, OverlapResultFcn, &overlap );
b3Pos origin = { m_rayOrigin.x - 1.0f, 2.0f, m_rayOrigin.z - 1.0f };
b3ShapeProxy proxy = { &b3Vec3_zero, 1, 0.3f };
b3World_OverlapShape( m_worldId, origin, &proxy, filter, OverlapResultFcn, &overlap );
b3HexColor color = overlap ? b3_colorDarkMagenta : b3_colorDarkSeaGreen;
b3Sphere sphere = { origin, 0.3f };
DrawSolidSphere( b3Transform_identity, sphere, MakeColor( color ) );
b3Sphere sphere = { b3Vec3_zero, 0.3f };
DrawSolidSphere( { origin, b3Quat_identity }, sphere, MakeColor( color ) );
}
if ( m_rayOrigin.x > 0.45f * m_worldWidth )
@ -792,7 +792,7 @@ public:
b3Compound* m_compound;
CharacterMover m_mover;
float m_worldWidth;
b3Vec3 m_rayOrigin;
b3Pos m_rayOrigin;
};
static int sampleVillage = RegisterSample( "Compound", "Village", Village::Create );

View file

@ -22,7 +22,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 30.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 30.0f, b3Pos_zero );
}
AddGroundBox( 40.0f );
@ -80,7 +80,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 45.0f, 50.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 45.0f, 50.0f, b3Pos_zero );
}
AddGroundBox( 10.0f );
@ -426,7 +426,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 20.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 20.0f, b3Pos_zero );
GetGuiDraw()->forceScale = 0.1f;
}
@ -650,7 +650,8 @@ public:
{
PickRay pickRay = m_camera->BuildPickRay( m_context->mouseX, m_context->mouseY );
b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() );
b3RayResult result =
b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() );
if ( result.hit )
{
@ -669,7 +670,7 @@ public:
continue;
}
b3Vec3 massCenter = b3Body_GetWorldCenterOfMass( bodyId );
b3Pos massCenter = b3Body_GetWorldCenterOfMass( bodyId );
if ( massCenter.y < -2.0f )
{
IndexPair pair = ConvertToPair( b3Body_GetUserData( bodyId ) );
@ -752,11 +753,11 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 20.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 20.0f, b3Pos_zero );
GetGuiDraw()->forceScale = 0.1f;
}
m_data = CreateMeshDrop( m_worldId );
m_data = CreateMeshDrop( m_worldId, b3Pos_zero );
m_failed = false;
}

View file

@ -16,7 +16,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Pos_zero );
}
m_data = CreateFallingRagdolls( m_worldId );

View file

@ -143,7 +143,7 @@ public:
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = origin;
bodyDef.position = b3OffsetPos( b3Pos_zero, origin );
b3BodyId prevBodyId = {};
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
@ -160,9 +160,9 @@ public:
{
b3Body_ApplyMassFromShapes( bodyId );
b3Vec3 center = b3Body_GetWorldCenterOfMass( bodyId );
b3Pos center = b3Body_GetWorldCenterOfMass( bodyId );
b3Vec3 omega = { 0.0f, 0.0f, -1.0f * velocityScale };
b3Vec3 v = b3Cross( omega, center - origin );
b3Vec3 v = b3Cross( omega, b3SubPos( center, b3OffsetPos( b3Pos_zero, origin ) ) );
b3Body_SetAngularVelocity( bodyId, omega );
b3Body_SetLinearVelocity( bodyId, v );
@ -206,20 +206,19 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 4.0f );
DrawAxes( b3MakeWorldTransform( transform ), 4.0f );
for ( int i = 0; i < m_eventCount; ++i )
{
// void DrawLine(Scene * Scene, b3Vector3 Vertex1, b3Vector3 Vertex2, b3Color Color);
// void DrawPoint(Scene * Scene, b3Vector3 Point, b3Color Color, float Size);
b3Vec3 p1 = m_events[i].point;
DrawPoint( m_events[i].point, 10.0f, MakeColor( b3_colorYellow ) );
b3Pos p1 = m_events[i].point;
b3Pos p2 = b3OffsetPos( p1, -m_events[i].approachSpeed * m_events[i].normal );
b3Vec3 p2 = p1 - m_events[i].approachSpeed * m_events[i].normal;
DrawPoint( p1, 10.0f, MakeColor( b3_colorYellow ) );
DrawLine( p1, p2, MakeColor( b3_colorYellow ) );
DrawWorldString( p1, MakeColor( b3_colorWhite ), "%.1f, %d", m_events[i].approachSpeed, m_events[i].userMaterialIdA );
DrawString3D( p1, MakeColor( b3_colorWhite ), "%.1f, %d", m_events[i].approachSpeed, m_events[i].userMaterialIdA );
}
DrawTextLine( "event count = %d", m_eventCount );
@ -260,7 +259,7 @@ public:
AddGroundBox( 40.0f );
b3BodyDef bodyDef = b3DefaultBodyDef();
b3Vec3 pivot = { 0.0f, 1.0f, 0.0f };
b3Pos pivot = { 0.0f, 1.0f, 0.0f };
bodyDef.type = b3_dynamicBody;
bodyDef.position = pivot;
bodyDef.name = "big box";
@ -273,9 +272,9 @@ public:
b3BoxHull dynamicBox = b3MakeTransformedBoxHull( 0.5f, 10.0f, 0.5f, { { 0.0f, 10.0f, 0.0f }, b3Quat_identity } );
b3CreateHullShape( m_bodyId, &shapeDef, &dynamicBox.base );
b3Vec3 center = b3Body_GetWorldCenterOfMass( m_bodyId );
b3Pos center = b3Body_GetWorldCenterOfMass( m_bodyId );
b3Vec3 r = pivot - center;
b3Vec3 r = b3SubPos( pivot, center );
float rr = b3LengthSquared( r );
if ( rr > 0.0f )
{
@ -378,13 +377,13 @@ public:
{
assert( index < e_count );
bodyDef.position = position;
bodyDef.position = b3OffsetPos( b3Pos_zero, position );
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
float length = 2.0f;
b3Vec3 pivot1 = { position.x, position.y + 1.0f + length, 0.0f };
b3Vec3 pivot2 = { position.x, position.y + 1.0f, 0.0f };
b3Pos pivot1 = { position.x, position.y + 1.0f + length, 0.0f };
b3Pos pivot2 = { position.x, position.y + 1.0f, 0.0f };
b3DistanceJointDef jointDef = b3DefaultDistanceJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = bodyId;
@ -433,11 +432,11 @@ public:
{
assert( index < e_count );
bodyDef.position = position;
bodyDef.position = b3OffsetPos( b3Pos_zero, position );
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f };
b3Pos pivot = { position.x - 1.0f, position.y, 0.0f };
b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = bodyId;
@ -457,11 +456,11 @@ public:
{
assert( index < e_count );
bodyDef.position = position;
bodyDef.position = b3OffsetPos( b3Pos_zero, position );
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f };
b3Pos pivot = { position.x - 1.0f, position.y, 0.0f };
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = bodyId;
@ -481,11 +480,11 @@ public:
{
assert( index < e_count );
bodyDef.position = position;
bodyDef.position = b3OffsetPos( b3Pos_zero, position );
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f };
b3Pos pivot = { position.x - 1.0f, position.y, 0.0f };
b3WeldJointDef jointDef = b3DefaultWeldJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = bodyId;
@ -636,7 +635,7 @@ public:
if ( B3_IS_NON_NULL( m_contactId ) && b3Contact_IsValid( m_contactId ) )
{
b3ContactData data = b3Contact_GetData( m_contactId );
b3Vec3 centerOfMass = b3Body_GetWorldCenterOfMass( b3Shape_GetBody( data.shapeIdA ) );
b3Pos centerOfMass = b3Body_GetWorldCenterOfMass( b3Shape_GetBody( data.shapeIdA ) );
for ( int i = 0; i < data.manifoldCount; ++i )
{
@ -644,12 +643,12 @@ public:
b3Vec3 normal = manifold->normal;
for ( int j = 0; j < manifold->pointCount; ++j )
{
const b3ManifoldPoint* manifoldPoint = manifold->points + i;
b3Vec3 p1 = manifoldPoint->anchorA + centerOfMass;
b3Vec3 p2 = p1 + manifoldPoint->totalNormalImpulse * normal;
const b3ManifoldPoint* manifoldPoint = manifold->points + j;
b3Pos p1 = b3OffsetPos( centerOfMass, manifoldPoint->anchorA );
b3Pos p2 = b3OffsetPos( p1, manifoldPoint->totalNormalImpulse * normal );
DrawLine( p1, p2, MakeColor( b3_colorCrimson ) );
DrawPoint( p1, 6.0f, MakeColor( b3_colorCrimson ) );
DrawWorldString( p1, MakeColor( b3_colorGray ), "%.2f", manifoldPoint->totalNormalImpulse );
DrawString3D( p1, MakeColor( b3_colorGray ), "%.2f", manifoldPoint->totalNormalImpulse );
}
}
}
@ -746,7 +745,7 @@ public:
b3Capsule capsule = { { 0.0f, 1.0f }, { 0.0f, 9.0f }, 0.1f };
m_dynamicSensorId = b3CreateCapsuleShape( m_dynamicBodyId, &shapeDef, &capsule );
b3Vec3 pivot = bodyDef.position + b3Vec3{ 0.0f, 6.0f, 0.0f };
b3Pos pivot = b3OffsetPos( bodyDef.position, b3Vec3{ 0.0f, 6.0f, 0.0f } );
b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = m_dynamicBodyId;
@ -822,15 +821,16 @@ public:
for ( int i = 0; i < count && m_transformCount < m_transformCapacity; ++i )
{
b3BodyId sensorBodyId = b3Shape_GetBody( sensorShapeId );
m_transforms[m_transformCount] = b3Body_GetTransform( sensorBodyId );
m_transforms[m_transformCount].p = b3Body_GetWorldCenterOfMass( sensorBodyId );
b3WorldTransform t = b3Body_GetTransform( sensorBodyId );
t.p = b3Body_GetWorldCenterOfMass( sensorBodyId );
m_transforms[m_transformCount] = t;
m_transformCount += 1;
}
}
void Step() override
{
b3Vec3 p = b3Body_GetPosition( m_kinematicBodyId );
b3Pos p = b3Body_GetPosition( m_kinematicBodyId );
if ( p.x > 1.0f )
{
b3Body_SetLinearVelocity( m_kinematicBodyId, { -0.5f, 0.0f } );
@ -891,7 +891,7 @@ public:
static constexpr int m_transformCapacity = 20;
int m_transformCount;
b3Transform m_transforms[m_transformCapacity];
b3WorldTransform m_transforms[m_transformCapacity];
bool m_isBullet;
int m_beginCount;

View file

@ -17,7 +17,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero );
}
m_hull = nullptr;
@ -112,10 +112,10 @@ public:
void Render() override
{
DrawHull( b3Transform_identity, m_hull, MakeColor( b3_colorYellow ) );
DrawHull( b3Transform_identity, &m_box.base, MakeColor( b3_colorCyan ) );
DrawHull( b3WorldTransform_identity, m_hull, MakeColor( b3_colorYellow ) );
DrawHull( b3WorldTransform_identity, &m_box.base, MakeColor( b3_colorCyan ) );
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
Sample::Render();
}
@ -143,7 +143,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero );
}
m_hull = nullptr;
@ -209,10 +209,10 @@ public:
{
if ( m_hull != nullptr )
{
DrawHull( b3Transform_identity, m_hull, MakeColor( b3_colorYellow ) );
DrawHull( b3WorldTransform_identity, m_hull, MakeColor( b3_colorYellow ) );
}
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
Sample::Render();
}
@ -244,7 +244,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero );
}
m_hull = nullptr;
@ -335,12 +335,12 @@ public:
{
if ( m_hull != nullptr )
{
DrawHull( b3Transform_identity, m_hull, MakeColor( b3_colorYellow ) );
DrawHull( b3WorldTransform_identity, m_hull, MakeColor( b3_colorYellow ) );
DrawTextLine( "v/f/e = %d/%d/%d", m_hull->vertexCount, m_hull->faceCount, m_hull->edgeCount / 2 );
}
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
Sample::Render();
}
@ -367,7 +367,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero );
}
m_original = b3CreateCylinder( 1.0f, 0.5f, 0.0f, 9 );
@ -406,9 +406,9 @@ public:
b3Transform transform1 = { { -2.0f, 0.0f, 0.0f }, b3Quat_identity };
b3Transform transform2 = { { 2.0f, 0.0f, 0.0f }, b3Quat_identity };
DrawHull( transform1, m_original, MakeColor( b3_colorGreen ) );
DrawHull( transform2, m_hull, MakeColor( b3_colorYellow ) );
DrawAxes( b3Transform_identity, 1.0f );
DrawHull( b3MakeWorldTransform( transform1 ), m_original, MakeColor( b3_colorGreen ) );
DrawHull( b3MakeWorldTransform( transform2 ), m_hull, MakeColor( b3_colorYellow ) );
DrawAxes( b3WorldTransform_identity, 1.0f );
DrawTextLine( "hull 1: area = %g, volume = %g, radius = %g", m_original->surfaceArea, m_original->volume,
m_original->innerRadius );
@ -495,7 +495,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero );
}
m_radius = 1.0f;
@ -579,15 +579,15 @@ public:
void Render() override
{
DrawSolidCapsule( b3Transform_identity, m_capsule, MakeColorAlpha( b3_colorAqua, 0.8f ) );
DrawHull( b3Transform_identity, &m_box.base, MakeColor( b3_colorBlueViolet ) );
DrawSolidCapsule( b3WorldTransform_identity, m_capsule, MakeColorAlpha( b3_colorAqua, 0.8f ) );
DrawHull( b3WorldTransform_identity, &m_box.base, MakeColor( b3_colorBlueViolet ) );
if ( m_hull != nullptr )
{
DrawHull( b3Transform_identity, m_hull, MakeColor( b3_colorYellow ) );
DrawHull( b3WorldTransform_identity, m_hull, MakeColor( b3_colorYellow ) );
}
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
if ( m_hull != nullptr )
{

View file

@ -179,7 +179,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero );
}
m_hull = nullptr;
@ -244,17 +244,17 @@ public:
{
if ( m_hull != nullptr )
{
DrawHull( b3Transform_identity, m_hull, MakeColor( b3_colorYellow ) );
DrawHull( b3WorldTransform_identity, m_hull, MakeColor( b3_colorYellow ) );
}
else
{
for ( int i = 0; i < m_count; ++i )
{
DrawPoint( m_points[i], 5.0f, MakeColor( b3_colorWhite ) );
DrawPoint( b3ToPos( m_points[i] ), 5.0f, MakeColor( b3_colorWhite ) );
}
}
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
Sample::Render();
}
@ -390,7 +390,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 12.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 12.0f, b3Pos_zero );
}
{
@ -445,7 +445,7 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 1.1f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 3.0f );
DrawAxes( b3MakeWorldTransform( transform ), 3.0f );
}
static Sample* Create( SampleContext* context )

View file

@ -1,13 +1,18 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "earcut.h"
#include "gfx/draw.h"
#include "gfx/keycodes.h"
#include "imgui.h"
#include "sample.h"
#include "utils.h"
#include "box3d/box3d.h"
#include <array>
#include <vector>
// Test the distance joint and all options
class DistanceJoint : public Sample
{
@ -92,8 +97,8 @@ public:
m_bodyIds[i] = b3CreateBody( m_worldId, &bodyDef );
b3CreateSphereShape( m_bodyIds[i], &shapeDef, &sphere );
b3Vec3 pivotA = { m_length * i, yOffset, 0.0f };
b3Vec3 pivotB = { m_length * ( i + 1.0f ), yOffset, 0.0f };
b3Pos pivotA = { m_length * i, yOffset, 0.0f };
b3Pos pivotB = { m_length * ( i + 1.0f ), yOffset, 0.0f };
jointDef.base.bodyIdA = prevBodyId;
jointDef.base.bodyIdB = m_bodyIds[i];
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( prevBodyId, pivotA );
@ -334,7 +339,7 @@ public:
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = { -2.0f, 2.0f, 0.0f };
bodyDef.position = { -2.0, 2.0, 0.0 };
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
@ -344,7 +349,7 @@ public:
b3MotorJointDef jointDef = b3DefaultMotorJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = bodyId;
jointDef.base.localFrameA.p = b3Add( bodyDef.position, { 0.25f, 1.25f, 0.0f } );
jointDef.base.localFrameA.p = { -1.75f, 3.25f, 0.0f };
jointDef.base.localFrameB.p = { 0.25f, 0.25f };
jointDef.base.collideConnected = true;
jointDef.linearHertz = 7.5f;
@ -401,7 +406,7 @@ public:
{
m_time += m_speed * timeStep;
b3Vec3 linearOffset;
b3Pos linearOffset;
linearOffset.x = 6.0f * sinf( 2.0f * m_time );
linearOffset.y = 10.0f + 4.0f * sinf( 1.0f * m_time );
linearOffset.z = 0.0f;
@ -430,7 +435,7 @@ public:
b3BodyId m_targetId;
b3BodyId m_bodyId;
b3JointId m_jointId;
b3Transform m_transform;
b3WorldTransform m_transform;
float m_time;
float m_speed;
float m_maxForce;
@ -524,13 +529,13 @@ public:
{
b3Sphere sphere = { { 0.0f, 10.0f, 0.0 }, 10.0f };
b3ExplosionDef def = b3DefaultExplosionDef();
def.position = sphere.center;
def.position = { 0.0, 10.0, 0.0 };
def.radius = sphere.radius;
def.falloff = 5.0f;
def.impulsePerArea = 10000.0f;
b3World_Explode( m_worldId, &def );
DrawSolidSphere( b3Transform_identity, sphere, MakeColor( b3_colorWhite ) );
DrawSolidSphere( b3WorldTransform_identity, sphere, MakeColor( b3_colorWhite ) );
}
return true;
@ -1156,7 +1161,7 @@ public:
b3Vec3 linearVelocity = b3Body_GetLinearVelocity( m_bodyId );
float kineticEnergy = 0.5f * b3Dot( angularVelocity, b3MulMV( massData.inertia, angularVelocity ) );
kineticEnergy += 0.5f * massData.mass * b3Dot( linearVelocity, linearVelocity );
b3Vec3 center = b3Body_GetWorldCenterOfMass( m_bodyId );
b3Pos center = b3Body_GetWorldCenterOfMass( m_bodyId );
b3Vec3 gravity = b3World_GetGravity( m_worldId );
float potentialEnergy = -massData.mass * center.y * gravity.y;
DrawTextLine( "kinetic energy = %g", kineticEnergy );
@ -1738,7 +1743,7 @@ public:
if ( ImGui::Button( "Impulse##Door" ) )
{
b3Vec3 p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } );
b3Pos p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } );
b3Body_ApplyLinearImpulse( m_doorId, { 0.0f, 0.0f, -m_magnitude }, p, true );
m_translationError1 = 0.0f;
m_translationError2 = 0.0f;
@ -1790,7 +1795,7 @@ public:
{
Sample::Step();
b3Vec3 p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } );
b3Pos p = b3Body_GetWorldPoint( m_doorId, { 0.75f, 0.0f, 0.0f } );
DrawPoint( p, 10.0f, MakeColor( b3_colorDarkKhaki ) );
float translationError1 = b3Joint_GetLinearSeparation( m_jointId1 );
@ -1876,7 +1881,7 @@ public:
b3CreateHullShape( m_bodyIds[i], &shapeDef, &box.base );
{
b3Vec3 pivot = { xbase + 2.0f * a * i, 20.0f, -0.5f };
b3Pos pivot = { xbase + 2.0f * a * i, 20.0, -0.5 };
jointDef.base.bodyIdA = prevBodyId;
jointDef.base.bodyIdB = m_bodyIds[i];
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
@ -1886,7 +1891,7 @@ public:
}
{
b3Vec3 pivot = { xbase + 2.0f * a * i, 20.0f, 0.5f };
b3Pos pivot = { xbase + 2.0f * a * i, 20.0, 0.5 };
jointDef.base.bodyIdA = prevBodyId;
jointDef.base.bodyIdB = m_bodyIds[i];
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
@ -1899,7 +1904,7 @@ public:
}
{
b3Vec3 pivot = { xbase + 2.0f * a * m_count, 20.0f, -0.5f };
b3Pos pivot = { xbase + 2.0f * a * m_count, 20.0, -0.5 };
jointDef.base.bodyIdA = prevBodyId;
jointDef.base.bodyIdB = groundId;
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
@ -1909,7 +1914,7 @@ public:
}
{
b3Vec3 pivot = { xbase + 2.0f * a * m_count, 20.0f, 0.5f };
b3Pos pivot = { xbase + 2.0f * a * m_count, 20.0, 0.5 };
jointDef.base.bodyIdA = prevBodyId;
jointDef.base.bodyIdB = groundId;
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
@ -1975,7 +1980,7 @@ public:
m_bodyIds[i] = b3_nullBodyId;
}
b3Vec3 position = { -12.5f, 10.0, 0.0f };
b3Pos position = { -12.5f, 10.0, 0.0f };
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.enableSleep = false;
@ -1999,8 +2004,8 @@ public:
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
float length = 2.0f;
b3Vec3 pivot1 = { position.x, position.y + 1.0f + length, 0.0f };
b3Vec3 pivot2 = { position.x, position.y + 1.0f, 0.0f };
b3Pos pivot1 = { position.x, position.y + 1.0f + length, 0.0f };
b3Pos pivot2 = { position.x, position.y + 1.0f, 0.0f };
b3DistanceJointDef jointDef = b3DefaultDistanceJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = m_bodyIds[m_count];
@ -2051,7 +2056,7 @@ public:
m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f };
b3Pos pivot = { position.x - 1.0f, position.y, 0.0f };
b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = m_bodyIds[m_count];
@ -2075,7 +2080,7 @@ public:
m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f };
b3Pos pivot = { position.x - 1.0f, position.y, 0.0f };
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = m_bodyIds[m_count];
@ -2099,7 +2104,7 @@ public:
m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
b3Vec3 pivot = { position.x - 1.0f, position.y, 0.0f };
b3Pos pivot = { position.x - 1.0f, position.y, 0.0f };
b3WeldJointDef jointDef = b3DefaultWeldJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = m_bodyIds[m_count];
@ -2557,7 +2562,7 @@ public:
float rightSteerTorque = b3WheelJoint_GetSteeringTorque( m_frontRightId );
DrawTextLine( "steering torque = %.1f/%.1f", leftSteerTorque, rightSteerTorque );
b3Transform transform = b3Transform_identity;
b3WorldTransform transform = b3WorldTransform_identity;
transform.p.y += 0.05f;
DrawAxes( transform, 2.0f );
}
@ -2602,7 +2607,7 @@ public:
if ( m_camera->m_thirdPerson )
{
b3Transform transform = b3Body_GetTransform( m_chassisId );
b3WorldTransform transform = b3Body_GetTransform( m_chassisId );
m_camera->m_pivot = transform.p;
m_camera->UpdateTransform();
}
@ -2641,3 +2646,479 @@ public:
};
static int sampleDriving = RegisterSample( "Joints", "Driving", Driving::Create );
// https://www.linkedin.com/feed/update/urn:li:activity:7308603994082353153/
class GearLift : public Sample
{
public:
explicit GearLift( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 18.0f, 12.0f, 17.0f, { -1.5f, 4.5f, 0.0f } );
}
AddGroundBox( 20.0f );
b3BodyId groundId;
{
b3BodyDef bodyDef = b3DefaultBodyDef();
groundId = b3CreateBody( m_worldId, &bodyDef );
}
CreateMesh( groundId );
// Gear cores shared by both shafts: a disk at each depth with an axle bridging
// them, all cylinders along z so the gears spin about z.
b3HullData* diskNear = MakeZCylinder( m_gearRadius, -m_gearZ - m_gearHalfDepth, -m_gearZ + m_gearHalfDepth, m_gearSides );
b3HullData* diskFar = MakeZCylinder( m_gearRadius, m_gearZ - m_gearHalfDepth, m_gearZ + m_gearHalfDepth, m_gearSides );
b3HullData* axle = MakeZCylinder( m_axleRadius, -m_gearZ, m_gearZ, m_axleSides );
m_motorTorque = 30000.0f;
m_motorSpeed = -0.3f;
m_enableMotor = true;
b3Pos gearPosition1 = { -4.25f, 9.75f, 0.0f };
b3Pos gearPosition2 = { -2.25f, 10.75f, 0.0f };
b3BodyId driverId = BuildGearBody( gearPosition1, m_gearRadius + m_toothHalfHeight, diskNear, diskFar, axle );
b3BodyId followerId = BuildGearBody( gearPosition2, m_gearRadius + m_toothHalfWidth, diskNear, diskFar, axle );
b3DestroyHull( diskNear );
b3DestroyHull( diskFar );
b3DestroyHull( axle );
// Driver shaft, motorized
{
b3RevoluteJointDef revoluteDef = b3DefaultRevoluteJointDef();
revoluteDef.base.bodyIdA = groundId;
revoluteDef.base.bodyIdB = driverId;
revoluteDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, gearPosition1 );
revoluteDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f };
revoluteDef.enableMotor = m_enableMotor;
revoluteDef.maxMotorTorque = m_motorTorque;
revoluteDef.motorSpeed = m_motorSpeed;
m_driverId = b3CreateRevoluteJoint( m_worldId, &revoluteDef );
}
// Follower shaft, swept between angle limits
{
b3RevoluteJointDef revoluteDef = b3DefaultRevoluteJointDef();
revoluteDef.base.bodyIdA = groundId;
revoluteDef.base.bodyIdB = followerId;
revoluteDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, gearPosition2 );
revoluteDef.base.localFrameA.q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.25f * B3_PI );
revoluteDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f };
revoluteDef.enableMotor = true;
revoluteDef.maxMotorTorque = 0.5f;
revoluteDef.lowerAngle = -0.3f * B3_PI;
revoluteDef.upperAngle = 0.8f * B3_PI;
revoluteDef.enableLimit = true;
b3CreateRevoluteJoint( m_worldId, &revoluteDef );
}
// One chain hangs from the follower rim at each depth and both lift the gate.
b3Pos linkAttach = { gearPosition2.x + m_gearRadius + 2.0f * m_toothHalfWidth + m_toothRadius, gearPosition2.y, 0.0f };
b3Pos doorPosition = { linkAttach.x, linkAttach.y - ( 2.0f * m_linkCount * m_linkHalfLength + m_doorHalfHeight ), 0.0f };
b3BodyId nearLink = CreateChain( followerId, { linkAttach.x, linkAttach.y, -m_gearZ } );
b3BodyId farLink = CreateChain( followerId, { linkAttach.x, linkAttach.y, m_gearZ } );
CreateDoor( groundId, doorPosition, nearLink, farLink );
CreateDebris();
}
~GearLift() override
{
b3DestroyMesh( m_mesh );
}
void CreateMesh( b3BodyId groundId )
{
// Silhouette of the Box2D stairwell traced as a closed loop. It is extruded
// four meters along z into a single triangle mesh, the 3D analogue of a
// b2Chain loop. The solid is inside the loop, so the windings face outward
// to put the collision normals on the open basin side where the debris sits.
static const b3Vec2 points[32] = {
{ -11.3000f, -0.2167f }, { 9.3375f, -0.2167f }, { 9.3375f, 7.1917f }, { 8.8083f, 7.1917f }, { 8.8083f, 0.3125f },
{ 0.3417f, 0.3125f }, { 0.3417f, 0.8417f }, { -0.1875f, 0.8417f }, { -0.1875f, 1.3708f }, { -0.7167f, 1.3708f },
{ -0.7167f, 1.9000f }, { -1.2458f, 1.9000f }, { -1.2458f, 2.4292f }, { -1.7750f, 2.4292f }, { -1.7750f, 2.9583f },
{ -2.3042f, 2.9583f }, { -2.3042f, 3.4875f }, { -2.8333f, 3.4875f }, { -2.8333f, 4.0167f }, { -3.3625f, 4.0167f },
{ -3.3625f, 4.5458f }, { -3.8917f, 4.5458f }, { -3.8917f, 5.0750f }, { -4.4208f, 5.0750f }, { -4.4208f, 5.6042f },
{ -4.9500f, 5.6042f }, { -4.9500f, 6.1333f }, { -5.4792f, 6.1333f }, { -5.4792f, 6.6625f }, { -6.0083f, 6.6625f },
{ -6.0083f, 7.1917f }, { -11.3000f, 7.1917f },
};
float zMin = -2.0f; // four meters across z
float zMax = 2.0f;
// Two vertices per silhouette point, one at each depth.
b3Vec3 vertices[64];
for ( int i = 0; i < 32; ++i )
{
vertices[2 * i + 0] = { points[i].x, points[i].y, zMin };
vertices[2 * i + 1] = { points[i].x, points[i].y, zMax };
}
std::vector<int> indices;
// Side walls: two triangles per silhouette edge, wound so the normal faces
// the open basin. The lo ring sits at zMin, the hi ring at zMax.
for ( int i = 0; i < 32; ++i )
{
int j = ( i + 1 ) % 32;
int aLo = 2 * i, aHi = 2 * i + 1;
int bLo = 2 * j, bHi = 2 * j + 1;
indices.push_back( aLo );
indices.push_back( bLo );
indices.push_back( bHi );
indices.push_back( aLo );
indices.push_back( bHi );
indices.push_back( aHi );
}
// End caps close the prism into a watertight manifold with no boundary edges.
// The non convex silhouette is triangulated with earcut, then each triangle is
// wound so the cap normal points out of the solid: the zMin cap faces -z and
// the zMax cap faces +z. The two ring edges each gain a second owner this way.
std::vector<std::array<double, 2>> ring;
ring.reserve( 32 );
for ( int i = 0; i < 32; ++i )
{
ring.push_back( { points[i].x, points[i].y } );
}
std::vector<std::vector<std::array<double, 2>>> polygon = { ring };
std::vector<uint32_t> cap = mapbox::earcut<uint32_t>( polygon );
for ( size_t k = 0; k + 3 <= cap.size(); k += 3 )
{
int r0 = (int)cap[k], r1 = (int)cap[k + 1], r2 = (int)cap[k + 2];
PushCap( indices, points, r0, r1, r2, 1, true ); // zMax cap, +z out
PushCap( indices, points, r0, r1, r2, 0, false ); // zMin cap, -z out
}
b3MeshDef def = {};
def.vertices = vertices;
def.vertexCount = 64;
def.indices = indices.data();
def.triangleCount = (int)( indices.size() / 3 );
def.identifyEdges = true;
m_mesh = b3CreateMesh( &def, nullptr, 0 );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.customColor = b3_colorDarkSeaGreen;
b3SurfaceMaterial material = shapeDef.baseMaterial;
shapeDef.materials = &material;
shapeDef.materialCount = 1;
b3CreateMeshShape( groundId, &shapeDef, m_mesh, b3Vec3_one );
// Back wall: a 0.1 m thick box closing the far side over the full mesh extent.
b3Vec2 lower = points[0];
b3Vec2 upper = points[0];
for ( int i = 1; i < 32; ++i )
{
lower.x = b3MinFloat( lower.x, points[i].x );
lower.y = b3MinFloat( lower.y, points[i].y );
upper.x = b3MaxFloat( upper.x, points[i].x );
upper.y = b3MaxFloat( upper.y, points[i].y );
}
float wallHalfThick = 0.05f;
b3Vec3 wallCenter = { 0.5f * ( lower.x + upper.x ), 0.5f * ( lower.y + upper.y ), -zMax - wallHalfThick };
b3BoxHull wall =
b3MakeOffsetBoxHull( 0.5f * ( upper.x - lower.x ), 0.5f * ( upper.y - lower.y ), wallHalfThick, wallCenter );
b3CreateHullShape( groundId, &shapeDef, &wall.base );
}
// Push one cap triangle, flipping the winding so its z-normal has the wanted sign.
void PushCap( std::vector<int>& indices, const b3Vec2* poly, int r0, int r1, int r2, int vOffset, bool wantPositiveZ )
{
float cross =
( poly[r1].x - poly[r0].x ) * ( poly[r2].y - poly[r0].y ) - ( poly[r1].y - poly[r0].y ) * ( poly[r2].x - poly[r0].x );
bool positive = cross > 0.0f;
int v0 = 2 * r0 + vOffset;
int v1 = 2 * r1 + vOffset;
int v2 = 2 * r2 + vOffset;
indices.push_back( v0 );
if ( positive == wantPositiveZ )
{
indices.push_back( v1 );
indices.push_back( v2 );
}
else
{
indices.push_back( v2 );
indices.push_back( v1 );
}
}
// A cylinder built directly along z (depth) so it needs no reorientation.
b3HullData* MakeZCylinder( float radius, float zMin, float zMax, int sides )
{
b3Vec3 points[64];
for ( int i = 0; i < sides; ++i )
{
float angle = ( 2.0f * B3_PI * i ) / sides;
float c = cosf( angle );
float s = sinf( angle );
points[2 * i + 0] = { radius * c, radius * s, zMin };
points[2 * i + 1] = { radius * c, radius * s, zMax };
}
return b3CreateHull( points, 2 * sides, 2 * sides );
}
// One gear shaft: a disk and tooth ring at each depth joined by an axle, all on
// one rigid body so the two depth gears share an angle and read as a clean gear.
b3BodyId BuildGearBody( b3Pos position, float toothCenterRadius, b3HullData* diskNear, b3HullData* diskFar, b3HullData* axle )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = position;
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.friction = 0.1f;
shapeDef.baseMaterial.customColor = b3_colorSaddleBrown;
b3CreateHullShape( bodyId, &shapeDef, diskNear );
b3CreateHullShape( bodyId, &shapeDef, diskFar );
shapeDef.baseMaterial.customColor = b3_colorSlateGray;
b3CreateHullShape( bodyId, &shapeDef, axle );
AddTeeth( bodyId, toothCenterRadius, -m_gearZ );
AddTeeth( bodyId, toothCenterRadius, m_gearZ );
return bodyId;
}
// Chain of capsule links hanging from a body down to the gate. Returns the last link.
b3BodyId CreateChain( b3BodyId topBodyId, b3Pos attach )
{
b3Capsule capsule = { { 0.0f, -m_linkHalfLength, 0.0f }, { 0.0f, m_linkHalfLength, 0.0f }, m_linkRadius };
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.customColor = b3_colorLightSteelBlue;
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
jointDef.maxMotorTorque = 0.05f;
jointDef.enableMotor = true;
jointDef.base.drawScale = 0.2f;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
b3Pos position = { attach.x, attach.y - m_linkHalfLength, attach.z };
b3BodyId prevBodyId = topBodyId;
for ( int i = 0; i < m_linkCount; ++i )
{
bodyDef.position = position;
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3CreateCapsuleShape( bodyId, &shapeDef, &capsule );
b3Pos pivot = { position.x, position.y + m_linkHalfLength, attach.z };
jointDef.base.bodyIdA = prevBodyId;
jointDef.base.bodyIdB = bodyId;
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( prevBodyId, pivot );
jointDef.base.localFrameB.p = b3Body_GetLocalPoint( bodyId, pivot );
b3CreateRevoluteJoint( m_worldId, &jointDef );
position.y -= 2.0f * m_linkHalfLength;
prevBodyId = bodyId;
}
return prevBodyId;
}
// A single gate box raised by both chains and held upright sliding along y.
void CreateDoor( b3BodyId groundId, b3Pos doorPosition, b3BodyId nearLink, b3BodyId farLink )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = doorPosition;
b3BodyId doorId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.density *= 0.5f;
shapeDef.baseMaterial.friction = 0.1f;
shapeDef.baseMaterial.customColor = b3MakeDebugColor( b3_colorDarkCyan, b3_debugMaterialMetallic );
b3BoxHull box = b3MakeBoxHull( 0.05f, m_doorHalfHeight, m_doorHalfDepth );
b3CreateHullShape( doorId, &shapeDef, &box.base );
// Hinge the gate to each chain at its depth.
b3BodyId links[2] = { nearLink, farLink };
float depths[2] = { -m_gearZ, m_gearZ };
for ( int i = 0; i < 2; ++i )
{
b3Pos pivot = { doorPosition.x, doorPosition.y + m_doorHalfHeight, depths[i] };
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
jointDef.base.bodyIdA = links[i];
jointDef.base.bodyIdB = doorId;
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( links[i], pivot );
jointDef.base.localFrameB.p = { 0.0f, m_doorHalfHeight, depths[i] };
jointDef.enableMotor = true;
jointDef.maxMotorTorque = 50.0f;
b3CreateRevoluteJoint( m_worldId, &jointDef );
}
// Prismatic axis is frame A local x, so rotate it onto world y.
b3Quat slideAxis = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, b3Vec3_axisY );
b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = doorId;
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, doorPosition );
jointDef.base.localFrameA.q = slideAxis;
jointDef.base.localFrameB.p = { 0.0f, 0.0f, 0.0f };
jointDef.base.localFrameB.q = slideAxis;
jointDef.maxMotorForce = 200.0f;
jointDef.enableMotor = true;
jointDef.base.collideConnected = true;
b3CreatePrismaticJoint( m_worldId, &jointDef );
}
// Ring of 16 teeth around the gear rim at depth zCenter, in the body frame. Box3D
// has no rounded hulls, so each tooth tapers tangentially toward the tip to clear
// the meshing teeth the way Box2D's rounded teeth do.
void AddTeeth( b3BodyId bodyId, float centerRadius, float zCenter )
{
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.friction = 0.1f;
shapeDef.baseMaterial.customColor = b3_colorGray;
int count = 16;
float deltaAngle = 2.0f * B3_PI / count;
float hx = m_toothHalfWidth; // radial half extent
float hz = m_gearHalfDepth; // depth half extent
float baseHalf = m_toothHalfHeight; // tangential half width at the base
float tipHalf = m_toothHalfHeight - m_toothRadius; // narrower at the tip
for ( int i = 0; i < count; ++i )
{
b3Quat q = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, i * deltaAngle );
b3Vec3 center = b3RotateVector( q, { centerRadius, 0.0f, 0.0f } );
center.z = zCenter;
// Base at the inner radius, narrower tip at the outer radius.
b3Vec3 local[8] = {
{ -hx, -baseHalf, -hz }, { -hx, baseHalf, -hz }, { -hx, baseHalf, hz }, { -hx, -baseHalf, hz },
{ hx, -tipHalf, -hz }, { hx, tipHalf, -hz }, { hx, tipHalf, hz }, { hx, -tipHalf, hz },
};
b3Vec3 points[8];
for ( int k = 0; k < 8; ++k )
{
points[k] = b3Add( center, b3RotateVector( q, local[k] ) );
}
b3HullData* tooth = b3CreateHull( points, 8, 8 );
b3CreateHullShape( bodyId, &shapeDef, tooth );
b3DestroyHull( tooth );
}
}
void CreateDebris()
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.rollingResistance = 0.3f;
b3HexColor colors[5] = {
b3_colorGray, b3_colorGainsboro, b3_colorLightGray, b3_colorLightSlateGray, b3_colorDarkGray,
};
b3HullData* rockHull = b3CreateRock( m_rockRadius );
float x = -5.0f;
int xCount = 12, yCount = 10;
for ( int i = 0; i < xCount; ++i )
{
float y = 6.5f - 0.25f * i;
for ( int j = 0; j < yCount; ++j )
{
// Spread the debris across the depth of the stairwell.
bodyDef.position = { x, y, RandomFloatRange( -1.65f, 0.35f ) };
bodyDef.rotation = RandomQuat();
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
shapeDef.baseMaterial.customColor = colors[(int)RandomIntRange( 0, 4 )];
b3CreateHullShape( bodyId, &shapeDef, rockHull );
y += 0.2f;
}
x += 0.3f;
}
b3DestroyHull( rockHull );
}
void SetMotorSpeed( float speed )
{
b3RevoluteJoint_SetMotorSpeed( m_driverId, speed );
b3Joint_WakeBodies( m_driverId );
}
bool DrawControls() override
{
if ( ImGui::Checkbox( "Motor", &m_enableMotor ) )
{
b3RevoluteJoint_EnableMotor( m_driverId, m_enableMotor );
b3Joint_WakeBodies( m_driverId );
}
ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() );
if ( ImGui::SliderFloat( "Max Torque", &m_motorTorque, 0.0f, 100000.0f, "%.0f" ) )
{
b3RevoluteJoint_SetMaxMotorTorque( m_driverId, m_motorTorque );
b3Joint_WakeBodies( m_driverId );
}
if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -0.3f, 0.3f, "%.2f" ) )
{
SetMotorSpeed( m_motorSpeed );
}
ImGui::PopItemWidth();
return true;
}
static Sample* Create( SampleContext* context )
{
return new GearLift( context );
}
static constexpr float m_gearRadius = 1.0f;
static constexpr float m_gearHalfDepth = 0.125f; // 25 cm gear depth
static constexpr float m_gearZ = 1.5f; // near and far gear planes
static constexpr float m_axleRadius = 0.2f;
static constexpr float m_toothHalfWidth = 0.11f;
static constexpr float m_toothHalfHeight = 0.09f;
static constexpr float m_toothRadius = 0.03f;
static constexpr float m_linkHalfLength = 0.07f;
static constexpr float m_linkRadius = 0.05f;
static constexpr int m_linkCount = 40;
static constexpr float m_doorHalfHeight = 1.5f;
static constexpr float m_doorHalfDepth = 1.95f;
static constexpr int m_gearSides = 24;
static constexpr int m_axleSides = 12;
static constexpr float m_rockRadius = 0.3f;
b3MeshData* m_mesh;
b3JointId m_driverId;
float m_motorTorque;
float m_motorSpeed;
bool m_enableMotor;
};
static int sampleGearLift = RegisterSample( "Joints", "Gear Lift", GearLift::Create );

View file

@ -1,10 +1,9 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "sample.h"
#include "gfx/draw.h"
#include "gfx/keycodes.h"
#include "sample.h"
#include "box3d/box3d.h"
#include "box3d/constants.h"
@ -39,11 +38,11 @@ public:
m_simplexCache = {};
m_satCache = {};
m_manualFeature = 0;
m_baseTranslation = b3Vec3_zero;
m_baseTranslation = b3Pos_zero;
m_baseQuaternion = b3Quat_identity;
m_baseX = 0;
m_baseY = 0;
m_origin = b3Vec3_zero;
m_origin = b3Pos_zero;
m_useCache = false;
m_tracking = false;
m_rotating = false;
@ -54,7 +53,7 @@ public:
DrawTextLine( "origin: %g %g %g", m_origin.x, m_origin.y, m_origin.z );
DrawTextLine( "count = %d", m_manifold.pointCount );
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
if ( m_manifold.pointCount == 0 )
{
@ -68,7 +67,7 @@ public:
{
const b3LocalManifoldPoint& manifoldPoint = m_manifold.points[pointIndex];
b3Vec3 point = b3TransformPoint( m_transformA, manifoldPoint.point );
b3Pos point = b3TransformWorldPoint( m_transformA, manifoldPoint.point );
DrawLine( point, point + length * normal, MakeColor( b3_colorWhite ) );
@ -81,11 +80,12 @@ public:
DrawPoint( point, 10.0f, MakeColor( b3_colorYellow ) );
}
DrawWorldString( point, MakeColor( b3_colorWhite ), " %.3f", manifoldPoint.separation );
DrawString3D( point, MakeColor( b3_colorWhite ), " %.3f", manifoldPoint.separation );
b3Vec3 perp = b3Perp( normal );
b3FeaturePair pair = manifoldPoint.pair;
DrawWorldString( point + 0.025f * normal + 0.05f * perp, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1, pair.index1, pair.owner2, pair.index2 );
DrawString3D( point + 0.025f * normal + 0.05f * perp, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1,
pair.index1, pair.owner2, pair.index2 );
}
Sample::Render();
@ -142,8 +142,8 @@ public:
if ( m_tracking )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transformB.p = m_baseTranslation + origin - m_origin;
b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transformB.p = m_baseTranslation + b3SubPos( origin, m_origin );
}
if ( m_rotating )
@ -160,11 +160,11 @@ public:
static constexpr int m_pointCapacity = 64;
b3LocalManifold m_manifold;
b3LocalManifoldPoint m_points[m_pointCapacity];
b3Transform m_transformA;
b3Transform m_transformB;
b3Vec3 m_baseTranslation;
b3WorldTransform m_transformA;
b3WorldTransform m_transformB;
b3Pos m_baseTranslation;
b3Quat m_baseQuaternion;
b3Vec3 m_origin;
b3Pos m_origin;
b3SimplexCache m_simplexCache;
b3SATCache m_satCache;
int m_manualFeature;
@ -202,11 +202,11 @@ public:
m_simplexCache = {};
m_satCache = {};
m_baseTranslation = b3Vec3_zero;
m_baseTranslation = b3Pos_zero;
m_baseQuaternion = b3Quat_identity;
m_baseX = 0;
m_baseY = 0;
m_origin = b3Vec3_zero;
m_origin = b3Pos_zero;
m_useCache = false;
m_tracking = false;
m_rotating = false;
@ -221,19 +221,19 @@ public:
DrawTextLine( "feature = %d", m_manifold.feature );
DrawTextLine( "cache hit = %d", m_satCache.hit );
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
if ( m_manifold.pointCount > 0 )
{
float length = 0.5f * b3GetLengthUnitsPerMeter();
b3Vec3 normal = b3RotateVector( m_transformB.q, m_manifold.normal );
for ( int pointIndex = 0; pointIndex < m_manifold.pointCount; ++pointIndex )
{
const b3LocalManifoldPoint& manifoldPoint = m_manifold.points[pointIndex];
b3Vec3 point = manifoldPoint.point;
DrawLine( point, point + length * m_manifold.normal, MakeColor( b3_colorWhite ) );
b3Pos point = b3TransformWorldPoint(m_transformB, manifoldPoint.point );
DrawLine( point, point + length * normal, MakeColor( b3_colorWhite ) );
if ( manifoldPoint.separation > 0.0f )
{
@ -244,24 +244,25 @@ public:
DrawPoint( point, 10.0f, MakeColor( b3_colorYellow ) );
}
DrawWorldString( point, MakeColor( b3_colorWhite ), " %.2f", 100.0f * manifoldPoint.separation );
DrawString3D( point, MakeColor( b3_colorWhite ), " %.2f", 100.0f * manifoldPoint.separation );
b3FeaturePair pair = manifoldPoint.pair;
DrawWorldString( point + 0.025f * m_manifold.normal, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1, pair.index1, pair.owner2, pair.index2 );
DrawString3D( point + 0.025f * normal, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1,
pair.index1, pair.owner2, pair.index2 );
}
}
b3Vec3 p1 = b3TransformPoint( m_transformA, m_triangle[0] );
b3Vec3 p2 = b3TransformPoint( m_transformA, m_triangle[1] );
b3Vec3 p3 = b3TransformPoint( m_transformA, m_triangle[2] );
DrawTriangle( p1, p2, p3, MakeColor( b3_colorCyan ) );
DrawTriangle( m_transformA, m_triangle[0], m_triangle[1], m_triangle[2], MakeColor( b3_colorCyan ) );
DrawWorldString( p1, MakeColor( b3_colorWhite ), "0" );
DrawWorldString( p2, MakeColor( b3_colorWhite ), "1" );
DrawWorldString( p3, MakeColor( b3_colorWhite ), "2" );
b3Pos p1 = b3TransformWorldPoint( m_transformA, m_triangle[0] );
b3Pos p2 = b3TransformWorldPoint( m_transformA, m_triangle[1] );
b3Pos p3 = b3TransformWorldPoint( m_transformA, m_triangle[2] );
DrawString3D( p1, MakeColor( b3_colorWhite ), "0" );
DrawString3D( p2, MakeColor( b3_colorWhite ), "1" );
DrawString3D( p3, MakeColor( b3_colorWhite ), "2" );
b3Vec3 center = 1.0f / 3.0f * ( p1 + p2 + p3 );
b3Vec3 normal = b3Normalize( b3Cross( p2 - p1, p3 - p1 ) );
b3Pos center = p1 + ( 1.0f / 3.0f ) * ( ( p2 - p1 ) + ( p3 - p1 ) );
DrawArrow( center, center + 0.5f * normal, MakeColor( b3_colorMediumPurple ) );
Sample::Render();
@ -318,8 +319,8 @@ public:
if ( m_tracking )
{
PickRay pickRay = m_camera->BuildPickRay( p.x, p.y );
b3Vec3 origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transformB.p = m_baseTranslation + origin - m_origin;
b3Pos origin = pickRay.origin + 10.0f * b3Normalize( pickRay.translation );
m_transformB.p = m_baseTranslation + b3SubPos( origin, m_origin );
}
if ( m_rotating )
@ -336,12 +337,17 @@ public:
static constexpr int m_pointCapacity = 8;
b3LocalManifold m_manifold;
b3LocalManifoldPoint m_points[m_pointCapacity];
b3Transform m_transformA;
b3Transform m_transformB;
// Triangle transform
b3WorldTransform m_transformA;
// Convex shape transform
b3WorldTransform m_transformB;
b3Vec3 m_triangle[3] = {};
b3Vec3 m_baseTranslation;
b3Pos m_baseTranslation;
b3Quat m_baseQuaternion;
b3Vec3 m_origin;
b3Pos m_origin;
b3SimplexCache m_simplexCache;
b3SATCache m_satCache;
int m_manualFeature;
@ -371,7 +377,7 @@ public:
void Step() override
{
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideSpheres( &m_manifold, m_pointCapacity, &m_sphere, &m_sphere, transformBtoA );
}
@ -413,7 +419,7 @@ public:
void Step() override
{
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideCapsuleAndSphere( &m_manifold, m_pointCapacity, &m_capsule, &m_sphere, transformBtoA );
}
@ -451,7 +457,7 @@ public:
m_simplexCache = {};
}
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideHullAndSphere( &m_manifold, m_pointCapacity, &m_hull.base, &m_sphere, transformBtoA, &m_simplexCache );
}
@ -479,10 +485,10 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Pos_zero );
}
m_sphere = { { 0.0f, 0.0f, 0.0f }, 1.0f };
m_sphere = { { 0.0f, 0.0f, 0.0f }, 0.25f };
m_triangle[0] = { 0.0f, 0.0f, 0.0f };
m_triangle[1] = { 4.0f, 0.0f, 4.0f };
m_triangle[2] = { 4.0f, 0.0f, 0.0f };
@ -490,13 +496,13 @@ public:
// b3Quat qA = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, 2.0f );
// m_transformA = { { 1.0f, 1.0f, 0.0f }, qA };
m_transformA = b3Transform_identity;
m_transformA = b3WorldTransform_identity;
m_transformB = { { 2.0f, 0.5f, 1.0f }, b3Quat_identity };
}
void Render() override
{
DrawSolidSphere( m_transformB, m_sphere, MakeColor( b3_colorGreen ) );
DrawSolidSphere( m_transformB, m_sphere, MakeColorAlpha( b3_colorGreen, 0.5f ) );
TriangleManifold::Render();
}
@ -504,7 +510,7 @@ public:
void Step() override
{
// Convert triangle to frame B
b3Transform xf = b3InvMulTransforms( m_transformB, m_transformA );
b3Transform xf = b3InvMulWorldTransforms( m_transformB, m_transformA );
b3Vec3 localTriangle[3] = {
b3TransformPoint( xf, m_triangle[0] ),
b3TransformPoint( xf, m_triangle[1] ),
@ -512,13 +518,6 @@ public:
};
b3CollideSphereAndTriangle( &m_manifold, m_pointCapacity, &m_sphere, localTriangle );
// Convert manifold from frame B to world space
m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal );
for ( int i = 0; i < m_manifold.pointCount; ++i )
{
m_manifold.points[i].point = b3TransformPoint( m_transformB, m_manifold.points[i].point );
}
}
b3Sphere m_sphere;
@ -548,7 +547,7 @@ public:
void Step() override
{
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideCapsules( &m_manifold, m_pointCapacity, &m_capsule, &m_capsule, transformBtoA );
}
@ -570,7 +569,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 5.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 5.0f, b3Pos_zero );
}
m_capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.5f };
@ -606,7 +605,7 @@ public:
m_simplexCache = {};
}
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideHullAndCapsule( &m_manifold, m_pointCapacity, &m_hull.base, &m_capsule, transformBtoA, &m_simplexCache );
}
@ -634,7 +633,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Pos_zero );
}
m_capsule = { { 0.0f, -0.2f, 0.0f }, { 0.0f, 0.2f, 0.0f }, 0.05f };
@ -642,7 +641,7 @@ public:
m_triangle[1] = { -4.0f, 0.0f, 0.0f };
m_triangle[2] = { 0.0f, 0.0f, 0.0f };
m_transformA = b3Transform_identity;
m_transformA = b3WorldTransform_identity;
m_transformB = { { 0.0f, 0.5f, 0.0f }, b3Quat_identity };
m_transformB.q = b3MakeQuatFromAxisAngle( { 0.0f, 0.0f, 1.0f }, 0.5f * B3_PI );
@ -652,7 +651,7 @@ public:
void Render() override
{
DrawSolidCapsule( m_transformB, m_capsule, MakeColor( b3_colorGreen ) );
DrawSolidCapsule( m_transformB, m_capsule, MakeColorAlpha( b3_colorGreen, 0.5f ) );
DrawAxes( m_transformB, 0.1f );
TriangleManifold::Render();
@ -666,7 +665,7 @@ public:
}
// Put triangle into capsule coordinates
b3Transform xf = b3InvMulTransforms( m_transformB, m_transformA );
b3Transform xf = b3InvMulWorldTransforms( m_transformB, m_transformA );
b3Vec3 localTriangle[3] = {
b3TransformPoint( xf, m_triangle[0] ),
b3TransformPoint( xf, m_triangle[1] ),
@ -674,13 +673,6 @@ public:
};
b3CollideCapsuleAndTriangle( &m_manifold, m_pointCapacity, &m_capsule, localTriangle, &m_simplexCache );
// Convert manifold from frame B to world space
m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal );
for ( int i = 0; i < m_manifold.pointCount; ++i )
{
m_manifold.points[i].point = b3TransformPoint( m_transformB, m_manifold.points[i].point );
}
}
b3Capsule m_capsule;
@ -696,7 +688,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 4.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 4.0f, b3Pos_zero );
}
// m_boxA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, { { 0.0f, -0.5f, 0.0f }, b3Quat_identity } );
@ -802,7 +794,7 @@ public:
m_satCache = {};
}
b3Transform transformBtoA = b3InvMulTransforms( m_transformA, m_transformB );
b3Transform transformBtoA = b3InvMulWorldTransforms( m_transformA, m_transformB );
b3CollideHulls( &m_manifold, m_pointCapacity, m_hullA, m_hullB, transformBtoA, &m_satCache );
DrawTextLine( "SAT type: %d", m_satCache.type );
@ -834,7 +826,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 3.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 30.0f, 3.0f, b3Pos_zero );
}
m_triangle[0] = { 1.00000000, 0, 1.00000000 };
@ -843,8 +835,8 @@ public:
m_boxHull = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
m_transformA = b3Transform_identity;
m_transformB = b3Transform_identity;
m_transformA = b3WorldTransform_identity;
m_transformB = b3WorldTransform_identity;
m_transformB.p = { 0.0f, 0.45f, 0.1f };
// b3MeshEdgeFlags
@ -881,8 +873,8 @@ public:
{
DrawHull( m_transformB, m_hull, MakeColor( b3_colorGreen ) );
b3Transform xf;
xf.p = b3TransformPoint( m_transformB, m_hull->center );
b3WorldTransform xf;
xf.p = b3TransformWorldPoint( m_transformB, m_hull->center );
xf.q = m_transformB.q;
DrawAxes( xf, 0.1f );
@ -911,7 +903,7 @@ public:
m_satCache = {};
}
b3Transform xf = b3InvMulTransforms( m_transformB, m_transformA );
b3Transform xf = b3InvMulWorldTransforms( m_transformB, m_transformA );
b3Vec3 localTriangle[3] = {
b3TransformPoint( xf, m_triangle[0] ),
b3TransformPoint( xf, m_triangle[1] ),
@ -920,13 +912,6 @@ public:
b3CollideHullAndTriangle( &m_manifold, m_pointCapacity, m_hull, localTriangle[0], localTriangle[1], localTriangle[2],
m_flags, &m_satCache );
// Convert manifold from frame B to world space
m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal );
for ( int i = 0; i < m_manifold.pointCount; ++i )
{
m_manifold.points[i].point = b3TransformPoint( m_transformB, m_manifold.points[i].point );
}
}
int m_flags;

View file

@ -30,7 +30,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Pos_zero );
}
b3BodyDef bodyDef = b3DefaultBodyDef();
@ -171,7 +171,7 @@ public:
DrawTextLine( "triangle count = %d, bytes = %d", m_gridMesh->triangleCount, m_gridMesh->byteCount );
b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 1.0f );
DrawAxes( b3MakeWorldTransform( transform ), 1.0f );
}
void Step() override
@ -215,7 +215,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Pos_zero );
}
b3BodyDef bodyDef = b3DefaultBodyDef();
@ -345,7 +345,7 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 1.0f );
DrawAxes( b3MakeWorldTransform( transform ), 1.0f );
}
void Step() override
@ -389,7 +389,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Pos_zero );
}
@ -563,7 +563,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Pos_zero );
}
b3BodyDef bodyDef = b3DefaultBodyDef();
@ -643,7 +643,7 @@ public:
m_humans[humanIndex] = {};
int groupIndex = humanIndex;
b3Vec3 position = { -14.0f + 1.5f * humanIndex, 8.0f, 0.0f };
b3Pos position = { -14.0f + 1.5f * humanIndex, 8.0f, 0.0f };
CreateHuman( &m_humans[humanIndex], m_worldId, position, frictionTorque, hertz, dampingRatio, groupIndex, nullptr,
colorize );
}
@ -741,7 +741,7 @@ static int sampleMeshReflection = RegisterSample( "Mesh", "Reflection", MeshRefl
struct CastContext
{
b3Vec3 point;
b3Pos point;
b3Vec3 normal;
float fraction;
bool hit;
@ -760,7 +760,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Pos_zero );
}
#if defined( NDEBUG )
@ -899,11 +899,11 @@ public:
{
Sample::Render();
b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 0.5f );
DrawAxes( b3MakeWorldTransform( transform ), 0.5f );
}
// This callback finds the closest hit.
static float CastCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t surfaceType,
static float CastCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t surfaceType,
int triangleIndex, int childIndex, void* context )
{
(void)shapeId;
@ -933,16 +933,17 @@ public:
// m_rayOrigin = { 0.0f, -FLT_EPSILON, 0.0f };
// m_rayTranslation = { -1000.0f, 0.0f, 0.0 };
b3RayResult result = b3World_CastRayClosest( m_worldId, m_rayOrigin, m_rayTranslation, b3DefaultQueryFilter() );
b3Pos origin = b3ToPos( m_rayOrigin );
b3RayResult result = b3World_CastRayClosest( m_worldId, origin, m_rayTranslation, b3DefaultQueryFilter() );
DrawPoint( m_rayOrigin, 6.0f, MakeColor( b3_colorGreenYellow ) );
DrawPoint( m_rayOrigin + m_rayTranslation, 6.0f, MakeColor( b3_colorRed ) );
DrawLine( m_rayOrigin, m_rayOrigin + m_rayTranslation, MakeColor( b3_colorGray ) );
DrawPoint( origin, 6.0f, MakeColor( b3_colorGreenYellow ) );
DrawPoint( b3OffsetPos( origin, m_rayTranslation ), 6.0f, MakeColor( b3_colorRed ) );
DrawLine( origin, b3OffsetPos( origin, m_rayTranslation ), MakeColor( b3_colorGray ) );
if ( result.hit )
{
b3Vec3 point = result.point;
DrawLine( point, point + 0.5f * result.normal, MakeColor( b3_colorGray ) );
b3Pos point = result.point;
DrawLine( point, b3OffsetPos( point, 0.5f * result.normal ), MakeColor( b3_colorGray ) );
DrawPoint( point, 10.0f, MakeColor( b3_colorOrange ) );
}
}
@ -957,19 +958,21 @@ public:
CastContext result = {};
result.fraction = 1.0f;
b3World_CastShape( m_worldId, &proxy, m_rayTranslation, b3DefaultQueryFilter(), CastCallback, &result );
b3World_CastShape( m_worldId, b3Pos_zero, &proxy, m_rayTranslation, b3DefaultQueryFilter(), CastCallback, &result );
DrawPoint( m_rayOrigin, 2.0f, MakeColor( b3_colorGreen ) );
DrawPoint( m_rayOrigin + m_rayTranslation, 2.0f, MakeColor( b3_colorRed ) );
DrawLine( m_rayOrigin, m_rayOrigin + m_rayTranslation, MakeColor( b3_colorYellow ) );
b3Pos origin = b3ToPos( m_rayOrigin );
DrawPoint( origin, 2.0f, MakeColor( b3_colorGreen ) );
DrawPoint( b3OffsetPos( origin, m_rayTranslation ), 2.0f, MakeColor( b3_colorRed ) );
DrawLine( origin, b3OffsetPos( origin, m_rayTranslation ), MakeColor( b3_colorYellow ) );
b3Sphere sphere = { b3Vec3_zero, m_radius };
DrawSolidSphere( { m_rayOrigin + result.fraction * m_rayTranslation, b3Quat_identity }, sphere, MakeColor( b3_colorOrange ) );
b3Transform sphereXf = { m_rayOrigin + result.fraction * m_rayTranslation, b3Quat_identity };
DrawSolidSphere( b3MakeWorldTransform( sphereXf ), sphere, MakeColor( b3_colorOrange ) );
if ( result.hit )
{
b3Vec3 point = result.point;
DrawLine( point, point + 0.5f * result.normal, MakeColor( b3_colorGreen ) );
b3Pos point = result.point;
DrawLine( point, b3OffsetPos( point, 0.5f * result.normal ), MakeColor( b3_colorGreen ) );
DrawPoint( point, 6.0f, MakeColor( b3_colorPurple ) );
}
}
@ -1023,7 +1026,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 50.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 50.0f, b3Pos_zero );
}
m_meshIndex = 0;
@ -1149,21 +1152,22 @@ public:
int axis = node->data.asNode.axis;
b3Vec3 center = b3AABB_Center( box );
b3Pos centerPos = b3ToPos( center );
if ( axis == 0 )
{
DrawArrow( center, center + 0.1f * b3Vec3_axisX, MakeColor( b3_colorRed ) );
DrawArrow( centerPos, b3OffsetPos( centerPos, 0.1f * b3Vec3_axisX ), MakeColor( b3_colorRed ) );
}
else if ( axis == 1 )
{
DrawArrow( center, center + 0.1f * b3Vec3_axisY, MakeColor( b3_colorGreen ) );
DrawArrow( centerPos, b3OffsetPos( centerPos, 0.1f * b3Vec3_axisY ), MakeColor( b3_colorGreen ) );
}
else if ( axis == 2 )
{
DrawArrow( center, center + 0.1f * b3Vec3_axisZ, MakeColor( b3_colorBlue ) );
DrawArrow( centerPos, b3OffsetPos( centerPos, 0.1f * b3Vec3_axisZ ), MakeColor( b3_colorBlue ) );
}
else
{
DrawSolidSphere( b3Transform_identity, { center, 0.03f }, MakeColor( b3_colorOrange ) );
DrawSolidSphere( b3WorldTransform_identity, { center, 0.03f }, MakeColor( b3_colorOrange ) );
}
}
@ -1225,7 +1229,7 @@ public:
void Render() override
{
Sample::Render();
DrawAxes( b3Transform_identity, 1.0f );
DrawAxes( b3WorldTransform_identity, 1.0f );
DrawTextLine( "triangle count = %d", m_mesh->triangleCount );
DrawTextLine( "vertex count = %d", m_mesh->vertexCount );
@ -1251,18 +1255,20 @@ public:
(void)area;
b3Vec3 p = ( 1.0f / 3.0f ) * ( v1 + v2 + v3 );
DrawPoint( p, 10.0f, MakeColor( b3_colorCyan ) );
DrawWorldString( p + offset, MakeColor( b3_colorOrange ), "%d", triangleIndex );
b3Pos pPos = b3ToPos( p );
DrawPoint( pPos, 10.0f, MakeColor( b3_colorCyan ) );
DrawString3D( b3OffsetPos( pPos, offset ), MakeColor( b3_colorOrange ), "%d", triangleIndex );
{
DrawPoint( v1, 10.0f, MakeColor( b3_colorRed ) );
DrawPoint( v2, 10.0f, MakeColor( b3_colorGreen ) );
DrawPoint( v3, 10.0f, MakeColor( b3_colorBlue ) );
DrawWorldString( v1 + offset, MakeColor( b3_colorRed ), "%d", i1 );
DrawWorldString( v2 + offset, MakeColor( b3_colorGreen ), "%d", i2 );
DrawWorldString( v3 + offset, MakeColor( b3_colorBlue ), "%d", i3 );
b3Pos p1 = b3ToPos( v1 );
b3Pos p2 = b3ToPos( v2 );
b3Pos p3 = b3ToPos( v3 );
DrawPoint( p1, 10.0f, MakeColor( b3_colorRed ) );
DrawPoint( p2, 10.0f, MakeColor( b3_colorGreen ) );
DrawPoint( p3, 10.0f, MakeColor( b3_colorBlue ) );
DrawString3D( b3OffsetPos( p1, offset ), MakeColor( b3_colorRed ), "%d", i1 );
DrawString3D( b3OffsetPos( p2, offset ), MakeColor( b3_colorGreen ), "%d", i2 );
DrawString3D( b3OffsetPos( p3, offset ), MakeColor( b3_colorBlue ), "%d", i3 );
}
}
}
@ -1303,7 +1309,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Pos_zero );
}
LoadTempMesh( "data/meshes/voxel_mesh_01.obj", m_tempMeshes + 0, 0.01f, true );
@ -1378,11 +1384,11 @@ public:
explicit VoxelMesh( SampleContext* context )
: Sample( context )
{
b3Vec3 origin = { 5000.0f, 3500.0f, -7000.0f };
b3Pos origin = { 5000.0f, 3500.0f, -7000.0f };
if ( context->restart == false )
{
m_camera->SetView( -115.0f, 5.0f, 5.0f, b3Vec3{ 0.0f, 10.0f, 0.0f } + origin );
m_camera->SetView( -115.0f, 5.0f, 5.0f, b3OffsetPos( origin, b3Vec3{ 0.0f, 10.0f, 0.0f } ) );
}
m_launchSpeedScale = 1.0f;
@ -1447,7 +1453,7 @@ public:
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.name = "cylinder";
bodyDef.type = b3_dynamicBody;
bodyDef.position = b3Vec3{ 5020.27734, 3506.22559, -6986.48584 };
bodyDef.position = { 5020.27734f, 3506.22559f, -6986.48584f };
bodyDef.rotation = { { 0.664546967, 0.669287264, 0.135021493 }, 0.303646326 };
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
@ -1497,7 +1503,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 30.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 30.0f, b3Pos_zero );
}
{
@ -1518,7 +1524,7 @@ public:
{
b3HullData* cylinderHull = b3CreateCylinder( 1.0f, 0.25f, 0.0f, 8 );
b3Vec3 positions[6] = {
b3Pos positions[6] = {
{ 0.0f, -10.2f, 0.0f }, { 0.0f, 9.2f, 0.0f }, { -9.8f, 0.0f, 0.0f },
{ 9.8f, 0.0f, 0.0f }, { 0.0f, 0.0f, -9.8f }, { 0.0f, 0.0f, 9.8f },
};
@ -1535,7 +1541,7 @@ public:
{
b3Capsule capsule ={{0.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, 0.25f};
b3Vec3 positions[8] = {
b3Pos positions[8] = {
{ 0.0f, -10.2f, 2.0f }, { 0.0f, 9.2f, 2.0f },
{ 0.0f, -9.9f, 4.0f }, { 0.0f, 8.9f, 4.0f },
{ -9.8f, 2.0f, 0.0f },
@ -1561,7 +1567,7 @@ public:
Sample::Render();
b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity };
DrawAxes( transform, 1.0f );
DrawAxes( b3MakeWorldTransform( transform ), 1.0f );
}
void Step() override

View file

@ -16,7 +16,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Pos_zero );
}
AddGroundBox( 20.0f );
@ -86,7 +86,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Pos_zero );
}
{
@ -221,7 +221,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 180.0f, 30.0f, 20.0f, b3Vec3_zero );
m_camera->SetView( 180.0f, 30.0f, 20.0f, b3Pos_zero );
}
b3BodyDef bodyDef = b3DefaultBodyDef();
@ -234,7 +234,7 @@ public:
for ( int i = 0; i < e_count; ++i )
{
b3Vec3 position = { 0.1f * i, 2.0f + 0.5f * i, -0.1f * i };
b3Pos position = { 0.1f * i, 2.0f + 0.5f * i, -0.1f * i };
float torque = 10.0f;
float hertz = 0.5f;
float damping = 0.7f;
@ -269,7 +269,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( -20.0f, 30.0f, 25.0f, b3Vec3_zero );
m_camera->SetView( -20.0f, 30.0f, 25.0f, b3Pos_zero );
}
b3ShapeDef shapeDef = b3DefaultShapeDef();
@ -292,7 +292,7 @@ public:
}
m_human = {};
b3Vec3 position = { -12.0f, 6.0f, 0.0f };
b3Pos position = { -12.0f, 6.0f, 0.0f };
float torque = 10.0f;
float hertz = 2.0f;
float damping = 0.7f;

View file

@ -21,7 +21,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 30.0f, 15.0f, 70.0f, b3Vec3_zero );
m_camera->SetView( 30.0f, 15.0f, 70.0f, b3Pos_zero );
}
@ -136,7 +136,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 45.0f, 20.0f, 15.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 20.0f, 15.0f, b3Pos_zero );
}
m_bodyIds = nullptr;
@ -442,7 +442,7 @@ public:
{
if ( m_context->restart == false )
{
m_camera->SetView( 30.0f, 35.0f, 15.0f, b3Vec3_zero );
m_camera->SetView( 30.0f, 35.0f, 15.0f, b3Pos_zero );
}

View file

@ -156,7 +156,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 30.0f, 150.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 30.0f, 150.0f, b3Pos_zero );
}
AddGroundBox( 100.0f );
@ -170,7 +170,7 @@ public:
{
float alpha = B3_PI / 16.0f * index;
b3CosSin cs = b3ComputeCosSin( alpha );
b3Vec3 position = { 15.0f * cs.cosine, 1.0f, 15.0f * cs.sine };
b3Pos position = { 15.0f * cs.cosine, 1.0f, 15.0f * cs.sine };
b3Quat orientation = b3MakeQuatFromAxisAngle( { 0.0f, 1.0f, 0.0f }, -alpha );
b3Vec3 velocity = { 25.0f * cs.cosine, 0.0f, 25.0f * cs.sine };
@ -648,10 +648,9 @@ public:
continue;
}
b3Vec3 p = 1.0f / 3.0f * ( v1 + v2 + v3 );
p = b3TransformPoint( m_meshTransform, p );
b3Pos p = b3TransformWorldPoint( m_meshTransform, 1.0f / 3.0f * ( v1 + v2 + v3 ) );
DrawWorldString( p, MakeColor( b3_colorAqua ), "%d", i );
DrawString3D( p, MakeColor( b3_colorAqua ), "%d", i );
int materialIndex = materialIndices[i];
@ -659,7 +658,7 @@ public:
DrawLine( p, p + v, MakeColor( b3_colorBlueViolet ) );
}
DrawAxes( b3Transform_identity, 0.5f );
DrawAxes( b3WorldTransform_identity, 0.5f );
}
static Sample* Create( SampleContext* context )
@ -667,7 +666,7 @@ public:
return new ConveyorMesh( context );
}
b3Transform m_meshTransform;
b3WorldTransform m_meshTransform;
b3MeshData* m_meshData;
b3HullData* m_cylinderHull;
b3Vec3 m_velocities[7];
@ -817,8 +816,8 @@ public:
b3Vec3 rand = RandomVec3( { -0.3f, -0.3f, -0.3f }, { 0.3f, 0.3f, 0.3f } );
m_noise = b3Lerp( m_noise, rand, 0.05f );
b3Vec3 p1 = { 0.0f, 0.5f, 0.0f };
b3Vec3 p2 = b3MulAdd( p1, 0.2f, wind );
b3Pos p1 = { 0.0f, 0.5f, 0.0f };
b3Pos p2 = p1 + b3MulSV( 0.2f, wind );
DrawArrow( p1, p2, MakeColor( b3_colorFuchsia ) );
}
}

View file

@ -275,7 +275,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 25.0f, 10.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 25.0f, 10.0f, b3Pos_zero );
}
AddGroundBox( 20.0f );
@ -298,7 +298,7 @@ public:
{
Sample::Step();
b3Vec3 position = b3Body_GetPosition( m_bodyId );
b3Pos position = b3Body_GetPosition( m_bodyId );
DrawTextLine( "(x, y, z) = (%.2g, %.2g, %.2g)", position.x, position.y, position.z );
}
@ -320,7 +320,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 10.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 10.0f, b3Pos_zero );
}
AddGroundBox( 10.0f );
@ -575,11 +575,11 @@ public:
{
if (m_isDebug)
{
m_camera->SetView( 0.0f, 15.0f, 25.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 25.0f, b3Pos_zero );
}
else
{
m_camera->SetView( 0.0f, 15.0f, 75.0f, b3Vec3_zero );
m_camera->SetView( 0.0f, 15.0f, 75.0f, b3Pos_zero );
}
}
@ -605,9 +605,9 @@ public:
// for (float Alpha = 0; Alpha <= 4.0f; Alpha += 4.0f)
{
b3CosSin cs = b3ComputeCosSin( B3_DEG_TO_RAD * alpha );
b3Vec3 position = { radius * cs.cosine, 0.8f, radius * cs.sine };
b3Pos position = { radius * cs.cosine, 0.8f, radius * cs.sine };
b3Vec3 normal = { cs.cosine, 0.0f, cs.sine };
position -= alpha / 630.0f * normal;
position = position - alpha / 630.0f * normal;
b3Quat orientation = b3MakeQuatFromAxisAngle( b3Vec3_axisY, -B3_DEG_TO_RAD * alpha );
@ -645,7 +645,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 75.0f, 10.0f, 10.0f );
m_camera->SetView( 75.0f, 10.0f, 10.0f, b3Pos_zero );
}
AddGroundBox( 20.0f );

View file

@ -150,7 +150,7 @@ public:
{
if ( context->restart == false )
{
m_camera->SetView( 45.0f, 45.0f, 250.0f, b3Vec3_zero );
m_camera->SetView( 45.0f, 45.0f, 250.0f, b3Pos_zero );
}
m_fileNames[0] = { "bounds01" };
@ -161,7 +161,7 @@ public:
m_config[2] = { 0.01f, false };
m_fileIndex = 0;
m_closestPoint = b3Vec3_zero;
m_closestPoint = b3Pos_zero;
m_timeStamp = 1;
m_doOverlap = false;
m_doClosest = false;
@ -510,7 +510,7 @@ public:
float distanceSquared = b3DistanceSquared( queryPoint, closestPoint );
if ( distanceSquared < minDistanceSquared )
{
sample->m_closestPoint = closestPoint;
sample->m_closestPoint = b3ToPos( closestPoint );
sample->m_haveClosest = true;
}
@ -606,7 +606,7 @@ public:
b3Vec3 point = m_closestPointQueries[i].center;
float radius = m_closestPointQueries[i].radius;
float distanceSquared = radius * radius;
m_closestPoint = point;
m_closestPoint = b3ToPos( point );
m_haveClosest = false;
b3DynamicTree_QueryClosest( &m_tree, point, B3_DEFAULT_MASK_BITS, false, ClosetPointCallback, this,
&distanceSquared );
@ -677,9 +677,9 @@ public:
{
Sample::Render();
DrawAxes( b3Transform_identity, 2.0f );
DrawAxes( b3WorldTransform_identity, 2.0f );
b3Vec3 cp = m_camera->GetPosition();
b3Pos cp = m_camera->m_worldEye;
b3TreeNode* nodes = m_tree.nodes;
float distSquared = m_drawDistance * m_drawDistance * 1000.0f * 1000.0f;
@ -704,7 +704,7 @@ public:
}
b3Vec3 c = b3AABB_Center( node->aabb );
if ( m_drawLevel < 10 || b3DistanceSquared( c, cp ) < distSquared )
if ( m_drawLevel < 10 || b3LengthSquared( b3SubPos( cp, b3ToPos( c ) ) ) < distSquared )
{
DrawBounds( node->aabb, 0.0f, MakeColor( colors[m_drawLevel % colorCount] ) );
}
@ -723,7 +723,7 @@ public:
}
b3Vec3 c = b3AABB_Center( node->aabb );
if ( b3DistanceSquared( c, cp ) > distSquared )
if ( b3LengthSquared( b3SubPos( cp, b3ToPos( c ) ) ) > distSquared )
{
continue;
}
@ -745,7 +745,7 @@ public:
if ( m_doRay )
{
Ray ray = m_rays[m_testIndex];
DrawLine( ray.origin, ray.origin + ray.translation, MakeColor( b3_colorRed ) );
DrawLine( b3ToPos( ray.origin ), b3ToPos( ray.origin + ray.translation ), MakeColor( b3_colorRed ) );
}
if ( m_doOverlap )
@ -755,7 +755,7 @@ public:
if ( m_doClosest )
{
DrawSolidSphere( b3Transform_identity, m_closestPointQueries[m_testIndex], MakeColor( b3_colorCyan ) );
DrawSolidSphere( b3WorldTransform_identity, m_closestPointQueries[m_testIndex], MakeColor( b3_colorCyan ) );
if ( m_haveClosest )
{
DrawPoint( m_closestPoint, 15.0f, MakeColor( b3_colorOrange ) );
@ -786,7 +786,7 @@ public:
b3Vec3 point = m_closestPointQueries[m_testIndex].center;
float radius = m_closestPointQueries[m_testIndex].radius;
float distanceSquared = radius * radius;
m_closestPoint = point;
m_closestPoint = b3ToPos( point );
m_haveClosest = false;
b3DynamicTree_QueryClosest( &m_tree, point, B3_DEFAULT_MASK_BITS, false, ClosetPointCallback, this,
&distanceSquared );
@ -825,7 +825,7 @@ public:
Ray m_rays[m_testCount];
b3AABB m_overlapQueries[m_testCount];
b3Sphere m_closestPointQueries[m_testCount];
b3Vec3 m_closestPoint;
b3Pos m_closestPoint;
float m_areaRatio;
float m_rayTime;
float m_overlapTime;

308
samples/sample_world.cpp Normal file
View file

@ -0,0 +1,308 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "gfx/debug_adapter.h"
#include "human.h"
#include "imgui.h"
#include "sample.h"
#include "stability.h"
#include "box3d/box3d.h"
// Builds an identical box stack a long way from the origin to show off double precision world
// positions. With double precision on, a stack at 1e7 settles exactly like one at the origin and
// renders crisply because the debug draw origin tracks the content. A float build of the same
// sample keeps full speed but loses sub-meter resolution far out, so the stack snaps to a coarse
// grid and jitters. The settled height readout stays put at any offset in double, and drifts in float.
class FarStack : public Sample
{
public:
static constexpr float m_maxOffset = 10000.0f;
explicit FarStack( SampleContext* context )
: Sample( context )
{
// Double precision opens at the dramatic offset, float opens at the origin so it is usable
// out of the box. Either way the slider sweeps the full range.
m_offsetKilometers = b3IsDoublePrecision() ? m_maxOffset : 0.0f;
m_columnCount = 6;
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 8.0f, 16.0f, { 0.0f, 2.0f, 0.0f } );
}
BuildScene();
}
// Place the ground and stack at the current offset and aim the camera at them. The draw origin
// rides the camera eye, so the float renderer works in a small relative frame near the content
// no matter how large the offset.
void BuildScene()
{
b3Pos base = { 1000.0f * m_offsetKilometers, 0.0f, 0.0f };
m_base = base;
m_camera->m_pivot = b3OffsetPos( base, { 0.0f, 2.0f, 0.0f } );
m_camera->UpdateTransform();
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.name = "ground";
bodyDef.position = b3OffsetPos( base, { 0.0f, -1.0f, 0.0f } );
b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3BoxHull groundHull = b3MakeBoxHull( 12.0f, 1.0f, 12.0f );
b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &groundHull.base );
SetGroundShape( groundShapeId );
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
b3BodyDef boxDef = b3DefaultBodyDef();
boxDef.type = b3_dynamicBody;
b3ShapeDef boxShape = b3DefaultShapeDef();
m_topBodyId = b3_nullBodyId;
for ( int i = 0; i < m_columnCount; ++i )
{
// A small alternating skew so a float build visibly drifts rather than balancing by luck.
float skew = 0.02f * ( i & 1 ? 1.0f : -1.0f );
boxDef.position = b3OffsetPos( base, { skew, 0.5f + 1.0f * i, 0.0f } );
b3BodyId body = b3CreateBody( m_worldId, &boxDef );
b3CreateHullShape( body, &boxShape, &box.base );
m_topBodyId = body;
}
}
void Rebuild()
{
b3Capacity capacity = {};
CreateWorld( &capacity );
BuildScene();
}
bool DrawControls() override
{
const float presets[] = { 0.0f, 10.0f, 100.0f, 1000.0f, m_maxOffset };
const char* labels[] = { "origin", "10km", "100km", "1000km", "10000km" };
for ( int i = 0; i < 5; ++i )
{
if ( ImGui::Button( labels[i] ) )
{
m_offsetKilometers = presets[i];
Rebuild();
}
}
return true;
}
void Step() override
{
Sample::Step();
// Height of the top box above the ground, measured in the offset's own frame. This holds
// steady at any offset under double precision and drifts once float runs out of resolution.
b3Vec3 top = b3SubPos( b3Body_GetWorldCenterOfMass( m_topBodyId ), m_base );
DrawTextLine( "double precision: %s", b3IsDoublePrecision() ? "ON" : "OFF" );
DrawTextLine( "world offset: %.1f km", m_offsetKilometers );
DrawTextLine( "top box height above ground: %.4f m", top.y );
}
static Sample* Create( SampleContext* context )
{
return new FarStack( context );
}
float m_offsetKilometers;
int m_columnCount;
b3BodyId m_topBodyId;
b3Pos m_base; // world position of the offset content, the frame the height readout uses
};
static int sampleLargeWorld = RegisterSample( "World", "Far Stack", FarStack::Create );
class FarPyramid : public Sample
{
public:
static constexpr float m_offsetKilometers = 10000.0f;
explicit FarPyramid( SampleContext* context )
: Sample( context )
{
b3Pos base = { 1000.0f * m_offsetKilometers, 0.0f, 0.0f };
if ( context->restart == false )
{
m_camera->SetView( 40.0f, -10.0f, 60.0f, b3OffsetPos( base, { 0.0f, 20.0f, 0.0f } ) );
}
int baseCount = 40;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = b3OffsetPos( base, { 0.0f, -1.0f, 0.0f } );
b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3BoxHull groundHull = b3MakeBoxHull( 400.0f, 1.0f, 400.0f );
b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &groundHull.base );
SetGroundShape( groundShapeId );
float h = 0.5f;
float shift = h;
b3BoxHull box = b3MakeBoxHull( h, h, h );
shapeDef.density = 100.0f;
bodyDef.type = b3_dynamicBody;
for ( int i = 0; i < baseCount; ++i )
{
float y = ( 2.0f * i + 1.0f ) * shift;
for ( int j = i; j < baseCount; ++j )
{
float x = ( i + 1.0f ) * shift + 2.0f * ( j - i ) * shift - h * baseCount;
bodyDef.position = b3OffsetPos( base, { x, y, 0.0f } );
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
}
}
}
void Step() override
{
Sample::Step();
DrawTextLine( "double precision: %s", b3IsDoublePrecision() ? "ON" : "OFF" );
DrawTextLine( "pyramid built %.0f km from the world origin", m_offsetKilometers );
}
static Sample* Create( SampleContext* context )
{
return new FarPyramid( context );
}
};
static int sampleFarPyramid = RegisterSample( "World", "Far Pyramid", FarPyramid::Create );
class FarRagdolls : public Sample
{
public:
static constexpr int m_count = 20;
static constexpr float m_offsetKilometers = 1000.0f;
explicit FarRagdolls( SampleContext* context )
: Sample( context )
{
b3Pos base = { 1000.0f * m_offsetKilometers, 0.0f, 0.0f };
if ( context->restart == false )
{
m_camera->SetView( 180.0f, 30.0f, 20.0f, base );
}
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = b3OffsetPos( base, { 0.0f, -1.0f, 0.0f } );
b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
m_groundMesh = b3CreateGridMesh( 20, 20, 1.0f, 1, true );
b3CreateMeshShape( groundId, &shapeDef, m_groundMesh, b3Vec3_one );
for ( int i = 0; i < m_count; ++i )
{
b3Vec3 offset = {
0.15f * ( i - 0.5f * m_count ),
2.0f + 0.25f * i,
0.15f * ( 0.5f * m_count - i ),
};
b3Pos position = b3OffsetPos( base, offset );
float torque = 10.0f;
float hertz = 0.5f;
float damping = 0.7f;
CreateHuman( m_humans + i, m_worldId, position, torque, hertz, damping, i, nullptr, false );
}
}
~FarRagdolls() override
{
b3DestroyMesh( m_groundMesh );
}
void Step() override
{
Sample::Step();
DrawTextLine( "double precision: %s", b3IsDoublePrecision() ? "ON" : "OFF" );
DrawTextLine( "%d ragdolls piled %.0f km from the world origin", m_count, m_offsetKilometers );
}
static Sample* Create( SampleContext* context )
{
return new FarRagdolls( context );
}
b3MeshData* m_groundMesh;
Human m_humans[m_count] = {};
};
static int sampleFarRagdolls = RegisterSample( "World", "Far Ragdolls", FarRagdolls::Create );
// The Mesh Drop unit test, run 1000 km out in both x and z. A field of thin boxes rains onto a
// wave mesh and must come to rest. Continuous collision and the contact solver work in delta
// space, so the pile settles far from the origin exactly as it does at the origin. Past 300 steps
// any body still moving trips the failure readout, the same check the unit test uses.
class FarMeshDrop : public Sample
{
public:
static constexpr float m_offsetKilometers = 1000.0f;
explicit FarMeshDrop( SampleContext* context )
: Sample( context )
{
b3Pos base = { 1000.0f * m_offsetKilometers, 0.0f, 1000.0f * m_offsetKilometers };
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 20.0f, base );
GetGuiDraw()->forceScale = 0.1f;
}
m_data = CreateMeshDrop( m_worldId, base );
m_failed = false;
}
~FarMeshDrop() override
{
DestroyMeshDrop( &m_data );
}
void Step() override
{
Sample::Step();
// Once the pile should have settled, any remaining body motion means it did not come to
// rest far from the origin.
if ( m_failed == false && m_stepCount >= 300 )
{
b3BodyEvents bodyEvents = b3World_GetBodyEvents( m_worldId );
if ( bodyEvents.moveCount > 0 )
{
m_failed = true;
}
}
DrawTextLine( "double precision: %s", b3IsDoublePrecision() ? "ON" : "OFF" );
DrawTextLine( "mesh drop running %.0f km from the world origin", m_offsetKilometers );
if ( m_failed )
{
DrawTextLine( "failed!" );
}
}
static Sample* Create( SampleContext* context )
{
return new FarMeshDrop( context );
}
MeshDropData m_data;
bool m_failed;
};
static int sampleFarMeshDrop = RegisterSample( "World", "Far Mesh Drop", FarMeshDrop::Create );

View file

@ -1,286 +0,0 @@
# Shaders
This directory contains all GPU shader code for the renderer. Authored in
GLSL, cross-compiled to per-backend variants (HLSL for D3D11, MSL for Metal,
GLSL 410 for desktop GL) by **sokol-shdc**.
Read this file before writing or editing any `.glsl` file. The root
`CLAUDE.md` covers project-wide conventions; this file covers shader-specific
rules that don't apply elsewhere.
## Toolchain
- Author in **GLSL 4.30 core** dialect, with sokol-shdc directives.
- sokol-shdc generates a single C header per shader file, containing the
bytecode/source for every enabled backend plus pipeline reflection.
- Generated headers live in `shaders/generated/` and **are committed**, so a
fresh clone builds without sokol-shdc. `BOX3D_BUILD_SHADERS` (default ON)
recompiles them in place via a CMake custom command; build with it OFF to
use the committed headers and fetch no tooling.
- Shader edits trigger the custom command automatically; run
`cmake --build build --target shaders` to force a rebuild.
Backends configured in `cmake/Shaders.cmake`:
- `hlsl5` for D3D11 (Windows)
- `metal_macos` for Metal (macOS)
- `glsl430` for GL 4.3 core (Linux). 4.3 is the floor sokol_gfx requires
for storage buffers; we use storage buffers for per-instance data.
If sokol-shdc rejects a shader, it's a hard build break - there is no
fallback path. Fix the shader, don't disable a backend.
## File layout
One shader program per `.glsl` file. The filename is the program name.
```
shaders/
CLAUDE.md this file
sky.glsl procedural Preetham sky
common/ shared @include fragments
pbr.glsl PBR helpers (BRDF, IBL evaluation)
preetham.glsl Preetham sky radiance function
xe_gtao_common.glsl XeGTAO helper block (PrefilterDepth,
MainPass, Denoise primitives)
shapes/
cube.glsl instanced cube
sphere.glsl ray-cast sphere impostor
capsule.glsl ray-cast capsule impostor
geom.glsl hulls + meshes + heightfields
edge.glsl per-shape edge overlay
shadow_caster_cube.glsl depth-only shadow caster,
shadow_caster_sphere.glsl per-shape, one per primitive class
shadow_caster_capsule.glsl
shadow_caster_geom.glsl
ibl/ one-time cubemap / LUT builders
sky_to_cube.glsl rasterize sky into a cubemap face
prefilter.glsl GGX prefilter the IBL cubemap
brdf_lut.glsl split-sum integration LUT
post/
depth_only_cube.glsl GTAO/shadow prepass per primitive
depth_only_sphere.glsl class - outputs linear view-Z
depth_only_capsule.glsl
depth_only_geom.glsl
gtao_prefilter_depth.glsl XeGTAO compute
gtao_main_pass.glsl
gtao_denoise.glsl
mask_sphere.glsl hover/select highlight mask,
mask_capsule.glsl uint output, GREATER depth test
mask_hull.glsl
highlight_outline.glsl edge-detect + premult-alpha composite
tonemap.glsl AgX
overlays/
line.glsl shader-expanded lines
point.glsl shader-expanded points
```
Each shader currently inlines its own UBO declarations (`ub_frame`,
`ub_pass`, `ub_draw`) - there is no shared `frame_ubo.glsl` /
`pass_ubo.glsl` block to `@include`. The three shared blocks that do
exist are `common/pbr.glsl`, `common/preetham.glsl`, and
`common/xe_gtao_common.glsl`; the lit shape shaders `@include` PBR and
the sky shaders include Preetham. Shadow-cascade sampling is inlined
per shape shader (`sampleShadowPCF` / `sampleCascade` /
`sampleShadowCascaded` in each of cube/sphere/capsule/geom) - keep
those four implementations in sync if you change the algorithm.
A shader file has, in order:
1. `@ctype` directives mapping GLSL types to C types (mat4 -> Mat4,
vec4 -> Vec4, vec3 -> b3Vec3, vec2 -> b3Vec2).
2. `@include` of any common blocks needed.
3. `@vs` block - vertex stage.
4. `@fs` block - fragment stage.
5. `@program <name> <vs> <fs>` declaration.
Don't put multiple programs in one file. Don't share `@vs` or `@fs` blocks
across programs except via `@include`.
## Naming inside shaders
- Uniform blocks: `ub_frame`, `ub_pass`, `ub_draw`. Lowercase, prefixed `ub_`.
- Samplers: `smp_<purpose>` (e.g. `smp_shadow`, `smp_blue_noise`).
- Textures: `tex_<purpose>` (e.g. `tex_depth`, `tex_normal`, `tex_shadow`).
- Vertex inputs: `in_<name>` (e.g. `in_pos`, `in_normal`, `in_inst_xform0`).
- Varyings (vs->fs): `v_<name>` (e.g. `v_view_pos`, `v_world_normal`).
- Fragment outputs: `out_<name>` (e.g. `out_color`, `out_normal`, `out_id`).
- Locals: `snake_case`. Constants: `SCREAMING_SNAKE_CASE`.
## Uniform organization
Three UBO frequencies, **never** mixed:
```glsl
// ub_frame - updated once per frame, bound at slot 0
layout(binding=0) uniform ub_frame {
mat4 view;
mat4 proj;
mat4 view_proj;
mat4 inv_view_proj;
vec4 camera_pos; // .xyz = pos, .w = time
vec4 viewport; // .xy = size px, .zw = 1/size
vec4 sun_dir; // .xyz = world-space dir TO sun, .w = unused
vec4 sun_color; // .rgb = color * intensity, .a = ambient strength
};
// ub_pass - updated once per render pass, bound at slot 1
layout(binding=1) uniform ub_pass {
// per-pass data: shadow matrices, post-process knobs, etc.
};
// ub_draw - updated per draw call, bound at slot 2
layout(binding=2) uniform ub_draw {
// small per-draw data; instance arrays go in storage buffers, not UBOs
};
```
Per-instance data lives in **storage buffers**, indexed by `gl_InstanceIndex`.
Don't pack instance data into UBOs - they're size-limited and the wrong tool.
A UBO declared at `binding=N` is **stage-specific** in sokol-shdc - declaring
the same UBO in both `@vs` and `@fs` collides. Place each UBO in the stage
that reads it; if a value is needed in both stages, either replicate it at
a different binding, precompute on the C side and split into VS/FS-only
UBOs, or pass via a varying. The three-tier scheme above describes update
frequencies, not a guarantee that every shader exposes all three to both
stages.
## Coordinate spaces
Be explicit about which space a value lives in. Suffix variable names:
`*_world`, `*_view`, `*_clip`, `*_ndc`, `*_screen`. Mixing spaces silently
is the single most common shader bug.
- **Reverse-Z**: clip-space Z is 1 at near, 0 at far. Depth comparison is
`GREATER`. Far-plane clears to 0, not 1.
- **View space**: right-handed, Y-up, Z out of screen (camera looks down -Z).
- **NDC**: `[-1, 1]` for X and Y; Z is `[0, 1]` (reverse-Z).
- **UV**: `[0, 1]`, top-left origin in sampling. sokol-shdc handles the
GL/D3D/Metal Y-flip difference automatically when sampling render targets;
don't flip manually.
When reconstructing world position from depth, sample reverse-Z and
transform through `inv_view_proj` (declared in the per-shader UBO that
needs it - see `sky.glsl`, `overlays/line.glsl`, the lit shape
shaders). There is no shared math helper for this - it's inlined where
needed.
## Flat shading
Compute view-space normal from `dFdx`/`dFdy` of view-space position:
```glsl
vec3 dx = dFdx(v_view_pos);
vec3 dy = dFdy(v_view_pos);
vec3 n_view = normalize(cross(dx, dy));
```
This works for any triangle mesh and gives free flat shading without
duplicating vertices. Impostors compute their analytic normal directly
and skip this.
Do **not** pass per-vertex normals as varyings unless the shape needs
smooth shading (which, in this renderer, none of them do).
## Impostor shaders
Sphere and capsule impostors all follow the same structure:
1. Vertex shader emits a bounding proxy (cube for spheres, OBB for capsules).
2. Fragment shader reconstructs the view ray from `gl_FragCoord` and
`inv_view_proj`.
3. Ray-cast against the analytic primitive in **world space** (not view
space - keeps the math simpler when transforming ray origins).
4. On miss: `discard`.
5. On hit: write `gl_FragDepth` from the hit point in clip space, output
the analytic surface normal in view space.
Reference math: Inigo Quilez, https://iquilezles.org/articles/intersectors/.
`gl_FragDepth` writes disable early-Z. That's fine for impostors but it
means the impostor pipelines don't get the hi-Z optimization. Don't write
`gl_FragDepth` from triangle-mesh shaders - let the rasterizer do it.
When writing `gl_FragDepth`, write the **reverse-Z** value (1 at near,
0 at far). The impostor shaders show the pattern: compute clip-space Z
from a world-space hit point via the view-proj matrix, then
`gl_FragDepth = clip.z / clip.w`.
## Shadow sampling
The four lit shape shaders (`cube.glsl`, `sphere.glsl`, `capsule.glsl`,
`geom.glsl`) each inline their own `sampleShadowPCF` /
`sampleCascade` / `sampleShadowCascaded` functions. They read
`cascade_matrices[3]`, `cascade_far_view_z`, and the shadow-atlas
sampler from the per-shape UBO + per-pass binding. The four
implementations are intentionally identical - if you change the
algorithm (kernel, bias formula, cascade-blend logic), update all four
at once. A future cleanup would lift them into a shared
`common/shadow.glsl` block; until then, treat the four copies as one
unit.
Don't sample shadow maps from any other shader without going through
the same helper trio - non-standard sample patterns (PCSS,
contact-hardening) belong inside `sampleShadowPCF` behind a flag, not
forked into new call sites.
## Things to NOT do
- Don't use `#ifdef <BACKEND>` to branch on backend. sokol-shdc handles
cross-compilation; backend-specific shader code means we've lost.
- Don't sample a depth target with a regular sampler when you need raw
depth values. Use the depth-specific sampler binding declared in the
pass - drivers compare-sample by default on depth textures otherwise.
- Don't use derivatives (`dFdx`/`dFdy`/`fwidth`) in non-uniform control
flow. They produce undefined results inside `if`/`discard` branches.
Compute derivatives at top level, then branch.
- Don't write to `gl_FragDepth` conditionally (some pixels write, others
don't). Either every pixel writes it or none do - otherwise the early-Z
decision is per-draw, not per-pixel, and you get the worst of both.
- Don't use `texture()` without an explicit LOD in fragment shaders that
contain `discard` or `gl_FragDepth` writes after the sample. The
derivative for mipmap selection becomes undefined. Use `textureLod()`
with LOD 0 if mipmapping isn't needed.
- Don't pack different update frequencies into one UBO. Use the three-tier
scheme above.
- Don't store full 4x4 instance transforms. 3x4 affine; the bottom row is
always `(0, 0, 0, 1)` and we don't need perspective from instance data.
- Don't encode normals as `vec3` in storage buffers if you're tight on
bandwidth - octahedral encoding to two `f16`s saves the byte. There's
no shared helper yet; if a second use case comes up, lift it into
`common/`. Don't do this preemptively; profile first.
- Don't write debug colors (red for "this branch ran") and leave them in.
Use the debug view-mode mechanism instead - it's a uniform switch, not
edited shader code.
- Don't add new uniform blocks without updating the C-side reflection.
sokol-shdc generates `_uniformblock_size()` and `_uniformblock_slot()`
helpers; use them, don't hardcode sizes or slot indices.
## Debugging shaders
- The **debug view-modes** machinery is controlled by
`fi.debug_view_mode` and runtime keys 0-6. A single uniform selects:
lit, view-space distance, CSM cascade index, view-space normal, raw
GTAO AO. This is faster than RenderDoc for 90% of issues.
- Use **RenderDoc** on Linux/Windows or **Xcode GPU frame capture** on
macOS for the other 10%. Both work with sokol's backends.
- When a shader looks wrong, check coordinate spaces first, depth
reverse-Z second, derivatives in branches third. In that order.
- sokol's validation layer catches most binding mistakes. If something
renders black, check the validation log before reading shader code.
## When adding a new shader
1. Decide which file it belongs in (or create a new one if it's a new
pipeline). One program per file.
2. Add `@ctype` directives matching the C-side struct layouts.
3. Pick the right UBO tier for each uniform - frame, pass, or draw.
4. Author the shader; build with `cmake --build build --target shaders`
and watch sokol-shdc output for warnings (it's strict, that's good).
5. Make sure at least one scene in `main.cpp`'s `k_sceneTable` exercises
the new pipeline - the smoke test cycles every scene, so a scene
that touches the shader is render3d's regression net for sokol
validation errors. If no existing scene covers it, add a new
`src/app/*_scene.{cpp,h}` (mirror the `pbr_scene` / `gtao_scene`
pattern) and wire it through `SceneKind`.
6. Update this file if the shader introduces a new convention worth
documenting.

View file

@ -204,24 +204,30 @@ float max_lod)
// at any distance / viewing angle. Caller passes a flat per-instance
// `cell_size`, the result is suitable as `base_color` for brdf_evaluate.
//
// `line_xz` is the world XZ with the draw origin wrapped to the grid period,
// so the repeating lines stay precise far from the world origin. `axis_xz` is
// the true world XZ for the colored origin axes, which there is only one of, so
// they cannot be wrapped. Far from the origin axis_xz is large and the axes
// vanish, which is what we want (the origin is not in view).
//
// Derivatives are valid here only because the geom shader calls this
// from a branch on a flat per-instance varying (every fragment in a
// quad of a single draw shares the same materialMode).
vec3 proceduralGrid(vec2 world_xz, vec3 base_color, float cell_size)
vec3 proceduralGrid(vec2 line_xz, vec2 axis_xz, vec3 base_color, float cell_size)
{
vec2 coord_minor = world_xz / cell_size;
vec2 coord_minor = line_xz / cell_size;
vec2 d_minor = max(fwidth(coord_minor), vec2(1.0e-6));
vec2 g_minor = abs(fract(coord_minor - 0.5) - 0.5) / d_minor;
float line_minor = 1.0 - clamp(min(g_minor.x, g_minor.y), 0.0, 1.0);
vec2 coord_major = world_xz / (cell_size * 10.0);
vec2 coord_major = line_xz / (cell_size * 10.0);
vec2 d_major = max(fwidth(coord_major), vec2(1.0e-6));
vec2 g_major = abs(fract(coord_major - 0.5) - 0.5) / d_major;
float line_major = 1.0 - clamp(min(g_major.x, g_major.y), 0.0, 1.0);
vec3 minor_color = base_color * 0.70;
vec3 major_color = base_color * 0.35;
vec3 result = mix(base_color, minor_color, line_minor);
result = mix(result, major_color, line_major);
@ -229,9 +235,9 @@ vec3 proceduralGrid(vec2 world_xz, vec3 base_color, float cell_size)
// lines, gated to the positive half so the origin reads as a corner.
// Saturated primaries are identical in linear and sRGB, so they need no
// encoding fixup before the BRDF.
vec2 d_axis = max(fwidth(world_xz), vec2(1.0e-6));
float axis_x = (1.0 - clamp(abs(world_xz.y) / d_axis.y, 0.0, 1.0)) * step(0.0, world_xz.x);
float axis_z = (1.0 - clamp(abs(world_xz.x) / d_axis.x, 0.0, 1.0)) * step(0.0, world_xz.y);
vec2 d_axis = max(fwidth(axis_xz), vec2(1.0e-6));
float axis_x = (1.0 - clamp(abs(axis_xz.y) / d_axis.y, 0.0, 1.0)) * step(0.0, axis_xz.x);
float axis_z = (1.0 - clamp(abs(axis_xz.x) / d_axis.x, 0.0, 1.0)) * step(0.0, axis_xz.y);
result = mix(result, vec3(1.0, 0.0, 0.0), axis_x);
result = mix(result, vec3(0.0, 0.0, 1.0), axis_z);
return result;

View file

@ -94,7 +94,7 @@ float XeGTAO_ScreenSpaceToViewSpaceDepth(float screenDepth, GTAOConsts consts)
}
// XeGTAO.esh:104-115 with the Esoterica reverse-Z patch (negate Z so it
// points OUT of the screen in view space). render3d's view space is
// points OUT of the screen in view space). Box3D's view space is
// also right-handed Y-up with camera looking down -Z, matching Esoterica.
vec3 XeGTAO_ComputeViewspacePosition(vec2 screenPos, float viewspaceDepth, GTAOConsts consts) {
vec3 ret;

File diff suppressed because it is too large Load diff

View file

@ -44,7 +44,7 @@
// XeGTAO_DecodeGatherPartial path collapses to identity, we inline it.
//
// * `XE_GTAO_SHOW_EDGES` / `XE_GTAO_SHOW_BENT_NORMALS` debug-viz
// branches dropped, render3d uses its own debug_view_mode plumbing.
// branches dropped, Box3D uses its own debug_view_mode plumbing.
//
// * `finalApply` is delivered as a per-dispatch UBO field instead
// HLSL root constants. Lives in a small dedicated ub_denoise UBO at

View file

@ -13,7 +13,7 @@
//
// Departures from XeGTAO_PrefilterDepths16x16:
//
// * Input depth is **already positive linear view-Z** (render3d's
// * Input depth is **already positive linear view-Z** (Box3D's
// R32F prepass target), not NDC reverse-Z. Esoterica's
// XeGTAO_ScreenSpaceToViewSpaceDepth call collapses to a pass-
// through, we just clamp. GTAOConstants.DepthUnpackConsts is

View file

@ -108,6 +108,7 @@ layout( binding = 1 ) uniform ub_pass
vec4 sun_color; // .rgb = color, .a = ambient strength (flat-ambient fallback)
ivec4 flags; // .x = debug_view_mode (0 lit, 1 view-depth, 2 cascade-index)
vec4 camera_pos; // .xyz = camera world pos, .w unused
vec4 grid_offset; // .xy = origin wrapped to grid period (lines), .zw = full origin (axes)
mat4 view; // for view-space depth debug mode + cascade selection
vec4 cascade_far_view_z; // .xyz = far view-space Z per cascade
mat4 cascade_matrices[3]; // light-space view-proj per cascade
@ -306,7 +307,14 @@ void main()
vec3 baseColor = v_base_color.rgb;
if ( v_material.z > 0.5 )
{
vec3 grid_color = proceduralGrid( world_pos.xz, v_base_color.rgb, v_material.w );
// world_pos is in the camera relative frame. The grid lines use the
// origin wrapped to one grid period: the pattern repeats over it, so
// this draws the same lines but stays small enough for a float to
// resolve far from the world origin. The axes need the true origin, so
// they fade out far away instead of wrapping into ghost axes.
vec2 line_xz = world_pos.xz + grid_offset.xy;
vec2 axis_xz = world_pos.xz + grid_offset.zw;
vec3 grid_color = proceduralGrid( line_xz, axis_xz, v_base_color.rgb, v_material.w );
float grid_blend = pow( clamp( n_world.y, 0.0, 1.0 ), 8.0 );
baseColor = mix( v_base_color.rgb, grid_color, grid_blend );
}

View file

@ -66,7 +66,7 @@ void CreateJointGrid( b3WorldId worldId )
bodyDef.type = b3_dynamicBody;
}
bodyDef.position = (b3Vec3){ fk, -fi, 0.0f };
bodyDef.position = (b3Pos){ fk, -fi, 0.0f };
b3BodyId body = b3CreateBody( worldId, &bodyDef );
@ -106,10 +106,10 @@ void CreateLargePyramid( b3WorldId worldId )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = (b3Vec3){ 0.0f, -1.0f, 0.0f };
bodyDef.position = (b3Pos){ 0.0f, -1.0f, 0.0f };
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
b3BoxHull box = b3MakeBoxHull( 100.0f, 1.0f, 100.0f );
b3BoxHull box = b3MakeBoxHull( 400.0f, 1.0f, 400.0f );
b3ShapeDef shapeDef = b3DefaultShapeDef();
g_groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base );
}
@ -118,7 +118,7 @@ void CreateLargePyramid( b3WorldId worldId )
bodyDef.type = b3_dynamicBody;
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.density = 1.0f;
shapeDef.density = 100.0f;
float h = 0.5f;
b3BoxHull box = b3MakeBoxHull( h, h, h );
@ -133,7 +133,7 @@ void CreateLargePyramid( b3WorldId worldId )
{
float x = ( i + 1.0f ) * shift + 2.0f * ( j - i ) * shift - h * baseCount;
bodyDef.position = (b3Vec3){ x, y, 0.0f };
bodyDef.position = (b3Pos){ x, y, 0.0f };
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
@ -145,7 +145,7 @@ void CreateWidePyramid( b3WorldId worldId )
{
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = (b3Vec3){ 0.0f, -1.0f, 0.0f };
bodyDef.position = (b3Pos){ 0.0f, -1.0f, 0.0f };
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
b3BoxHull box = b3MakeBoxHull( 100.0f, 1.0f, 100.0f );
@ -174,7 +174,7 @@ void CreateWidePyramid( b3WorldId worldId )
float x = -pyramidHeight + boxSize * j + ( i & 1 ? halfBoxSize : 0.0f );
float y = 1.0f + ( boxSize + boxSeparation ) * i;
float z = -pyramidHeight + boxSize * k + ( i & 1 ? halfBoxSize : 0.0f );
bodyDef.position = (b3Vec3){ x, y, z };
bodyDef.position = (b3Pos){ x, y, z };
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
@ -201,7 +201,7 @@ static void CreateSmallPyramid( b3WorldId worldId, int baseCount, float extent,
for ( int j = i; j < baseCount; ++j )
{
float x = ( i + 1.0f ) * extent + 2.0f * ( j - i ) * extent + centerX - 0.5f;
bodyDef.position = (b3Vec3){ x, y, baseZ };
bodyDef.position = (b3Pos){ x, y, baseZ };
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
@ -219,7 +219,7 @@ void CreateManyPyramids( b3WorldId worldId )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = (b3Vec3){ 0.0f, -1.0f, 0.0f };
bodyDef.position = (b3Pos){ 0.0f, -1.0f, 0.0f };
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
@ -351,7 +351,7 @@ void CreateGroup( b3WorldId worldId, int rowIndex, int columnIndex )
float span = RAIN_GRID_COUNT * RAIN_GRID_SIZE;
float groupDistance = 1.0f * span / RAIN_GRID_COUNT;
b3Vec3 position;
b3Pos position;
position.x = -0.5f * span + groupDistance * ( columnIndex + 0.5f );
position.y = 20.0f;
position.z = -0.5f * span + groupDistance * ( rowIndex + 0.5f );
@ -468,7 +468,7 @@ void CreateLargeWorld( b3WorldId worldId )
for ( int j = 0; j < gridCount; ++j )
{
float z = -halfSpan + ( j + 0.5f ) * cell;
bodyDef.position = (b3Vec3){ x, 0.0f, z };
bodyDef.position = (b3Pos){ x, 0.0f, z };
b3BodyId body = b3CreateBody( worldId, &bodyDef );
b3CreateHullShape( body, &shapeDef, &box.base );
}
@ -512,7 +512,7 @@ void StepLargeWorld( b3WorldId worldId, int stepCount )
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = (b3Vec3){ x, 1.5f, z };
bodyDef.position = (b3Pos){ x, 1.5f, z };
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f };
@ -551,7 +551,7 @@ void CreateWasher( b3WorldId worldId )
float motorSpeed = 25.0f;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = (b3Vec3){ 0.0f, 21.0f, 0.0f };
bodyDef.position = (b3Pos){ 0.0f, 21.0f, 0.0f };
if ( kinematic == true )
{
@ -659,7 +659,7 @@ void CreateWasher( b3WorldId worldId )
float z = -2.0f * a * gridCount;
for ( int k = 0; k < gridCount; ++k )
{
bodyDef.position = (b3Vec3){ x, y, z };
bodyDef.position = (b3Pos){ x, y, z };
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &cube.base );
@ -685,7 +685,7 @@ static void CreateTrees( b3WorldId worldId, int scale )
// float tilt = 0.15f * B3_PI;
float tilt = 0.0f * B3_PI;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = (b3Vec3){ 0.0f, 0.0f, 0.0f };
bodyDef.position = (b3Pos){ 0.0f, 0.0f, 0.0f };
bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 1.0f, 0.0f, 0.0 }, tilt );
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
@ -732,7 +732,7 @@ static void CreateTrees( b3WorldId worldId, int scale )
float yTilt = cs.sine / cs.cosine;
for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex )
{
bodyDef.position = (b3Vec3){ 0.0, 1.0f - z * yTilt, z };
bodyDef.position = (b3Pos){ 0.0, 1.0f - z * yTilt, z };
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
for ( int shapeIndex = 0; shapeIndex < 22; ++shapeIndex )
@ -749,9 +749,9 @@ static void CreateTrees( b3WorldId worldId, int scale )
float velocityScale = 0.5f + ( 0.5f * bodyIndex ) / bodyCount;
b3Body_ApplyMassFromShapes( bodyId );
b3Vec3 center = b3Body_GetWorldCenterOfMass( bodyId );
b3Pos center = b3Body_GetWorldCenterOfMass( bodyId );
b3Vec3 omega = { 0.0f, 0.0f, velocityScale * angularVelocity };
b3Vec3 v = b3Cross( omega, b3Sub( center, bodyDef.position ) );
b3Vec3 v = b3Cross( omega, b3SubPos( center, bodyDef.position ) );
b3Body_SetAngularVelocity( bodyId, omega );
b3Body_SetLinearVelocity( bodyId, v );
@ -830,29 +830,7 @@ void CreateJunkyard( b3WorldId worldId )
}
}
{
b3HullData* hull;
{
float radius = 1.5f;
int pointCount = 10;
// Golden ratio
const float phi = ( 1.0f + sqrtf( 5.0f ) ) / 2.0f;
// Random points on sphere (Fibonacci lattice)
b3Vec3 points[10];
for ( int i = 0; i < pointCount; ++i )
{
float Theta = 2.0f * B3_PI * i / phi; // Azimuthal angle
float Z = 1.0f - ( 2.0f * i + 1.0f ) / pointCount; // Z coordinate
float Radius_XY = sqrtf( 1.0f - Z * Z ); // Radius in xy-plane
points[i].x = radius * Radius_XY * cosf( Theta );
points[i].y = radius * Radius_XY * sinf( Theta );
points[i].z = radius * Z;
}
hull = b3CreateHull( points, pointCount, pointCount );
}
b3HullData* rockHull = b3CreateRock( 1.5f );
int count = BENCHMARK_DEBUG ? 2 : 24;
float height = 24.0f;
@ -869,12 +847,12 @@ void CreateJunkyard( b3WorldId worldId )
bodyDef.position.y = 4.0f * Y + height + 1.0f;
bodyDef.position.z = -40.0f + 4.0f * Z;
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, hull );
b3CreateHullShape( bodyId, &shapeDef, rockHull );
}
}
}
b3DestroyHull( hull );
b3DestroyHull( rockHull );
}
g_junkyardData.radius = 35.0f;
@ -883,7 +861,7 @@ void CreateJunkyard( b3WorldId worldId )
b3HullData* hull = b3CreateCylinder( mHeight, 4.0f, 0.0f, 16 );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_kinematicBody;
bodyDef.position = (b3Vec3){ g_junkyardData.radius, 0.0f, 0.0f };
bodyDef.position = (b3Pos){ g_junkyardData.radius, 0.0f, 0.0f };
g_junkyardData.pusherId = b3CreateBody( worldId, &bodyDef );
g_junkyardData.degrees = 0.0f;
b3ShapeDef shapeDef = b3DefaultShapeDef();
@ -901,7 +879,7 @@ void StepJunkyard( b3WorldId worldId, int stepCount )
g_junkyardData.degrees += omega * timeStep;
b3CosSin cs = b3ComputeCosSin( g_junkyardData.degrees * B3_PI / 180.0f );
float r = g_junkyardData.radius;
b3Vec3 targetPos = { r * cs.cosine, 0.0f, r * cs.sine };
b3Transform target = { .p = targetPos, .q = b3Quat_identity };
b3Pos targetPos = { r * cs.cosine, 0.0f, r * cs.sine };
b3WorldTransform target = { .p = targetPos, .q = b3Quat_identity };
b3Body_SetTargetTransform( g_junkyardData.pusherId, target, timeStep, false );
}

View file

@ -21,7 +21,7 @@ static void CreateGroup( FallingRagdollData* data, b3WorldId worldId, int rowInd
float span = RAGDOLL_GRID_COUNT * GRID_SIZE;
float groupDistance = 1.0f * span / RAGDOLL_GRID_COUNT;
b3Vec3 position;
b3Pos position;
position.x = -0.5f * span + groupDistance * ( columnIndex + 0.5f );
position.y = 15.0f;
position.z = -0.5f * span + groupDistance * ( rowIndex + 0.5f );
@ -98,8 +98,8 @@ bool UpdateFallingRagdolls( b3WorldId worldId, FallingRagdollData* data )
for ( int b = 0; b < bone_count; ++b )
{
b3BodyId bodyId = human->bones[b].bodyId;
b3Transform xf = b3Body_GetTransform( bodyId );
data->hash = b3Hash( data->hash, (uint8_t*)( &xf ), sizeof( b3Transform ) );
b3WorldTransform xf = b3Body_GetTransform( bodyId );
data->hash = b3Hash( data->hash, (uint8_t*)( &xf ), sizeof( b3WorldTransform ) );
}
}
}

View file

@ -10,8 +10,8 @@
#include <assert.h>
#include <stddef.h>
void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float frictionTorque, float hertz,
float dampingRatio, int groupIndex, void* userData, bool colorize )
void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictionTorque, float hertz, float dampingRatio,
int groupIndex, void* userData, bool colorize )
{
assert( human->isSpawned == false );
@ -50,9 +50,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bone->parentIndex = -1;
bodyDef.name = "pelvis";
bone->referenceFrame = ( b3Transform ){ { 0.0f, 0.932087f, -0.051708f }, { { 0.739169f, 0.0f, 0.0f }, 0.673520f } };
bone->referenceFrame = (b3Transform){ { 0.0f, 0.932087f, -0.051708f }, { { 0.739169f, 0.0f, 0.0f }, 0.673520f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { 0.07f, 0.0f, -0.08f }, { -0.07f, 0.0f, -0.08f }, 0.13f };
@ -67,10 +67,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bone->parentIndex = bone_pelvis;
bodyDef.name = "spine_01";
bone->referenceFrame = ( b3Transform ){ { 0.0f, 1.113505f, -0.03481f }, { { 0.739973f, 0.0f, 0.0f }, 0.672637f } };
bone->referenceFrame = (b3Transform){ { 0.0f, 1.113505f, -0.03481f }, { { 0.739973f, 0.0f, 0.0f }, 0.672637f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
//bodyDef.type = b3_staticBody;
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
// bodyDef.type = b3_staticBody;
bone->bodyId = b3CreateBody( worldId, &bodyDef );
bodyDef.type = b3_dynamicBody;
@ -80,12 +80,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule );
bone->jointType = b3_sphericalJoint;
bone->localFrameA =
( b3Transform ){ { 0.000000, 0.000000, -0.182204 }, { { -0.999999, 0.000000, -0.000000 }, 0.001194 } };
bone->localFrameB =
( b3Transform ){ { 0.000000, 0.000000, -0.007736 }, { { -1.000000, 0.000000, -0.000000 }, 0.000000 } };
bone->localFrameA = (b3Transform){ { 0.000000, 0.000000, -0.182204 }, { { -0.999999, 0.000000, -0.000000 }, 0.001194 } };
bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, -0.007736 }, { { -1.000000, 0.000000, -0.000000 }, 0.000000 } };
bone->swingLimit = 25.0f * B3_DEG_TO_RAD;
bone->twistLimit = ( b3Vec2 ){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD };
bone->twistLimit = (b3Vec2){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD };
}
{
@ -93,9 +91,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bone->parentIndex = bone_spine_01;
// bodyDef.name = "spine_02";
bone->referenceFrame = ( b3Transform ){ { 0.0f, 1.194336f, -0.027087f }, { { 0.703611f, 0.0f, 0.0f }, 0.710586f } };
bone->referenceFrame = (b3Transform){ { 0.0f, 1.194336f, -0.027087f }, { { 0.703611f, 0.0f, 0.0f }, 0.710586f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { 0.08f, -0.015133f, -0.091801f }, { -0.08f, -0.015133f, -0.091801f }, 0.10f };
@ -105,11 +103,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bone->jointType = b3_sphericalJoint;
bone->localFrameA =
( b3Transform ){ { 0.000000, -0.000000, -0.088935 }, { { -0.998619, -0.000000, 0.000000 }, -0.052540 } };
bone->localFrameB =
( b3Transform ){ { -0.000000, 0.000000, -0.008199 }, { { -1.000000, 0.000000, -0.000000 }, 0.000000 } };
(b3Transform){ { 0.000000, -0.000000, -0.088935 }, { { -0.998619, -0.000000, 0.000000 }, -0.052540 } };
bone->localFrameB = (b3Transform){ { -0.000000, 0.000000, -0.008199 }, { { -1.000000, 0.000000, -0.000000 }, 0.000000 } };
bone->swingLimit = 25.0f * B3_DEG_TO_RAD;
bone->twistLimit = ( b3Vec2 ){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD };
bone->twistLimit = (b3Vec2){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD };
}
{
@ -118,9 +115,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bodyDef.name = "spine_03";
bone->referenceFrame =
( b3Transform ){ { -0.0f, 1.31043f, -0.028232f }, { { 0.669856f, 0.000001f, -0.000001f }, 0.742491f } };
(b3Transform){ { -0.0f, 1.31043f, -0.028232f }, { { 0.669856f, 0.000001f, -0.000001f }, 0.742491f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { 0.11f, -0.039753f, -0.13f }, { -0.11f, -0.039753f, -0.13f }, 0.145f };
@ -130,10 +127,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bone->jointType = b3_sphericalJoint;
bone->localFrameA =
( b3Transform ){ { -0.000000, 0.000000, -0.124298 }, { { -0.998921, 0.000001, -0.000001 }, -0.046434 } };
bone->localFrameB = ( b3Transform ){ { 0.000000, 0.000000, 0.000000 }, { { -1.000000, 0.000000, -0.000001 }, 0.000000 } };
(b3Transform){ { -0.000000, 0.000000, -0.124298 }, { { -0.998921, 0.000001, -0.000001 }, -0.046434 } };
bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, 0.000000 }, { { -1.000000, 0.000000, -0.000001 }, 0.000000 } };
bone->swingLimit = 15.0f * B3_DEG_TO_RAD;
bone->twistLimit = ( b3Vec2 ){ -10.0f * B3_DEG_TO_RAD, 10.0f * B3_DEG_TO_RAD };
bone->twistLimit = (b3Vec2){ -10.0f * B3_DEG_TO_RAD, 10.0f * B3_DEG_TO_RAD };
}
{
@ -141,9 +138,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bone->parentIndex = bone_spine_03;
bodyDef.name = "neck";
bone->referenceFrame = ( b3Transform ){ { 0.0f, 1.575582f, -0.055837f }, { { 0.879922f, 0.0f, 0.0f }, 0.475118f } };
bone->referenceFrame = (b3Transform){ { 0.0f, 1.575582f, -0.055837f }, { { 0.879922f, 0.0f, 0.0f }, 0.475118f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { -0.000001f, -0.0f, -0.02f }, { 0.0f, -0.005f, -0.08f }, 0.07f };
@ -152,11 +149,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule );
bone->jointType = b3_sphericalJoint;
bone->localFrameA =
( b3Transform ){ { 0.000001, -0.000259, -0.266585 }, { { -0.942192, -0.000001, 0.000000 }, 0.335074 } };
bone->localFrameB = ( b3Transform ){ { 0.000000, 0.000000, 0.000000 }, { { -1.000000, 0.000000, -0.000001 }, 0.000000 } };
bone->localFrameA = (b3Transform){ { 0.000001, -0.000259, -0.266585 }, { { -0.942192, -0.000001, 0.000000 }, 0.335074 } };
bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, 0.000000 }, { { -1.000000, 0.000000, -0.000001 }, 0.000000 } };
bone->swingLimit = 45.0f * B3_DEG_TO_RAD;
bone->twistLimit = ( b3Vec2 ){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD };
bone->twistLimit = (b3Vec2){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD };
bone->jointFriction = 0.8f;
}
@ -165,9 +161,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bone->parentIndex = bone_neck;
bodyDef.name = "head";
bone->referenceFrame = ( b3Transform ){ { 0.0f, 1.653348f, -0.003241f }, { { 0.750288f, 0.0f, 0.0f }, 0.661111f } };
bone->referenceFrame = (b3Transform){ { 0.0f, 1.653348f, -0.003241f }, { { 0.750288f, 0.0f, 0.0f }, 0.661111f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { -0.000001f, 0.016892f, -0.05869f }, { 0.0f, -0.003629f, -0.115072f }, 0.0975f };
@ -177,10 +173,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bone->jointType = b3_sphericalJoint;
bone->localFrameA =
( b3Transform ){ { 0.000000, 0.001321, -0.093873 }, { { -0.974301, -0.000000, -0.000000 }, -0.225251 } };
bone->localFrameB = ( b3Transform ){ { 0.000000, 0.001268, -0.005104 }, { { -1.000000, 0.000000, -0.00000 }, 0.000000 } };
(b3Transform){ { 0.000000, 0.001321, -0.093873 }, { { -0.974301, -0.000000, -0.000000 }, -0.225251 } };
bone->localFrameB = (b3Transform){ { 0.000000, 0.001268, -0.005104 }, { { -1.000000, 0.000000, -0.00000 }, 0.000000 } };
bone->swingLimit = 15.0f * B3_DEG_TO_RAD;
bone->twistLimit = ( b3Vec2 ){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD };
bone->twistLimit = (b3Vec2){ -15.0f * B3_DEG_TO_RAD, 15.0f * B3_DEG_TO_RAD };
bone->jointFriction = 0.4f;
}
@ -190,9 +186,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bodyDef.name = "thigh_l";
bone->referenceFrame =
( b3Transform ){ { 0.090416f, 0.986104f, -0.035090f }, { { -0.703287f, -0.070715f, 0.053866f }, 0.705327f } };
(b3Transform){ { 0.090416f, 0.986104f, -0.035090f }, { { -0.703287f, -0.070715f, 0.053866f }, 0.705327f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { 0.023719f, 0.006008f, -0.039068f }, { -0.064492f, -0.004664f, -0.424718f }, 0.09f };
@ -201,11 +197,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule );
bone->jointType = b3_sphericalJoint;
bone->localFrameA =
( b3Transform ){ { 0.05f, 0.011537f, -0.055325f }, { { -0.714896, -0.022305, -0.698361 }, -0.026790 } };
bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { -0.002064, 0.758987, 0.017046 }, 0.650880 } };
bone->localFrameA = (b3Transform){ { 0.05f, 0.011537f, -0.055325f }, { { -0.714896, -0.022305, -0.698361 }, -0.026790 } };
bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.002064, 0.758987, 0.017046 }, 0.650880 } };
bone->swingLimit = 10.0f * B3_DEG_TO_RAD;
bone->twistLimit = ( b3Vec2 ){ -60.0f * B3_DEG_TO_RAD, 40.0f * B3_DEG_TO_RAD };
bone->twistLimit = (b3Vec2){ -60.0f * B3_DEG_TO_RAD, 40.0f * B3_DEG_TO_RAD };
}
{
@ -214,9 +209,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bodyDef.name = "calf_l";
bone->referenceFrame =
( b3Transform ){ { 0.101198f, 0.527027f, -0.037374f }, { { -0.653328f, -0.066860f, 0.058582f }, 0.751838f } };
(b3Transform){ { 0.101198f, 0.527027f, -0.037374f }, { { -0.653328f, -0.066860f, 0.058582f }, 0.751838f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { 0.001778f, 0.0f, 0.009841f }, { -0.078577f, 0.014707f, -0.41816f }, 0.075f };
@ -225,10 +220,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule );
bone->jointType = b3_revoluteJoint;
bone->localFrameA =
( b3Transform ){ { -0.069989, 0.000253, -0.453844 }, { { -0.000677, 0.760087, 0.105674 }, 0.641171 } };
bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { -0.044589, 0.765540, 0.053368 }, 0.639619 } };
bone->twistLimit = ( b3Vec2 ){ -5.0f * B3_DEG_TO_RAD, 45.0f * B3_DEG_TO_RAD };
bone->localFrameA = (b3Transform){ { -0.069989, 0.000253, -0.453844 }, { { -0.000677, 0.760087, 0.105674 }, 0.641171 } };
bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.044589, 0.765540, 0.053368 }, 0.639619 } };
bone->twistLimit = (b3Vec2){ -5.0f * B3_DEG_TO_RAD, 45.0f * B3_DEG_TO_RAD };
}
{
@ -237,9 +231,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bodyDef.name = "thigh_r";
bone->referenceFrame =
( b3Transform ){ { -0.090416f, 0.986104f, -0.03509f }, { { -0.703287f, 0.070715f, -0.053865f }, 0.705326f } };
(b3Transform){ { -0.090416f, 0.986104f, -0.03509f }, { { -0.703287f, 0.070715f, -0.053865f }, 0.705326f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { -0.023719f, 0.006008f, -0.039068f }, { 0.064492f, -0.004664f, -0.424718f }, 0.09f };
@ -248,10 +242,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule );
bone->jointType = b3_sphericalJoint;
bone->localFrameA = ( b3Transform ){ { -0.05, 0.011537, -0.055326 }, { { -0.039089, -0.714094, 0.043177 }, 0.697623 } };
bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { 0.758805, -0.019886, -0.651012 }, -0.001759 } };
bone->localFrameA = (b3Transform){ { -0.05, 0.011537, -0.055326 }, { { -0.039089, -0.714094, 0.043177 }, 0.697623 } };
bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { 0.758805, -0.019886, -0.651012 }, -0.001759 } };
bone->swingLimit = 10.0f * B3_DEG_TO_RAD;
bone->twistLimit = ( b3Vec2 ){ -30.0f * B3_DEG_TO_RAD, 60.0f * B3_DEG_TO_RAD };
bone->twistLimit = (b3Vec2){ -30.0f * B3_DEG_TO_RAD, 60.0f * B3_DEG_TO_RAD };
}
{
@ -260,9 +254,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bodyDef.name = "calf_r";
bone->referenceFrame =
( b3Transform ){ { -0.101198f, 0.527027f, -0.037373f }, { { -0.653327f, 0.06686f, -0.058582f }, 0.751839f } };
(b3Transform){ { -0.101198f, 0.527027f, -0.037373f }, { { -0.653327f, 0.06686f, -0.058582f }, 0.751839f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { -0.001820f, 0.0f, 0.010071f }, { 0.077883f, 0.014825f, -0.418047f }, 0.075f };
@ -271,10 +265,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule );
bone->jointType = b3_revoluteJoint;
bone->localFrameA =
( b3Transform ){ { 0.069988, 0.000253, -0.453844 }, { { 0.760086, -0.000675, -0.641171 }, -0.105676 } };
bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { 0.765540, -0.044589, -0.639619 }, -0.053368 } };
bone->twistLimit = ( b3Vec2 ){ -45.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD };
bone->localFrameA = (b3Transform){ { 0.069988, 0.000253, -0.453844 }, { { 0.760086, -0.000675, -0.641171 }, -0.105676 } };
bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { 0.765540, -0.044589, -0.639619 }, -0.053368 } };
bone->twistLimit = (b3Vec2){ -45.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD };
}
{
@ -283,9 +276,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bodyDef.name = "upper_arm_l";
bone->referenceFrame =
( b3Transform ){ { 0.20378f, 1.484275f, -0.115897f }, { { 0.143082f, 0.695980f, -0.690130f }, 0.13733f } };
(b3Transform){ { 0.20378f, 1.484275f, -0.115897f }, { { 0.143082f, 0.695980f, -0.690130f }, 0.13733f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { -0.091118f, 0.037775f, 0.229719f }, 0.075f };
@ -294,11 +287,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule );
bone->jointType = b3_sphericalJoint;
bone->localFrameA =
( b3Transform ){ { 0.203780, -0.069369, -0.181921 }, { { -0.278486, 0.445600, -0.097014 }, 0.845266 } };
bone->localFrameB = ( b3Transform ){ { 0.000000, 0.000000, 0.000000 }, { { -0.201396, -0.001586, 0.901850 }, 0.382234 } };
bone->localFrameA = (b3Transform){ { 0.203780, -0.069369, -0.181921 }, { { -0.278486, 0.445600, -0.097014 }, 0.845266 } };
bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, 0.000000 }, { { -0.201396, -0.001586, 0.901850 }, 0.382234 } };
bone->swingLimit = 60.0f * B3_DEG_TO_RAD;
bone->twistLimit = ( b3Vec2 ){ -5.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD };
bone->twistLimit = (b3Vec2){ -5.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD };
}
{
@ -307,9 +299,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bodyDef.name = "lower_arm_l";
bone->referenceFrame =
( b3Transform ){ { 0.305614f, 1.242908f, -0.117599f }, { { 0.165048f, 0.563437f, -0.802002f }, 0.109959f } };
(b3Transform){ { 0.305614f, 1.242908f, -0.117599f }, { { 0.165048f, 0.563437f, -0.802002f }, 0.109959f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { -0.142406f, 0.039392f, 0.261092f }, 0.05f };
@ -318,9 +310,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule );
bone->jointType = b3_revoluteJoint;
bone->localFrameA = ( b3Transform ){ { -0.095482, 0.039584, 0.240723 }, { { 0.512487, -0.180629, 0.839474 }, 0.003742 } };
bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { 0.503803, -0.029831, 0.858168 }, 0.094017 } };
bone->twistLimit = ( b3Vec2 ){ -5.0f * B3_DEG_TO_RAD, 60.0f * B3_DEG_TO_RAD };
bone->localFrameA = (b3Transform){ { -0.095482, 0.039584, 0.240723 }, { { 0.512487, -0.180629, 0.839474 }, 0.003742 } };
bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { 0.503803, -0.029831, 0.858168 }, 0.094017 } };
bone->twistLimit = (b3Vec2){ -5.0f * B3_DEG_TO_RAD, 60.0f * B3_DEG_TO_RAD };
}
{
@ -329,9 +321,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bodyDef.name = "upper_arm_r";
bone->referenceFrame =
( b3Transform ){ { -0.20378f, 1.484276f, -0.115899f }, { { 0.143083f, -0.695978f, 0.690132f }, 0.137329f } };
(b3Transform){ { -0.20378f, 1.484276f, -0.115899f }, { { 0.143083f, -0.695978f, 0.690132f }, 0.137329f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { 0.091118f, 0.037775f, 0.229718f }, 0.075f };
@ -341,10 +333,10 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bone->jointType = b3_sphericalJoint;
bone->localFrameA =
( b3Transform ){ { -0.203779, -0.069371, -0.181922 }, { { -0.253621, -0.414842, 0.106962 }, 0.867261 } };
bone->localFrameB = ( b3Transform ){ { 0.000000, 0.000000, 0.000000 }, { { -0.201397, 0.001587, -0.901850 }, 0.382233 } };
(b3Transform){ { -0.203779, -0.069371, -0.181922 }, { { -0.253621, -0.414842, 0.106962 }, 0.867261 } };
bone->localFrameB = (b3Transform){ { 0.000000, 0.000000, 0.000000 }, { { -0.201397, 0.001587, -0.901850 }, 0.382233 } };
bone->swingLimit = 60.0f * B3_DEG_TO_RAD;
bone->twistLimit = ( b3Vec2 ){ -5.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD };
bone->twistLimit = (b3Vec2){ -5.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD };
}
{
@ -353,9 +345,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
bodyDef.name = "lower_arm_r";
bone->referenceFrame =
( b3Transform ){ { -0.305614f, 1.242907f, -0.117599f }, { { 0.165048f, -0.563437f, 0.802002f }, 0.109959f } };
(b3Transform){ { -0.305614f, 1.242907f, -0.117599f }, { { 0.165048f, -0.563437f, 0.802002f }, 0.109959f } };
bodyDef.rotation = bone->referenceFrame.q;
bodyDef.position = b3Add( bone->referenceFrame.p, position );
bodyDef.position = b3OffsetPos( position, bone->referenceFrame.p );
bone->bodyId = b3CreateBody( worldId, &bodyDef );
b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { 0.142406f, 0.039392f, 0.261092f }, 0.05f };
@ -364,10 +356,9 @@ void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float fricti
b3CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule );
bone->jointType = b3_revoluteJoint;
bone->localFrameA =
( b3Transform ){ { 0.095484, 0.039585, 0.240723 }, { { -0.180627, 0.512487, -0.003744 }, -0.839474 } };
bone->localFrameB = ( b3Transform ){ { 0.0f, 0.0f, 0.0f }, { { -0.029831, 0.503803, -0.094017 }, -0.858169 } };
bone->twistLimit = ( b3Vec2 ){ -60.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD };
bone->localFrameA = (b3Transform){ { 0.095484, 0.039585, 0.240723 }, { { -0.180627, 0.512487, -0.003744 }, -0.839474 } };
bone->localFrameB = (b3Transform){ { 0.0f, 0.0f, 0.0f }, { { -0.029831, 0.503803, -0.094017 }, -0.858169 } };
bone->twistLimit = (b3Vec2){ -60.0f * B3_DEG_TO_RAD, 5.0f * B3_DEG_TO_RAD };
}
// float dampingRatio = 0.9f;
@ -541,10 +532,10 @@ void Human_SetJointDampingRatio( Human* human, float dampingRatio )
}
}
void Human_AlignSpring(Human* human, b3WorldId worldId, b3BodyId groundId, float hertz, float dampingRatio)
void Human_AlignSpring( Human* human, b3WorldId worldId, b3BodyId groundId, float hertz, float dampingRatio )
{
assert( human->isSpawned == true );
Bone* bone = human->bones + bone_pelvis;
assert( B3_IS_NULL( bone->jointId ) == true );
b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY );
@ -580,10 +571,10 @@ void Human_CreateMotorAnchors( Human* human, b3WorldId worldId )
{
Bone* bone = human->bones + i;
b3Transform bodyTransform = b3Body_GetTransform( bone->bodyId );
b3WorldTransform bodyTransform = b3Body_GetTransform( bone->bodyId );
anchorDef.position = bodyTransform.p;
anchorDef.rotation = bodyTransform.q;
bone->anchorId = b3CreateBody(worldId, &anchorDef );
bone->anchorId = b3CreateBody( worldId, &anchorDef );
motorDef.base.bodyIdA = bone->anchorId;
motorDef.base.bodyIdB = bone->bodyId;
@ -607,10 +598,10 @@ void Human_CreateParallelAnchors( Human* human, b3WorldId worldId )
{
Bone* bone = human->bones + i;
b3Transform bodyTransform = b3Body_GetTransform( bone->bodyId );
b3WorldTransform bodyTransform = b3Body_GetTransform( bone->bodyId );
anchorDef.position = bodyTransform.p;
anchorDef.rotation = bodyTransform.q;
bone->anchorId = b3CreateBody(worldId, &anchorDef );
bone->anchorId = b3CreateBody( worldId, &anchorDef );
jointDef.base.bodyIdA = bone->anchorId;
jointDef.base.bodyIdB = bone->bodyId;
@ -623,7 +614,7 @@ void Human_CreateParallelAnchors( Human* human, b3WorldId worldId )
}
}
void Human_SetBullet(Human* human, bool flag)
void Human_SetBullet( Human* human, bool flag )
{
for ( int i = 0; i < bone_count; ++i )
{

View file

@ -57,7 +57,7 @@ extern "C"
{
#endif
void CreateHuman( Human* human, b3WorldId worldId, b3Vec3 position, float frictionTorque, float hertz, float dampingRatio,
void CreateHuman( Human* human, b3WorldId worldId, b3Pos position, float frictionTorque, float hertz, float dampingRatio,
int groupIndex, void* userData, bool colorize );
// There are no allocations so this does not need to be called when the world is destroyed.

View file

@ -20,7 +20,7 @@ OverflowColorPileData CreateOverflowColorPile( b3WorldId worldId )
// Static ground (top surface at y = 0)
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = (b3Vec3){ 0.0f, -1.0f, 0.0f };
bodyDef.position = (b3Pos){ 0.0f, -1.0f, 0.0f };
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
b3BoxHull box = b3MakeBoxHull( 20.0f, 1.0f, 20.0f );
@ -37,7 +37,7 @@ OverflowColorPileData CreateOverflowColorPile( b3WorldId worldId )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = (b3Vec3){ 0.0f, hubHalfY, 0.0f };
bodyDef.position = (b3Pos){ 0.0f, hubHalfY, 0.0f };
data.hubId = b3CreateBody( worldId, &bodyDef );
b3BoxHull box = b3MakeBoxHull( hubHalfX, hubHalfY, hubHalfZ );
@ -71,7 +71,7 @@ OverflowColorPileData CreateOverflowColorPile( b3WorldId worldId )
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = (b3Vec3){ ringRadius * cosf( theta ), y, ringRadius * sinf( theta ) };
bodyDef.position = (b3Pos){ ringRadius * cosf( theta ), y, ringRadius * sinf( theta ) };
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3CreateHullShape( bodyId, &neighborShape, &neighborBox.base );

View file

@ -12,11 +12,12 @@
#define MESH_DROP_GRID_COUNT 32
MeshDropData CreateMeshDrop( b3WorldId worldId )
MeshDropData CreateMeshDrop( b3WorldId worldId, b3Pos origin )
{
MeshDropData data = { 0 };
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = origin;
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
int gridCount = 40;
@ -55,7 +56,8 @@ MeshDropData CreateMeshDrop( b3WorldId worldId )
b3Vec3 linearVelocity = RandomVec3Uniform( -1.0f, 1.0f );
b3Vec3 angularVelocity = RandomVec3Uniform( -5.0f, 5.0f );
bodyDef.position = (b3Vec3){ 0.5f * ( i - 0.5f * gridCount ), 5.0f, 0.5f * ( j - 0.5f * gridCount ) };
bodyDef.position =
b3OffsetPos( origin, (b3Vec3){ 0.5f * ( i - 0.5f * gridCount ), 5.0f, 0.5f * ( j - 0.5f * gridCount ) } );
bodyDef.linearVelocity = linearVelocity;
bodyDef.angularVelocity = angularVelocity;
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );

View file

@ -3,6 +3,7 @@
#pragma once
#include "box3d/id.h"
#include "box3d/math_functions.h"
#include <stdbool.h>
@ -16,7 +17,7 @@ typedef struct MeshDropData
struct b3MeshData* mesh;
} MeshDropData;
MeshDropData CreateMeshDrop( b3WorldId worldId );
MeshDropData CreateMeshDrop( b3WorldId worldId, b3Pos origin );
void DestroyMeshDrop( MeshDropData* data );
#ifdef __cplusplus

View file

@ -71,6 +71,16 @@ B3_INLINE b3Vec3 RandomVec3( b3Vec3 lo, b3Vec3 hi )
return v;
}
// Random world position with coordinates in range [lo, hi]
B3_INLINE b3Pos RandomPos( b3Vec3 lo, b3Vec3 hi )
{
b3Pos v;
v.x = RandomFloatRange( lo.x, hi.x );
v.y = RandomFloatRange( lo.y, hi.y );
v.z = RandomFloatRange( lo.z, hi.z );
return v;
}
B3_INLINE b3Vec3 RandomVec3Uniform( float lo, float hi )
{
b3Vec3 v;

View file

@ -149,10 +149,16 @@ if(BOX3D_VALIDATE)
endif()
if (BOX3D_DISABLE_SIMD)
message(STATUS "Box3D SIMD disabled")
message(STATUS "Box3D SIMD disabled")
target_compile_definitions(box3d PRIVATE BOX3D_DISABLE_SIMD)
endif()
if (BOX3D_DOUBLE_PRECISION)
message(STATUS "Box3D double precision ON")
# PUBLIC: consumers must see the same precision mode in the headers or they cannot match the ABI
target_compile_definitions(box3d PUBLIC BOX3D_DOUBLE_PRECISION)
endif()
if (MSVC)
message(STATUS "Box3D on MSVC")
if (BUILD_SHARED_LIBS)

View file

@ -33,14 +33,14 @@ b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId )
return b3Array_Get( world->bodies, bodyId.index1 - 1 );
}
b3Transform b3GetBodyTransformQuick( b3World* world, b3Body* body )
b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body )
{
b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex );
b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex );
return bodySim->transform;
}
b3Transform b3GetBodyTransform( b3World* world, int bodyId )
b3WorldTransform b3GetBodyTransform( b3World* world, int bodyId )
{
b3Body* body = b3Array_Get( world->bodies, bodyId );
return b3GetBodyTransformQuick( world, body );
@ -71,7 +71,7 @@ b3BodyState* b3GetBodyState( b3World* world, b3Body* body )
return NULL;
}
static void b3SyncBodyFlags(b3World* world, b3Body* body)
static void b3SyncBodyFlags( b3World* world, b3Body* body )
{
// Never sync transient flags
uint32_t flags = body->flags & ~b3_bodyTransientFlags;
@ -80,7 +80,7 @@ static void b3SyncBodyFlags(b3World* world, b3Body* body)
bodySim->flags = flags;
b3BodyState* bodyState = b3GetBodyState( world, body );
if (bodyState != NULL)
if ( bodyState != NULL )
{
bodyState->flags = flags;
}
@ -156,7 +156,7 @@ static void b3DestroyBodyContacts( b3World* world, b3Body* body, bool wakeBodies
b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def )
{
B3_CHECK_DEF( def );
B3_ASSERT( b3IsValidVec3( def->position ) );
B3_ASSERT( b3IsValidPosition( def->position ) );
B3_ASSERT( b3IsValidQuat( def->rotation ) );
B3_ASSERT( b3IsValidVec3( def->linearVelocity ) );
B3_ASSERT( b3IsValidVec3( def->angularVelocity ) );
@ -508,8 +508,9 @@ b3AABB b3Body_ComputeAABB( b3BodyId bodyId )
b3Body* body = b3GetBodyFullId( world, bodyId );
if ( body->headShapeId == B3_NULL_INDEX )
{
b3Transform transform = b3GetBodyTransform( world, body->id );
return (b3AABB){ transform.p, transform.p };
b3WorldTransform transform = b3GetBodyTransform( world, body->id );
b3Vec3 p = b3ToVec3( transform.p );
return (b3AABB){ p, p };
}
b3Shape* shape = b3Array_Get( world->shapes, body->headShapeId );
@ -533,15 +534,17 @@ float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target )
}
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3GetBodyTransform( world, body->id );
b3WorldTransform worldTransform = b3GetBodyTransform( world, body->id );
b3Transform transform = b3ToRelativeTransform( worldTransform, b3Pos_zero );
float closestDistance = FLT_MAX;
b3Vec3 closestPoint = transform.p;
b3DistanceInput input = { 0 };
input.proxyA = (b3ShapeProxy){ &target, 1, 0.0f };
input.transformA.q = b3Quat_identity;
input.transformB = transform;
// Target rides in frame A at the origin, so the relative pose of the shape in A is the body transform
input.transform = transform;
input.useRadii = false;
int shapeId = body->headShapeId;
@ -571,7 +574,8 @@ float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target )
return closestDistance;
}
b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, const b3BodyRayCastInput* input, b3Transform bodyTransform )
b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, float maxFraction,
b3WorldTransform bodyTransform )
{
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
if ( world == NULL )
@ -582,128 +586,13 @@ b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, const b3BodyRayCastInput* inpu
b3BodyCastResult result = { 0 };
b3Body* body = b3GetBodyFullId( world, bodyId );
// The consistent framing is to center on the ray origin.
b3RayCastInput shapeInput = { 0 };
shapeInput.origin = input->origin;
shapeInput.translation = input->translation;
shapeInput.maxFraction = input->maxFraction;
shapeInput.origin = b3Vec3_zero;
shapeInput.translation = translation;
shapeInput.maxFraction = maxFraction;
int shapeId = body->headShapeId;
while ( shapeId != B3_NULL_INDEX )
{
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
shapeId = shape->nextShapeId;
if ( b3ShouldQueryCollide( &shape->filter, &input->filter ) == false )
{
continue;
}
b3CastOutput shapeOutput = b3RayCastShape( shape, bodyTransform, &shapeInput );
if ( shapeOutput.hit == false )
{
continue;
}
if ( shapeOutput.fraction > shapeInput.maxFraction )
{
continue;
}
// Careful with id, shapeId is the next shape.
b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation };
int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 );
uint64_t userMaterialId = shape->materials[materialIndex].userMaterialId;
result = (b3BodyCastResult){
.shapeId = id,
.point = shapeOutput.point,
.normal = shapeOutput.normal,
.fraction = shapeOutput.fraction,
.triangleIndex = shapeOutput.triangleIndex,
.userMaterialId = userMaterialId,
.iterations = shapeOutput.iterations,
.hit = true,
};
shapeInput.maxFraction = shapeOutput.fraction;
}
return result;
}
b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, const b3BodyShapeCastInput* input, b3Transform bodyTransform )
{
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
if ( world == NULL )
{
return (b3BodyCastResult){ 0 };
}
b3BodyCastResult result = { 0 };
b3Body* body = b3GetBodyFullId( world, bodyId );
b3ShapeCastInput shapeInput = { 0 };
shapeInput.proxy = input->proxy;
shapeInput.translation = input->translation;
shapeInput.maxFraction = input->maxFraction;
shapeInput.canEncroach = input->canEncroach;
int shapeId = body->headShapeId;
while ( shapeId != B3_NULL_INDEX )
{
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
shapeId = shape->nextShapeId;
if ( b3ShouldQueryCollide( &shape->filter, &input->filter ) == false )
{
continue;
}
b3CastOutput shapeOutput = b3ShapeCastShape( shape, bodyTransform, &shapeInput );
if ( shapeOutput.hit == false )
{
continue;
}
if ( shapeOutput.fraction > shapeInput.maxFraction )
{
continue;
}
// Careful with id, shapeId is the next shape.
b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation };
int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 );
uint64_t userMaterialId = shape->materials[materialIndex].userMaterialId;
result = (b3BodyCastResult){
.shapeId = id,
.point = shapeOutput.point,
.normal = shapeOutput.normal,
.fraction = shapeOutput.fraction,
.triangleIndex = shapeOutput.triangleIndex,
.userMaterialId = userMaterialId,
.iterations = shapeOutput.iterations,
.hit = true,
};
shapeInput.maxFraction = shapeOutput.fraction;
}
return result;
}
bool b3Body_OverlapShape( b3BodyId bodyId, const b3ShapeProxy* proxy, b3QueryFilter filter, b3Transform bodyTransform )
{
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
if ( world == NULL )
{
return false;
}
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3ToRelativeTransform( bodyTransform, origin );
int shapeId = body->headShapeId;
while ( shapeId != B3_NULL_INDEX )
@ -716,7 +605,130 @@ bool b3Body_OverlapShape( b3BodyId bodyId, const b3ShapeProxy* proxy, b3QueryFil
continue;
}
bool overlaps = b3OverlapShape( shape, bodyTransform, proxy );
b3CastOutput shapeOutput = b3RayCastShape( shape, transform, &shapeInput );
if ( shapeOutput.hit == false )
{
continue;
}
if ( shapeOutput.fraction > shapeInput.maxFraction )
{
continue;
}
// Careful with id, shapeId is the next shape.
b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation };
int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 );
uint64_t userMaterialId = shape->materials[materialIndex].userMaterialId;
result = (b3BodyCastResult){
.shapeId = id,
.point = b3OffsetPos( origin, shapeOutput.point ),
.normal = shapeOutput.normal,
.fraction = shapeOutput.fraction,
.triangleIndex = shapeOutput.triangleIndex,
.userMaterialId = userMaterialId,
.iterations = shapeOutput.iterations,
.hit = true,
};
shapeInput.maxFraction = shapeOutput.fraction;
}
return result;
}
b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,
b3QueryFilter filter, float maxFraction, bool canEncroach, b3WorldTransform bodyTransform )
{
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
if ( world == NULL )
{
return (b3BodyCastResult){ 0 };
}
b3BodyCastResult result = { 0 };
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3ToRelativeTransform( bodyTransform, origin );
b3ShapeCastInput shapeInput = { 0 };
shapeInput.proxy = *proxy;
shapeInput.translation = translation;
shapeInput.maxFraction = maxFraction;
shapeInput.canEncroach = canEncroach;
int shapeId = body->headShapeId;
while ( shapeId != B3_NULL_INDEX )
{
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
shapeId = shape->nextShapeId;
if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false )
{
continue;
}
b3CastOutput shapeOutput = b3ShapeCastShape( shape, transform, &shapeInput );
if ( shapeOutput.hit == false )
{
continue;
}
if ( shapeOutput.fraction > shapeInput.maxFraction )
{
continue;
}
// Careful with id, shapeId is the next shape.
b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation };
int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 );
uint64_t userMaterialId = shape->materials[materialIndex].userMaterialId;
result = (b3BodyCastResult){
.shapeId = id,
.point = b3OffsetPos( origin, shapeOutput.point ),
.normal = shapeOutput.normal,
.fraction = shapeOutput.fraction,
.triangleIndex = shapeOutput.triangleIndex,
.userMaterialId = userMaterialId,
.iterations = shapeOutput.iterations,
.hit = true,
};
shapeInput.maxFraction = shapeOutput.fraction;
}
return result;
}
bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter,
b3WorldTransform bodyTransform )
{
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
if ( world == NULL )
{
return false;
}
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3ToRelativeTransform( bodyTransform, origin );
int shapeId = body->headShapeId;
while ( shapeId != B3_NULL_INDEX )
{
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
shapeId = shape->nextShapeId;
if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false )
{
continue;
}
bool overlaps = b3OverlapShape( shape, transform, proxy );
if ( overlaps )
{
return true;
@ -726,8 +738,8 @@ bool b3Body_OverlapShape( b3BodyId bodyId, const b3ShapeProxy* proxy, b3QueryFil
return false;
}
int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, const b3Capsule* mover,
b3QueryFilter filter, b3Transform bodyTransform )
int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, const b3Capsule* mover,
b3QueryFilter filter, b3WorldTransform bodyTransform )
{
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
if ( world == NULL )
@ -743,6 +755,8 @@ int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int pla
int resultCount = 0;
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3ToRelativeTransform( bodyTransform, origin );
int shapeId = body->headShapeId;
while ( shapeId != B3_NULL_INDEX )
{
@ -761,11 +775,11 @@ int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int pla
}
b3PlaneResult plane;
int count = b3CollideMover( &plane, 1, shape, bodyTransform, mover );
int count = b3CollideMover( &plane, 1, shape, transform, mover );
if ( count > 0 )
{
b3ShapeId id = { shape->id + 1, world->worldId, shape->generation };
b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation };
bodyPlanes[resultCount] = (b3BodyPlaneResult){ .shapeId = id, .result = plane };
resultCount += 1;
if ( resultCount == planeCapacity )
@ -823,9 +837,13 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
return;
}
int shapeCount = body->shapeCount;
b3MassData* masses = b3StackAlloc( &world->stack, shapeCount * sizeof( b3MassData ), "mass data" );
// Accumulate mass over all shapes.
b3Vec3 localCenter = b3Vec3_zero;
int shapeId = body->headShapeId;
int shapeIndex = 0;
while ( shapeId != B3_NULL_INDEX )
{
const b3Shape* s = b3Array_Get( world->shapes, shapeId );
@ -833,13 +851,17 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
if ( s->density == 0.0f )
{
masses[shapeIndex] = (b3MassData){ 0 };
shapeIndex += 1;
continue;
}
b3MassData massData = b3ComputeShapeMass( s );
body->mass += massData.mass;
localCenter = b3MulAdd( localCenter, massData.mass, massData.center );
body->inertia = b3AddMM( body->inertia, massData.inertia );
masses[shapeIndex] = massData;
shapeIndex += 1;
}
// Compute center of mass.
@ -847,10 +869,31 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
{
bodySim->invMass = 1.0f / body->mass;
localCenter = b3MulSV( bodySim->invMass, localCenter );
}
// Shift inertia to mass center
body->inertia = b3SubMM( body->inertia, b3Steiner( body->mass, localCenter ) );
// Second loop to accumulate the rotational inertia about the center of mass
for ( shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex )
{
b3MassData massData = masses[shapeIndex];
if ( massData.mass == 0.0f )
{
continue;
}
// Shift to center of mass. This is safe because it can only increase.
b3Vec3 offset = b3Sub( localCenter, massData.center );
b3Matrix3 inertia = b3AddMM( massData.inertia, b3Steiner( massData.mass, offset ) );
body->inertia = b3AddMM(body->inertia, inertia);
}
b3StackFree( &world->stack, masses );
masses = NULL;
float det = b3Det( body->inertia );
B3_ASSERT( det >= 0.0f );
if ( det > 0.0f )
{
// This call is faster than b3Invert
bodySim->invInertiaLocal = b3InvertT( body->inertia );
@ -859,16 +902,16 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
}
// Move center of mass.
b3Vec3 oldCenter = bodySim->center;
b3Pos oldCenter = bodySim->center;
bodySim->localCenter = localCenter;
bodySim->center = b3TransformPoint( bodySim->transform, bodySim->localCenter );
bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter );
bodySim->center0 = bodySim->center;
// Update center of mass velocity
b3BodyState* state = b3GetBodyState( world, body );
if ( state != NULL )
{
b3Vec3 deltaLinear = b3Cross( state->angularVelocity, b3Sub( bodySim->center, oldCenter ) );
b3Vec3 deltaLinear = b3Cross( state->angularVelocity, b3SubPos( bodySim->center, oldCenter ) );
state->linearVelocity = b3Add( state->linearVelocity, deltaLinear );
}
@ -899,7 +942,7 @@ void b3DumpBody( b3World* world, b3Body* body )
// int32 bodyIndex = m_islandIndex;
b3BodySim* sim = b3GetBodySim( world, body );
b3Vec3 p = sim->transform.p;
b3Vec3 p = b3ToVec3( sim->transform.p );
b3Quat q = sim->transform.q;
b3Vec3 v = b3Vec3_zero;
@ -946,11 +989,11 @@ void b3DumpBody( b3World* world, b3Body* body )
b3Dump( "}\n" );
}
b3Vec3 b3Body_GetPosition( b3BodyId bodyId )
b3Pos b3Body_GetPosition( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3WorldTransform transform = b3GetBodyTransformQuick( world, body );
return transform.p;
}
@ -958,38 +1001,38 @@ b3Quat b3Body_GetRotation( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3WorldTransform transform = b3GetBodyTransformQuick( world, body );
return transform.q;
}
b3Transform b3Body_GetTransform( b3BodyId bodyId )
b3WorldTransform b3Body_GetTransform( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
return b3GetBodyTransformQuick( world, body );
}
b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Vec3 worldPoint )
b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Pos worldPoint )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
return b3InvTransformPoint( transform, worldPoint );
b3WorldTransform transform = b3GetBodyTransformQuick( world, body );
return b3InvTransformWorldPoint( transform, worldPoint );
}
b3Vec3 b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint )
b3Pos b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
return b3TransformPoint( transform, localPoint );
b3WorldTransform transform = b3GetBodyTransformQuick( world, body );
return b3TransformWorldPoint( transform, localPoint );
}
b3Vec3 b3Body_GetLocalVector( b3BodyId bodyId, b3Vec3 worldVector )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3WorldTransform transform = b3GetBodyTransformQuick( world, body );
return b3InvRotateVector( transform.q, worldVector );
}
@ -997,13 +1040,13 @@ b3Vec3 b3Body_GetWorldVector( b3BodyId bodyId, b3Vec3 localVector )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3WorldTransform transform = b3GetBodyTransformQuick( world, body );
return b3RotateVector( transform.q, localVector );
}
void b3Body_SetTransform( b3BodyId bodyId, b3Vec3 position, b3Quat rotation )
void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation )
{
B3_ASSERT( b3IsValidVec3( position ) );
B3_ASSERT( b3IsValidPosition( position ) );
B3_ASSERT( b3IsValidQuat( rotation ) );
B3_ASSERT( b3Body_IsValid( bodyId ) );
b3World* world = b3GetWorld( bodyId.world0 );
@ -1014,7 +1057,7 @@ void b3Body_SetTransform( b3BodyId bodyId, b3Vec3 position, b3Quat rotation )
bodySim->transform.p = position;
bodySim->transform.q = rotation;
bodySim->center = b3TransformPoint( bodySim->transform, bodySim->localCenter );
bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter );
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
@ -1024,20 +1067,14 @@ void b3Body_SetTransform( b3BodyId bodyId, b3Vec3 position, b3Quat rotation )
b3BroadPhase* broadPhase = &world->broadPhase;
b3Transform transform = bodySim->transform;
b3WorldTransform transform = bodySim->transform;
const float speculativeDistance = B3_SPECULATIVE_DISTANCE;
int shapeId = body->headShapeId;
while ( shapeId != B3_NULL_INDEX )
{
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
b3AABB aabb = b3ComputeShapeAABB( shape, transform );
aabb.lowerBound.x -= speculativeDistance;
aabb.lowerBound.y -= speculativeDistance;
aabb.lowerBound.z -= speculativeDistance;
aabb.upperBound.x += speculativeDistance;
aabb.upperBound.y += speculativeDistance;
aabb.upperBound.z += speculativeDistance;
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance );
shape->aabb = aabb;
if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )
@ -1145,9 +1182,9 @@ void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity )
state->angularVelocity = w;
}
void b3Body_SetTargetTransform( b3BodyId bodyId, b3Transform target, float timeStep, bool wake )
void b3Body_SetTargetTransform( b3BodyId bodyId, b3WorldTransform target, float timeStep, bool wake )
{
B3_ASSERT( b3IsValidTransform( target ) );
B3_ASSERT( b3IsValidWorldTransform( target ) );
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
@ -1169,11 +1206,11 @@ void b3Body_SetTargetTransform( b3BodyId bodyId, b3Transform target, float timeS
b3BodySim* sim = b3GetBodySim( world, body );
// Compute linear velocity
b3Vec3 center1 = sim->center;
b3Vec3 center2 = b3TransformPoint( target, sim->localCenter );
// Compute linear velocity. The center difference is taken in world precision then demoted.
b3Pos center1 = sim->center;
b3Pos center2 = b3TransformWorldPoint( target, sim->localCenter );
float invTimeStep = 1.0f / timeStep;
b3Vec3 linearVelocity = b3MulSV( invTimeStep, b3Sub( center2, center1 ) );
b3Vec3 linearVelocity = b3MulSV( invTimeStep, b3SubPos( center2, center1 ) );
// Compute angular velocity:
// q' = 0.5 * w * q
@ -1232,7 +1269,7 @@ b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint )
return v;
}
b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Vec3 worldPoint )
b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Pos worldPoint )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
@ -1245,12 +1282,12 @@ b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Vec3 worldPoint )
b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex );
b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex );
b3Vec3 r = b3Sub( worldPoint, bodySim->center );
b3Vec3 r = b3SubPos( worldPoint, bodySim->center );
b3Vec3 v = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, r ) );
return v;
}
void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Vec3 point, bool wake )
void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Pos point, bool wake )
{
B3_ASSERT( b3IsValidVec3( force ) );
@ -1266,7 +1303,7 @@ void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Vec3 point, bool wake )
{
b3BodySim* bodySim = b3GetBodySim( world, body );
bodySim->force = b3Add( bodySim->force, force );
bodySim->torque = b3Add( bodySim->torque, b3Cross( b3Sub( point, bodySim->center ), force ) );
bodySim->torque = b3Add( bodySim->torque, b3Cross( b3SubPos( point, bodySim->center ), force ) );
}
}
@ -1308,10 +1345,10 @@ void b3Body_ApplyTorque( b3BodyId bodyId, b3Vec3 torque, bool wake )
}
}
void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Vec3 point, bool wake )
void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Pos point, bool wake )
{
B3_ASSERT( b3IsValidVec3( impulse ) );
B3_ASSERT( b3IsValidVec3( point ) );
B3_ASSERT( b3IsValidPosition( point ) );
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
@ -1336,7 +1373,7 @@ void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Vec3 point, b
state->linearVelocity = b3MulSV( maxLinearSpeed, b3Normalize( state->linearVelocity ) );
}
b3Vec3 delta = b3MulMV( bodySim->invInertiaWorld, b3Cross( b3Sub( point, bodySim->center ), impulse ) );
b3Vec3 delta = b3MulMV( bodySim->invInertiaWorld, b3Cross( b3SubPos( point, bodySim->center ), impulse ) );
state->angularVelocity = b3Add( state->angularVelocity, delta );
}
}
@ -1597,7 +1634,7 @@ void b3Body_SetType( b3BodyId bodyId, b3BodyType type )
}
// Recreate shape proxies in broadphase
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3WorldTransform transform = b3GetBodyTransformQuick( world, body );
int shapeId = body->headShapeId;
while ( shapeId != B3_NULL_INDEX )
{
@ -1729,7 +1766,7 @@ b3Vec3 b3Body_GetLocalCenterOfMass( b3BodyId bodyId )
return bodySim->localCenter;
}
b3Vec3 b3Body_GetWorldCenterOfMass( b3BodyId bodyId )
b3Pos b3Body_GetWorldCenterOfMass( b3BodyId bodyId )
{
b3World* world = b3GetWorld( bodyId.world0 );
b3Body* body = b3GetBodyFullId( world, bodyId );
@ -1756,7 +1793,7 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
body->inertia = massData.inertia;
bodySim->localCenter = massData.center;
b3Vec3 center = b3TransformPoint( bodySim->transform, massData.center );
b3Pos center = b3TransformWorldPoint( bodySim->transform, massData.center );
bodySim->center = center;
bodySim->center0 = center;
@ -2048,7 +2085,7 @@ void b3Body_Enable( b3BodyId bodyId )
b3TransferBody( world, targetSet, disabledSet, body );
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3WorldTransform transform = b3GetBodyTransformQuick( world, body );
// Add shapes to broad-phase
b3BodyType proxyType = body->type;

View file

@ -180,15 +180,15 @@ static const b3BodyState b3_identityBodyState = {
// Transform data used for collision and solver preparation.
typedef struct b3BodySim
{
// transform for body origin
b3Transform transform;
// transform for body origin, double translation in large world mode
b3WorldTransform transform;
// center of mass position in world space
b3Vec3 center;
b3Pos center;
// previous rotation and COM for TOI
b3Quat rotation0;
b3Vec3 center0;
b3Pos center0;
// location of center of mass relative to the body origin
b3Vec3 localCenter;
@ -220,8 +220,8 @@ typedef struct b3BodySim
// Get a validated body from a world using an id.
b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId );
b3Transform b3GetBodyTransformQuick( b3World* world, b3Body* body );
b3Transform b3GetBodyTransform( b3World* world, int bodyId );
b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body );
b3WorldTransform b3GetBodyTransform( b3World* world, int bodyId );
// Create a b3BodyId from a raw id.
b3BodyId b3MakeBodyId( b3World* world, int bodyId );
@ -239,11 +239,12 @@ bool b3WakeBodyWithLock( b3World* world, b3Body* body );
void b3UpdateBodyMassData( b3World* world, b3Body* body );
void b3DumpBody( b3World* world, b3Body* body );
static inline b3Sweep b3MakeSweep( const b3BodySim* bodySim )
// Make a sweep relative to a base position to keep TOI in float precision far from the origin.
static inline b3Sweep b3MakeRelativeSweep( const b3BodySim* bodySim, b3Pos base )
{
b3Sweep s;
s.c1 = bodySim->center0;
s.c2 = bodySim->center;
s.c1 = b3SubPos( bodySim->center0, base );
s.c2 = b3SubPos( bodySim->center, base );
s.q1 = bodySim->rotation0;
s.q2 = bodySim->transform.q;
s.localCenter = bodySim->localCenter;

View file

@ -14,6 +14,15 @@
<DisplayString>[ {v}, {s} ]</DisplayString>
</Type>
<!-- Distinct types only in BOX3D_DOUBLE_PRECISION builds; otherwise they alias b3Vec3 / b3Transform -->
<Type Name="b3Pos">
<DisplayString>[ {x}, {y}, {z} ]</DisplayString>
</Type>
<Type Name="b3WorldTransform">
<DisplayString>[ {p}, {q} ]</DisplayString>
</Type>
<Type Name="b3WorldId">
<DisplayString>index: {index1}</DisplayString>
<Expand>

View file

@ -176,7 +176,8 @@ static bool b3PairQueryCallback( int proxyId, uint64_t userData, void* context )
b3Shape* shape = b3Array_Get( world->shapes, shapeIndex );
if ( shape->type == b3_compoundShape )
{
b3Transform compoundTransform = b3GetBodyTransform( world, shape->bodyId );
// Query bounds are float world space, so the demoted transform is the matching float frame
b3Transform compoundTransform = b3ToRelativeTransform( b3GetBodyTransform( world, shape->bodyId ), b3Pos_zero );
b3AABB localAABB = b3AABB_Transform( b3InvertTransform( compoundTransform ), queryContext->aabb );
// recurse

View file

@ -39,14 +39,15 @@ b3MassData b3ComputeCapsuleMass( const b3Capsule* shape, float density )
rotation = b3MakeMatrixFromQuat( q );
}
// Parallel Axis Theorem capsule
float mass = sphereMass + cylinderMass;
b3Vec3 center = b3MulSV( 0.5f, b3Add( c1, c2 ) );
b3MassData out;
out.mass = mass;
out.center = center;
out.inertia = b3AddMM( b3MulMM( rotation, b3MulMM( inertia, b3Transpose( rotation ) ) ), b3Steiner( mass, center ) );
// Rotate the central inertia into the shape frame
out.inertia = b3MulMM( rotation, b3MulMM( inertia, b3Transpose( rotation ) ) );
return out;
}
@ -106,8 +107,7 @@ bool b3OverlapCapsule( const b3Capsule* shape, b3Transform shapeTransform, const
b3DistanceInput input;
input.proxyA = (b3ShapeProxy){ &shape->center1, 2, shape->radius };
input.proxyB = *proxy;
input.transformA = shapeTransform;
input.transformB = b3Transform_identity;
input.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity );
input.useRadii = true;
b3SimplexCache cache = { 0 };
@ -255,8 +255,7 @@ b3CastOutput b3ShapeCastCapsule( const b3Capsule* capsule, const b3ShapeCastInpu
b3ShapeCastPairInput pairInput;
pairInput.proxyA = (b3ShapeProxy){ &capsule->center1, 2, capsule->radius };
pairInput.proxyB = input->proxy;
pairInput.transformA = b3Transform_identity;
pairInput.transformB = b3Transform_identity;
pairInput.transform = b3Transform_identity;
pairInput.translationB = input->translation;
pairInput.maxFraction = input->maxFraction;
pairInput.canEncroach = input->canEncroach;

View file

@ -759,6 +759,8 @@ struct b3CompoundCastContext
{
const b3Compound* compound;
b3CastOutput* output;
// origin of the shape cast, the box cast callback only carries the advancing fraction
const b3ShapeCastInput* shapeInput;
};
static float b3CompoundRayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context )
@ -833,32 +835,35 @@ b3CastOutput b3RayCastCompound( const b3Compound* shape, const b3RayCastInput* i
return result;
}
static float b3CompoundShapeCastCallback( const b3ShapeCastInput* input, int proxyId, uint64_t userData, void* context )
static float b3CompoundShapeCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context )
{
B3_UNUSED( proxyId );
struct b3CompoundCastContext* castContext = context;
const b3Compound* compound = castContext->compound;
const b3ShapeCastInput* shapeInput = castContext->shapeInput;
int childIndex = (int)userData;
b3ChildShape child = b3GetCompoundChild( compound, childIndex );
b3ShapeCastInput localInput = *input;
// Rebuild from the carried shape cast input, taking only the advancing fraction from the tree
b3ShapeCastInput localInput = *shapeInput;
localInput.maxFraction = input->maxFraction;
b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS];
localInput.proxy.count = b3MinInt( input->proxy.count, B3_MAX_SHAPE_CAST_POINTS );
localInput.proxy.count = b3MinInt( shapeInput->proxy.count, B3_MAX_SHAPE_CAST_POINTS );
b3Transform invTransform = b3InvertTransform( child.transform );
b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q );
for ( int i = 0; i < localInput.proxy.count; ++i )
{
localPoints[i] = b3Add( b3MulMV( R, input->proxy.points[i] ), invTransform.p );
localPoints[i] = b3Add( b3MulMV( R, shapeInput->proxy.points[i] ), invTransform.p );
}
localInput.proxy.points = localPoints;
localInput.translation = b3MulMV( R, input->translation );
localInput.translation = b3MulMV( R, shapeInput->translation );
b3CastOutput output = { 0 };
@ -909,11 +914,21 @@ b3CastOutput b3ShapeCastCompound( const b3Compound* shape, const b3ShapeCastInpu
{
b3CastOutput result = { 0 };
if ( input->proxy.count == 0 )
{
return result;
}
struct b3CompoundCastContext context = {
.compound = shape,
.output = &result,
.shapeInput = input,
};
(void)b3DynamicTree_ShapeCast( &shape->tree, input, ~0ull, false, b3CompoundShapeCastCallback, &context );
// The compound tree is in the compound local frame, so the proxy box needs no origin offset
b3AABB box = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius );
b3BoxCastInput treeInput = { box, input->translation, input->maxFraction };
(void)b3DynamicTree_BoxCast( &shape->tree, &treeInput, ~0ull, false, b3CompoundShapeCastCallback, &context );
return result;
}

View file

@ -471,8 +471,8 @@ void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies )
}
}
static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, b3Transform xfA,
const b3Shape* shapeB, b3Transform xfB, b3Arena arena )
static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA,
b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, b3Arena arena )
{
b3ShapeType typeA = shapeA->type;
b3ShapeType typeB = shapeB->type;
@ -485,7 +485,7 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact*
b3LocalManifold geomManifold = { 0 };
geomManifold.points = pointBuffer;
b3Transform transformBtoA = b3InvMulTransforms( xfA, xfB );
b3Transform transformBtoA = b3InvMulWorldTransforms( xfA, xfB );
if ( typeA == b3_sphereShape )
{
@ -567,7 +567,7 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact*
// Contact points are computed in frame A
target->anchorA = b3MulMV( matrixA, source->point );
target->anchorB = b3Add( target->anchorA, b3Sub( xfA.p, xfB.p ) );
target->anchorB = b3Add( target->anchorA, b3SubPos( xfA.p, xfB.p ) );
target->separation = source->separation;
target->featureId = b3MakeFeatureId( source->pair );
target->triangleIndex = B3_NULL_INDEX;
@ -606,8 +606,8 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact*
return true;
}
static bool b3UpdateConvexContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Transform xfA,
b3Shape* shapeB, b3Transform xfB, bool flip, b3Arena arena )
static bool b3UpdateConvexContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3WorldTransform xfA,
b3Shape* shapeB, b3WorldTransform xfB, bool flip, b3Arena arena )
{
// Compute new manifold
bool touching = b3ComputeConvexManifold( world, workerIndex, contact, shapeA, xfA, shapeB, xfB, arena );
@ -693,7 +693,7 @@ static bool b3UpdateConvexContact( b3World* world, int workerIndex, b3Contact* c
b3ShapeId shapeIdB = { shapeB->id + 1, world->worldId, shapeB->generation };
// this call assumes thread safety
b3Vec3 point = b3Add( contact->manifolds[0].points[0].anchorA, xfA.p );
b3Pos point = b3OffsetPos( xfA.p, contact->manifolds[0].points[0].anchorA );
b3Vec3 normal = contact->manifolds[0].normal;
touching = world->preSolveFcn( shapeIdA, shapeIdB, point, normal, world->preSolveContext );
if ( touching == false )
@ -720,8 +720,8 @@ static bool b3UpdateConvexContact( b3World* world, int workerIndex, b3Contact* c
// Update the contact manifold and touching status.
// Note: do not assume the shape AABBs are overlapping or are valid.
bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, b3Transform xfA,
b3Shape* shapeB, b3Vec3 localCenterB, b3Transform xfB, bool isFast, b3Arena arena )
bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, b3WorldTransform xfA,
b3Shape* shapeB, b3Vec3 localCenterB, b3WorldTransform xfB, bool isFast, b3Arena arena )
{
bool touching;
@ -756,14 +756,14 @@ bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Sha
else if ( child.type == b3_hullShape )
{
childShapeA.hull = child.hull;
b3Transform xfChild = b3MulTransforms( xfA, child.transform );
b3WorldTransform xfChild = b3MulWorldTransforms( xfA, child.transform );
bool flip = false;
touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfChild, shapeB, xfB, flip, arena );
}
else if ( child.type == b3_meshShape )
{
childShapeA.mesh = child.mesh;
b3Transform xfChild = b3MulTransforms( xfA, child.transform );
b3WorldTransform xfChild = b3MulWorldTransforms( xfA, child.transform );
touching = b3ComputeMeshManifolds( world, workerIndex, contact, &childShapeA, child.materialIndices, xfChild, shapeB,
xfB, isFast, arena );

View file

@ -133,8 +133,10 @@ typedef struct b3Contact
b3Manifold* manifolds;
int manifoldCount;
b3Transform cachedTransformA;
b3Transform cachedTransformB;
// Cache for contact recycling.
b3Quat cachedRotationA;
b3Quat cachedRotationB;
b3Transform cachedRelativePose;
// Mixed friction and restitution
float friction;
@ -171,8 +173,8 @@ void b3InitializeContactRegisters( void );
void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex );
void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies );
bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, b3Transform xfA,
b3Shape* shapeB, b3Vec3 localCenterB, b3Transform xfB, bool isFast, b3Arena arena );
bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, b3WorldTransform xfA,
b3Shape* shapeB, b3Vec3 localCenterB, b3WorldTransform xfB, bool isFast, b3Arena arena );
bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap,
b3Transform xfA, const b3Shape* shapeB, b3Transform xfB, bool isFast, b3Arena arena );
b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena );

View file

@ -586,8 +586,7 @@ void b3CollideHullAndSphere( b3LocalManifold* manifold, int capacity, const b3Hu
b3DistanceInput distanceInput;
distanceInput.proxyA = (b3ShapeProxy){ b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f };
distanceInput.proxyB = (b3ShapeProxy){ &center, 1, 0.0f };
distanceInput.transformA = b3Transform_identity;
distanceInput.transformB = b3Transform_identity;
distanceInput.transform = b3Transform_identity;
distanceInput.useRadii = false;
float radiusA = 0.0f;
@ -948,8 +947,7 @@ void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3H
b3DistanceInput distanceInput;
distanceInput.proxyA = (b3ShapeProxy){ b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f };
distanceInput.proxyB = (b3ShapeProxy){ &capsuleB->center1, 2, 0.0f };
distanceInput.transformA = b3Transform_identity;
distanceInput.transformB = transformBtoA;
distanceInput.transform = transformBtoA;
distanceInput.useRadii = false;
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 );

View file

@ -105,6 +105,15 @@ b3Version b3GetVersion( void )
return (b3Version){ 0, 2, 0 };
}
bool b3IsDoublePrecision( void )
{
#if defined( BOX3D_DOUBLE_PRECISION )
return true;
#else
return false;
#endif
}
static b3AllocFcn* b3_allocFcn = NULL;
static b3FreeFcn* b3_freeFcn = NULL;

View file

@ -712,17 +712,12 @@ static void b3ComputeWitnessPoints( const b3Simplex* simplex, b3Vec3* vertexA, b
b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* cache, b3Simplex* simplexes, int simplexCapacity )
{
// Build initial simplex
b3Transform transformA = input->transformA;
b3Transform transformB = input->transformB;
// Work in relative space
b3Transform xf = b3InvMulTransforms( transformA, transformB );
// The query runs in frame A using the relative pose of B in A.
b3Transform xf = input->transform;
// Use matrices for faster math
b3Matrix3 m = b3MakeMatrixFromQuat( xf.q );
b3Matrix3 mt = b3Transpose( m );
b3Matrix3 mA = b3MakeMatrixFromQuat( transformA.q );
const b3ShapeProxy* proxyA = &input->proxyA;
const b3ShapeProxy* proxyB = &input->proxyB;
@ -850,8 +845,8 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache*
// Overlap
b3Vec3 localPointA, localPointB;
b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB );
distanceOutput.pointA = b3Add( b3MulMV( mA, localPointA ), transformA.p );
distanceOutput.pointB = b3Add( b3MulMV( mA, localPointB ), transformA.p );
distanceOutput.pointA = localPointA;
distanceOutput.pointB = localPointB;
return distanceOutput;
}
@ -946,8 +941,8 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache*
// Thus the shapes are overlapped.
b3Vec3 localPointA, localPointB;
b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB );
distanceOutput.pointA = b3Add( b3MulMV( mA, localPointA ), transformA.p );
distanceOutput.pointB = b3Add( b3MulMV( mA, localPointB ), transformA.p );
distanceOutput.pointA = localPointA;
distanceOutput.pointB = localPointB;
B3_VALIDATE( b3Distance( localPointA, localPointB ) < FLT_EPSILON );
return distanceOutput;
}
@ -1001,10 +996,9 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache*
b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB );
b3WriteCache( cache, &simplex );
// Convert results from frame A into world space
normal = b3MulMV( mA, normal );
distanceOutput.pointA = b3Add( b3MulMV( mA, localPointA ), transformA.p );
distanceOutput.pointB = b3Add( b3MulMV( mA, localPointB ), transformA.p );
// Results stay in frame A
distanceOutput.pointA = localPointA;
distanceOutput.pointB = localPointB;
distanceOutput.distance = b3Distance( localPointA, localPointB );
distanceOutput.normal = normal;
distanceOutput.iterations = iteration;
@ -1053,10 +1047,12 @@ b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input )
b3DistanceInput distanceInput = { 0 };
distanceInput.proxyA = input->proxyA;
distanceInput.proxyB = input->proxyB;
distanceInput.transformA = input->transformA;
distanceInput.transformB = input->transformB;
distanceInput.useRadii = false;
// The whole cast runs in frame A. Advance the relative pose of B in float each iteration,
// which keeps the math near the local origin and avoids re-relativizing world poses.
distanceInput.transform = input->transform;
b3Vec3 delta2 = input->translationB;
b3DistanceOutput distanceOutput = { 0 };
b3CastOutput output = { 0 };
@ -1111,14 +1107,9 @@ b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input )
b3Log( "radiusB = %.9f", input->proxyB.radius );
{
b3Transform xfA = input->transformA;
b3Log( "xfA = {{%.9f, %.9f, %.9f}, {{%.9f, %.9f, %.9f}, %.9f}", xfA.p.x, xfA.p.y, xfA.p.z, xfA.q.v.x,
xfA.q.v.y, xfA.q.v.z, xfA.q.s );
}
{
b3Transform xfB = input->transformB;
b3Log( "xfB = {{%.9f, %.9f, %.9f}, {{%.9f, %.9f, %.9f}, %.9f}", xfB.p.x, xfB.p.y, xfB.p.z, xfB.q.v.x,
xfB.q.v.y, xfB.q.v.z, xfB.q.s );
b3Transform xf = input->transform;
b3Log( "transform = {{%.9f, %.9f, %.9f}, {{%.9f, %.9f, %.9f}, %.9f}", xf.p.x, xf.p.y, xf.p.z, xf.q.v.x,
xf.q.v.y, xf.q.v.z, xf.q.s );
}
{
@ -1162,7 +1153,7 @@ b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input )
return output;
}
distanceInput.transformB.p = b3MulAdd( input->transformB.p, alpha, delta2 );
distanceInput.transform.p = b3MulAdd( input->transform.p, alpha, delta2 );
}
// Failure!
@ -1257,10 +1248,10 @@ typedef struct b3SeparationFunction
static b3SeparationFunction b3MakeSeparationFunction( const b3SimplexCache cache, const b3ShapeProxy* proxyA,
const b3Sweep* sweepA, const b3ShapeProxy* proxyB, const b3Sweep* sweepB,
const b3DistanceOutput* query, float t1 )
b3Vec3 worldNormal, float t1 )
{
B3_ASSERT( 1 <= cache.count && cache.count <= 3 );
B3_VALIDATE( b3IsNormalized( query->normal ) );
B3_VALIDATE( b3IsNormalized( worldNormal ) );
b3SeparationFunction fcn = { 0 };
fcn.proxyA = proxyA;
@ -1290,7 +1281,7 @@ static b3SeparationFunction b3MakeSeparationFunction( const b3SimplexCache cache
{
// Witness is the world space direction
fcn.type = b3_separationVertices;
fcn.witness1 = query->normal;
fcn.witness1 = worldNormal;
}
break;
@ -1318,7 +1309,7 @@ static b3SeparationFunction b3MakeSeparationFunction( const b3SimplexCache cache
{
// The axis is not safe to normalize so we use a world axis instead!
fcn.type = b3_separationVertices;
fcn.witness1 = query->normal;
fcn.witness1 = worldNormal;
}
else
{
@ -1351,11 +1342,11 @@ static b3SeparationFunction b3MakeSeparationFunction( const b3SimplexCache cache
}
else
{
B3_VALIDATE( b3IsNormalized( query->normal ) );
B3_VALIDATE( b3IsNormalized( worldNormal ) );
// Vertex versus edge, use world axis witness
fcn.type = b3_separationVertices;
fcn.witness1 = query->normal;
fcn.witness1 = worldNormal;
}
}
break;
@ -1447,7 +1438,7 @@ static b3SeparationFunction b3MakeSeparationFunction( const b3SimplexCache cache
{
// The axis is not safe to normalize so we use a world axis instead!
fcn.type = b3_separationVertices;
fcn.witness1 = query->normal;
fcn.witness1 = worldNormal;
}
else
{
@ -1693,11 +1684,17 @@ b3TOIOutput b3TimeOfImpact( const b3TOIInput* input )
for ( ;; )
{
// Get the distance between shapes. We can also use the results to get a separating axis.
distanceInput.transformA = b3GetSweepTransform( &sweepA, t1 );
distanceInput.transformB = b3GetSweepTransform( &sweepB, t1 );
b3Transform xfA = b3GetSweepTransform( &sweepA, t1 );
b3Transform xfB = b3GetSweepTransform( &sweepB, t1 );
distanceInput.transform = b3InvMulTransforms( xfA, xfB );
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 );
output.distance = distanceOutput.distance;
// The distance query runs in frame A, project the witness data back to the shifted world
b3Vec3 worldNormal = b3RotateVector( xfA.q, distanceOutput.normal );
b3Vec3 worldPointA = b3TransformPoint( xfA, distanceOutput.pointA );
b3Vec3 worldPointB = b3TransformPoint( xfA, distanceOutput.pointB );
output.distanceIterations += 1;
distanceIterations += 1;
@ -1715,11 +1712,11 @@ b3TOIOutput b3TimeOfImpact( const b3TOIInput* input )
output.state = b3_toiStateHit;
// Averaged hit point
b3Vec3 pA = b3MulAdd( distanceOutput.pointA, proxyA.radius, distanceOutput.normal );
b3Vec3 pB = b3MulAdd( distanceOutput.pointB, -proxyB.radius, distanceOutput.normal );
b3Vec3 pA = b3MulAdd( worldPointA, proxyA.radius, worldNormal );
b3Vec3 pB = b3MulAdd( worldPointB, -proxyB.radius, worldNormal );
output.point = b3Lerp( pA, pB, 0.5f );
output.point = b3Add( output.point, origin );
output.normal = distanceOutput.normal;
output.normal = worldNormal;
output.fraction = t1;
break;
}
@ -1732,17 +1729,17 @@ b3TOIOutput b3TimeOfImpact( const b3TOIInput* input )
output.fraction = t1;
// Averaged hit point
b3Vec3 pA = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal );
b3Vec3 pB = b3MulAdd( distanceOutput.pointB, -input->proxyB.radius, distanceOutput.normal );
b3Vec3 pA = b3MulAdd( worldPointA, input->proxyA.radius, worldNormal );
b3Vec3 pB = b3MulAdd( worldPointB, -input->proxyB.radius, worldNormal );
output.point = b3Lerp( pA, pB, 0.5f );
output.point = b3Add( output.point, origin );
output.normal = distanceOutput.normal;
output.normal = worldNormal;
break;
}
// Initialize the separating axis.
b3SeparationFunction function =
b3MakeSeparationFunction( cache, &proxyA, &sweepA, &proxyB, &sweepB, &distanceOutput, t1 );
b3MakeSeparationFunction( cache, &proxyA, &sweepA, &proxyB, &sweepB, worldNormal, t1 );
#if B3_ENABLE_VALIDATION && 0
// todo this can give a negative value for diagonal edge contact on faces, typical GJK problem
@ -1890,11 +1887,11 @@ b3TOIOutput b3TimeOfImpact( const b3TOIInput* input )
if ( done )
{
// Averaged hit point
b3Vec3 pA = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal );
b3Vec3 pB = b3MulAdd( distanceOutput.pointB, -input->proxyB.radius, distanceOutput.normal );
b3Vec3 pA = b3MulAdd( worldPointA, input->proxyA.radius, worldNormal );
b3Vec3 pB = b3MulAdd( worldPointB, -input->proxyB.radius, worldNormal );
output.point = b3Lerp( pA, pB, 0.5f );
output.point = b3Add( output.point, origin );
output.normal = distanceOutput.normal;
output.normal = worldNormal;
break;
}
}

View file

@ -80,12 +80,12 @@ float b3DistanceJoint_GetCurrentLength( b3JointId jointId )
return 0.0f;
}
b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3Vec3 pA = b3TransformPoint( transformA, base->localFrameA.p );
b3Vec3 pB = b3TransformPoint( transformB, base->localFrameB.p );
b3Vec3 d = b3Sub( pB, pA );
b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p );
b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p );
b3Vec3 d = b3SubPos( pB, pA );
float length = b3Length( d );
return length;
}
@ -195,12 +195,12 @@ b3Vec3 b3GetDistanceJointForce( b3World* world, b3JointSim* base )
{
b3DistanceJoint* joint = &base->distanceJoint;
b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3Vec3 pA = b3TransformPoint( transformA, base->localFrameA.p );
b3Vec3 pB = b3TransformPoint( transformB, base->localFrameB.p );
b3Vec3 d = b3Sub( pB, pA );
b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p );
b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p );
b3Vec3 d = b3SubPos( pB, pA );
b3Vec3 axis = b3Normalize( d );
float force = ( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ) * world->inv_h;
return b3MulSV( force, axis );
@ -262,7 +262,7 @@ void b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context )
// initial anchors in world space
joint->anchorA = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
joint->anchorB = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center );
joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
b3Vec3 rA = joint->anchorA;
b3Vec3 rB = joint->anchorB;
@ -514,21 +514,21 @@ void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBia
}
}
void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB )
void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB )
{
B3_ASSERT( base->type == b3_distanceJoint );
b3DistanceJoint* joint = &base->distanceJoint;
b3Vec3 pA = b3TransformPoint( transformA, base->localFrameA.p );
b3Vec3 pB = b3TransformPoint( transformB, base->localFrameB.p );
b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p );
b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p );
b3Vec3 axis = b3Normalize( b3Sub( pB, pA ) );
b3Vec3 axis = b3Normalize( b3SubPos( pB, pA ) );
if ( joint->minLength < joint->maxLength && joint->enableLimit )
{
b3Vec3 pMin = b3MulAdd( pA, joint->minLength, axis );
b3Vec3 pMax = b3MulAdd( pA, joint->maxLength, axis );
b3Pos pMin = b3OffsetPos( pA, b3MulSV( joint->minLength, axis ) );
b3Pos pMax = b3OffsetPos( pA, b3MulSV( joint->maxLength, axis ) );
if ( joint->minLength > B3_LINEAR_SLOP )
{
@ -552,7 +552,7 @@ void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform trans
if ( joint->hertz > 0.0f && joint->enableSpring )
{
b3Vec3 pRest = b3MulAdd( pA, joint->length, axis );
b3Pos pRest = b3OffsetPos( pA, b3MulSV( joint->length, axis ) );
draw->DrawPointFcn( pRest, 4.0f, b3_colorBlue, draw->context );
}
}

View file

@ -761,12 +761,6 @@ static void b3RemoveLeaf( b3DynamicTree* tree, int leaf )
int b3DynamicTree_CreateProxy( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData )
{
B3_ASSERT( b3IsValidAABB( aabb ) );
B3_VALIDATE( -B3_HUGE < aabb.lowerBound.x && aabb.lowerBound.x < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.lowerBound.y && aabb.lowerBound.y < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.lowerBound.z && aabb.lowerBound.z < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.upperBound.x && aabb.upperBound.x < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.upperBound.y && aabb.upperBound.y < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.upperBound.z && aabb.upperBound.z < B3_HUGE );
int proxyId = b3AllocateNode( tree );
b3TreeNode* node = tree->nodes + proxyId;
@ -805,12 +799,6 @@ int b3DynamicTree_GetProxyCount( const b3DynamicTree* tree )
void b3DynamicTree_MoveProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb )
{
B3_ASSERT( b3IsValidAABB( aabb ) );
B3_VALIDATE( -B3_HUGE < aabb.lowerBound.x && aabb.lowerBound.x < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.lowerBound.y && aabb.lowerBound.y < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.lowerBound.z && aabb.lowerBound.z < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.upperBound.x && aabb.upperBound.x < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.upperBound.y && aabb.upperBound.y < B3_HUGE );
B3_VALIDATE( -B3_HUGE < aabb.upperBound.z && aabb.upperBound.z < B3_HUGE );
B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity );
B3_ASSERT( b3IsLeaf( tree->nodes + proxyId ) );
@ -826,9 +814,6 @@ void b3DynamicTree_EnlargeProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb )
{
b3TreeNode* nodes = tree->nodes;
B3_VALIDATE( b3IsValidAABB( aabb ) );
B3_VALIDATE( aabb.upperBound.x - aabb.lowerBound.x < B3_HUGE );
B3_VALIDATE( aabb.upperBound.y - aabb.lowerBound.y < B3_HUGE );
B3_VALIDATE( aabb.upperBound.z - aabb.lowerBound.z < B3_HUGE );
B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity );
B3_VALIDATE( b3IsLeaf( tree->nodes + proxyId ) );
@ -1375,32 +1360,22 @@ b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInp
return result;
}
b3TreeStats b3DynamicTree_ShapeCast( const b3DynamicTree* tree, const b3ShapeCastInput* input, uint64_t maskBits,
bool requireAllBits, b3TreeShapeCastCallbackFcn* callback, void* context )
b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits,
b3TreeBoxCastCallbackFcn* callback, void* context )
{
b3TreeStats stats = { 0 };
if ( tree->nodeCount == 0 || input->proxy.count == 0 )
if ( tree->nodeCount == 0 )
{
return stats;
}
b3AABB originAABB = { input->proxy.points[0], input->proxy.points[0] };
for ( int i = 1; i < input->proxy.count; ++i )
{
originAABB.lowerBound = b3Min( originAABB.lowerBound, input->proxy.points[i] );
originAABB.upperBound = b3Max( originAABB.upperBound, input->proxy.points[i] );
}
b3Vec3 radius = { input->proxy.radius, input->proxy.radius, input->proxy.radius };
originAABB.lowerBound = b3Sub( originAABB.lowerBound, radius );
originAABB.upperBound = b3Add( originAABB.upperBound, radius );
// The caller folds the shape radius and the world origin into the box
b3AABB originAABB = input->box;
b3Vec3 p1 = b3AABB_Center( originAABB );
b3Vec3 extension = b3AABB_Extents( originAABB );
// v is perpendicular to the segment.
b3Vec3 d = input->translation;
b3V32 pv1 = b3LoadV( &p1.x );
@ -1409,14 +1384,14 @@ b3TreeStats b3DynamicTree_ShapeCast( const b3DynamicTree* tree, const b3ShapeCas
float maxFraction = input->maxFraction;
// Build total box for the shape cast
// Build total box for the cast
b3Vec3 t = b3MulSV( maxFraction, input->translation );
b3AABB totalAABB = {
b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ),
b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ),
};
b3ShapeCastInput subInput = *input;
b3BoxCastInput subInput = *input;
const b3TreeNode* nodes = tree->nodes;
int stack[B3_TREE_STACK_SIZE];
@ -1428,7 +1403,6 @@ b3TreeStats b3DynamicTree_ShapeCast( const b3DynamicTree* tree, const b3ShapeCas
int nodeId = stack[--stackCount];
if ( nodeId == B3_NULL_INDEX )
{
// todo is this possible?
B3_ASSERT( false );
continue;
}
@ -1436,7 +1410,6 @@ b3TreeStats b3DynamicTree_ShapeCast( const b3DynamicTree* tree, const b3ShapeCas
const b3TreeNode* node = nodes + nodeId;
stats.nodeVisits += 1;
// todo look at disassembly
uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits );
if ( bitMatch == 0 || b3AABB_Overlaps( node->aabb, totalAABB ) == false )
@ -1462,13 +1435,12 @@ b3TreeStats b3DynamicTree_ShapeCast( const b3DynamicTree* tree, const b3ShapeCas
if ( value == 0.0f )
{
// The client has terminated the ray cast.
// The client has terminated the cast.
return stats;
}
if ( 0.0f < value && value < maxFraction )
{
// Update segment bounding box.
maxFraction = value;
t = b3MulSV( maxFraction, input->translation );
totalAABB.lowerBound = b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) );

View file

@ -755,9 +755,8 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S
B3_UNUSED( cellCount );
b3ShapeCastPairInput pairInput = { 0 };
pairInput.transformA = b3Transform_identity;
pairInput.proxyB = input->proxy;
pairInput.transformB = b3Transform_identity;
pairInput.transform = b3Transform_identity;
pairInput.translationB = input->translation;
pairInput.canEncroach = input->canEncroach;
@ -919,7 +918,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S
b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point21, origin ), b3Sub( point12, origin ) };
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
pairInput.maxFraction = bestFraction;
pairInput.transformB.p = b3Neg( origin );
pairInput.transform.p = b3Neg( origin );
b3CastOutput pairOutput = b3ShapeCast( &pairInput );
@ -939,7 +938,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S
b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point22, origin ), b3Sub( point12, origin ) };
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
pairInput.maxFraction = bestFraction;
pairInput.transformB.p = b3Neg( origin );
pairInput.transform.p = b3Neg( origin );
b3CastOutput pairOutput = b3ShapeCast( &pairInput );
@ -1047,8 +1046,7 @@ bool b3OverlapHeightField( const b3HeightField* shape, b3Transform shapeTransfor
b3DistanceInput input;
input.proxyB = localProxy;
input.transformA = b3Transform_identity;
input.transformB = b3Transform_identity;
input.transform = b3Transform_identity;
input.useRadii = true;
b3SimplexCache cache = { 0 };
@ -1202,8 +1200,7 @@ int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3H
{
b3DistanceInput distanceInput = { 0 };
distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f };
distanceInput.transformA = b3Transform_identity;
distanceInput.transformB = b3Transform_identity;
distanceInput.transform = b3Transform_identity;
distanceInput.useRadii = false;
b3SimplexCache cache = { 0 };

View file

@ -157,7 +157,7 @@ static inline void b3QHList_Init( b3QHListNode* head )
head->next = head;
}
#define B3_LIST_EMPTY( A ) ( ( A )->next == (A) )
#define B3_LIST_EMPTY( A ) ( ( A )->next == ( A ) )
static inline bool b3QHList_Contains( const b3QHListNode* node )
{
@ -1837,6 +1837,41 @@ b3HullData* b3CreateCone( float height, float radius1, float radius2, int slices
return hull;
}
b3HullData* b3CreateRock( float radius )
{
int pointCount = 10;
// Golden ratio
const float phi = ( 1.0f + sqrtf( 5.0f ) ) / 2.0f;
// Fibonacci lattice
b3Vec3 points[10];
// Azimuthal angle
float theta = 2.0f * B3_PI / phi;
b3CosSin cs = { 1.0f, 0.0 };
b3CosSin deltaCS = b3ComputeCosSin( theta );
for ( int i = 0; i < pointCount; ++i )
{
// Z coordinate
float z = 1.0f - ( 2.0f * i + 1.0f ) / pointCount;
// Radius in xy-plane
float radius_XY = sqrtf( 1.0f - z * z );
points[i].x = radius * radius_XY * cs.cosine;
points[i].y = radius * radius_XY * cs.sine;
points[i].z = radius * z;
b3CosSin cs0 = cs;
cs.cosine = deltaCS.cosine * cs0.cosine - deltaCS.sine * cs0.sine;
cs.sine = deltaCS.sine * cs0.cosine + deltaCS.cosine * cs0.sine;
}
return b3CreateHull( points, pointCount, pointCount );
}
static void b3UpdateHullBounds( b3HullData* hull )
{
const b3Vec3* points = b3GetHullPoints( hull );
@ -2384,8 +2419,8 @@ b3MassData b3ComputeHullMass( const b3HullData* shape, float density )
out.mass = density * shape->volume;
out.center = shape->center;
// todo_erin switch to central
out.inertia = b3AddMM( b3MulSM( density, shape->centralInertia ), b3Steiner( out.mass, out.center ) );
// Inertia about the center of mass
out.inertia = b3MulSM( density, shape->centralInertia );
return out;
}
@ -2408,8 +2443,7 @@ bool b3OverlapHull( const b3HullData* shape, b3Transform shapeTransform, const b
b3DistanceInput input;
input.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f };
input.proxyB = *proxy;
input.transformA = shapeTransform;
input.transformB = b3Transform_identity;
input.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity );
input.useRadii = true;
b3SimplexCache cache = { 0 };
@ -2492,8 +2526,7 @@ b3CastOutput b3ShapeCastHull( const b3HullData* shape, const b3ShapeCastInput* i
b3ShapeCastPairInput pairInput;
pairInput.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f };
pairInput.proxyB = input->proxy;
pairInput.transformA = b3Transform_identity;
pairInput.transformB = b3Transform_identity;
pairInput.transform = b3Transform_identity;
pairInput.translationB = input->translation;
pairInput.maxFraction = input->maxFraction;
pairInput.canEncroach = input->canEncroach;
@ -2508,8 +2541,7 @@ int b3CollideMoverAndHull( b3PlaneResult* result, const b3HullData* shape, const
b3DistanceInput distanceInput;
distanceInput.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f };
distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, mover->radius };
distanceInput.transformA = b3Transform_identity;
distanceInput.transformB = b3Transform_identity;
distanceInput.transform = b3Transform_identity;
distanceInput.useRadii = false;
float totalRadius = mover->radius;

View file

@ -1045,8 +1045,8 @@ void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, flo
b3SphericalJoint* joint = &sim->sphericalJoint;
linearImpulse = b3Length( joint->linearImpulse );
b3Transform xfA = b3GetBodyTransform( world, sim->bodyIdA );
b3Transform xfB = b3GetBodyTransform( world, sim->bodyIdB );
b3WorldTransform xfA = b3GetBodyTransform( world, sim->bodyIdA );
b3WorldTransform xfB = b3GetBodyTransform( world, sim->bodyIdB );
b3Quat qA = b3MulQuat( xfA.q, sim->localFrameA.q );
b3Quat qB = b3MulQuat( xfB.q, sim->localFrameB.q );
@ -1185,12 +1185,12 @@ float b3Joint_GetLinearSeparation( b3JointId jointId )
b3Joint* joint = b3GetJointFullId( world, jointId );
b3JointSim* base = b3GetJointSim( world, joint );
b3Transform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId );
b3Transform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId );
b3WorldTransform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId );
b3WorldTransform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId );
b3Vec3 pA = b3TransformPoint( xfA, base->localFrameA.p );
b3Vec3 pB = b3TransformPoint( xfB, base->localFrameB.p );
b3Vec3 dp = b3Sub( pB, pA );
b3Pos pA = b3TransformWorldPoint( xfA, base->localFrameA.p );
b3Pos pB = b3TransformWorldPoint( xfB, base->localFrameB.p );
b3Vec3 dp = b3SubPos( pB, pA );
switch ( joint->type )
{
@ -1309,8 +1309,8 @@ float b3Joint_GetAngularSeparation( b3JointId jointId )
b3Joint* joint = b3GetJointFullId( world, jointId );
b3JointSim* base = b3GetJointSim( world, joint );
b3Transform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId );
b3Transform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId );
b3WorldTransform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId );
b3WorldTransform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId );
b3Quat relQ = b3InvMulQuat( xfA.q, xfB.q );
@ -1638,10 +1638,10 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint )
b3JointSim* jointSim = b3GetJointSim( world, joint );
b3Transform transformA = b3GetBodyTransformQuick( world, bodyA );
b3Transform transformB = b3GetBodyTransformQuick( world, bodyB );
b3Vec3 pA = b3TransformPoint( transformA, jointSim->localFrameA.p );
b3Vec3 pB = b3TransformPoint( transformB, jointSim->localFrameB.p );
b3WorldTransform transformA = b3GetBodyTransformQuick( world, bodyA );
b3WorldTransform transformB = b3GetBodyTransformQuick( world, bodyB );
b3Pos pA = b3TransformWorldPoint( transformA, jointSim->localFrameA.p );
b3Pos pB = b3TransformWorldPoint( transformB, jointSim->localFrameB.p );
b3HexColor color = b3_colorDarkSeaGreen;
@ -1705,7 +1705,7 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint )
int colorIndex = joint->colorIndex;
if ( colorIndex != B3_NULL_INDEX )
{
b3Vec3 p = b3Lerp( pA, pB, 0.5f );
b3Pos p = b3LerpPosition( pA, pB, 0.5f );
draw->DrawPointFcn( p, 5.0f, graphColors[colorIndex], draw->context );
}
}
@ -1714,9 +1714,9 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint )
{
b3Vec3 force = b3GetJointConstraintForce( world, joint );
b3Vec3 torque = b3GetJointConstraintTorque( world, joint );
b3Vec3 p = b3Lerp( pA, pB, 0.5f );
b3Pos p = b3LerpPosition( pA, pB, 0.5f );
draw->DrawSegmentFcn( p, b3MulAdd( p, 0.001f, force ), b3_colorAzure, draw->context );
draw->DrawSegmentFcn( p, b3OffsetPos( p, b3MulSV( 0.001f, force ) ), b3_colorAzure, draw->context );
char buffer[64];
snprintf( buffer, 64, "f = %g, t = %g", b3Length( force ), b3Length( torque ) );

View file

@ -406,10 +406,10 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi
void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias );
void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias );
void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB );
void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale );
void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale );
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale );
void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale );
void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale );
void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale );
void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB );
void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );

View file

@ -96,6 +96,21 @@ bool b3IsValidAABB( b3AABB a )
return true;
}
bool b3IsBoundedAABB( b3AABB a )
{
if ( a.lowerBound.x < -B3_HUGE || a.lowerBound.y < -B3_HUGE || a.lowerBound.z < -B3_HUGE )
{
return false;
}
if ( a.upperBound.x > B3_HUGE || a.upperBound.y > B3_HUGE || a.upperBound.z > B3_HUGE )
{
return false;
}
return true;
}
bool b3IsSaneAABB( b3AABB a )
{
if ( b3IsValidAABB( a ) == false )
@ -131,6 +146,26 @@ bool b3IsValidPlane( b3Plane a )
return b3IsValidFloat( a.offset );
}
bool b3IsValidPosition( b3Pos p )
{
if ( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) )
{
return false;
}
if ( isinf( p.x ) || isinf( p.y ) || isinf( p.z ) )
{
return false;
}
return true;
}
bool b3IsValidWorldTransform( b3WorldTransform t )
{
return b3IsValidPosition( t.p ) && b3IsValidQuat( t.q );
}
// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4
float b3Atan2( float y, float x )
{

View file

@ -76,7 +76,6 @@ b3Matrix3 b3CylinderInertia( float mass, float radius, float height );
b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max );
// Inertia helper (Io = Ic + Is and Ic = Io - Is)
b3Matrix3 b3Steiner( float mass, b3Vec3 origin );
int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis );
int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis );

View file

@ -1791,8 +1791,7 @@ bool b3OverlapMesh( const b3Mesh* shape, b3Transform shapeTransform, const b3Sha
b3DistanceInput input;
input.proxyB = localProxy;
input.transformA = b3Transform_identity;
input.transformB = b3Transform_identity;
input.transform = b3Transform_identity;
input.useRadii = true;
int count = 0;
@ -2098,8 +2097,7 @@ b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input
b3ShapeCastPairInput pairInput;
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
pairInput.proxyB = input->proxy;
pairInput.transformA = b3Transform_identity;
pairInput.transformB = shiftedOrigin;
pairInput.transform = shiftedOrigin;
pairInput.maxFraction = bestOutput.fraction;
pairInput.translationB = input->translation;
pairInput.canEncroach = input->canEncroach;
@ -2212,8 +2210,7 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh
b3DistanceInput distanceInput = { 0 };
distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f };
distanceInput.transformA = b3Transform_identity;
distanceInput.transformB = b3Transform_identity;
distanceInput.transform = b3Transform_identity;
distanceInput.useRadii = false;
b3SimplexCache cache = { 0 };

View file

@ -74,7 +74,7 @@ static int b3QueryHeightFieldTriangles( int* indices, int capacity, const b3Heig
return context.count;
}
static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3Transform xfA, const b3AABB* bounds )
static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTransform xfA, const b3AABB* bounds )
{
B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape );
@ -106,8 +106,10 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3Transfo
int triangleIndices[B3_MAX_MESH_CONTACT_TRIANGLES];
// Bounds are in world space. Convert to the local mesh frame.
b3AABB localBounds = b3AABB_Transform( b3InvertTransform( xfA ), meshContact->queryBounds );
// Bounds are in world space. Convert to the local mesh frame. The broadphase bounds are float,
// so the demoted mesh transform is the matching float world frame (exact in float mode).
b3Transform meshTransform = b3ToRelativeTransform( xfA, b3Pos_zero );
b3AABB localBounds = b3AABB_Transform( b3InvertTransform( meshTransform ), meshContact->queryBounds );
int triangleCount;
if ( shapeA->type == b3_meshShape )
{
@ -521,7 +523,7 @@ typedef struct b3Cluster
} b3Cluster;
bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap,
b3Transform xfA, const b3Shape* shapeB, b3Transform xfB, bool isFast, b3Arena arena )
b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena )
{
B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape );
B3_UNUSED( workerIndex );
@ -549,7 +551,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
foundVertices.count = 0;
// This transform converts from mesh frame into the shapeB frame
b3Transform transformAtoB = b3InvMulTransforms( xfB, xfA );
b3Transform transformAtoB = b3InvMulWorldTransforms( xfB, xfA );
b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q );
float linearSlop = B3_LINEAR_SLOP;
@ -1006,7 +1008,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
}
b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q );
b3Vec3 offsetA = b3Sub( xfB.p, xfA.p );
b3Vec3 offsetA = b3SubPos( xfB.p, xfA.p );
const float normalMatchTolerance = 0.995f;
for ( int i = 0; i < clusterCount; ++i )

View file

@ -200,7 +200,7 @@ void b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context )
joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
// Compute the initial center delta. Incremental position updates are relative to this.
joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center );
joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
joint->linearSpring = b3MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h );
joint->angularSpring = b3MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h );

View file

@ -232,13 +232,13 @@ void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context )
}
}
void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale )
void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
{
float length = 0.1f * scale;
b3Transform frameA = b3MulTransforms( transformA, base->localFrameA );
draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length, b3RotateVector( frameA.q, b3Vec3_axisZ ) ), b3_colorGreen, draw->context );
b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorGreen, draw->context );
b3Transform frameB = b3MulTransforms( transformB, base->localFrameB );
draw->DrawSegmentFcn( frameB.p, b3MulAdd( frameB.p, length, b3RotateVector( frameB.q, b3Vec3_axisZ ) ), b3_colorBlue, draw->context );
b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
draw->DrawSegmentFcn( frameB.p, b3OffsetPos( frameB.p, b3MulSV( length, b3RotateVector( frameB.q, b3Vec3_axisZ ) ) ), b3_colorBlue, draw->context );
}

View file

@ -629,8 +629,8 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
}
}
b3Transform transformA = bodySimA->transform;
b3Transform transformB = bodySimB->transform;
b3WorldTransform transformA = bodySimA->transform;
b3WorldTransform transformB = bodySimB->transform;
bool isFast = ( bodySimA->flags & b3_isFast ) || ( bodySimB->flags & b3_isFast );
@ -647,12 +647,12 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
if ( ( isFast == false || isMeshContact == false ) && recycleDistance > 0.0f &&
( contact->flags & b3_relativeTransformValid ) && ( contact->flags & b3_contactRecycleFlag ) )
{
float angleA = b3DotQuat( transformA.q, contact->cachedTransformA.q );
float angleB = b3DotQuat( transformB.q, contact->cachedTransformB.q );
float angleA = b3DotQuat( transformA.q, contact->cachedRotationA );
float angleB = b3DotQuat( transformB.q, contact->cachedRotationB );
float angularDistance = b3MinFloat( angleA * angleA, angleB * angleB );
b3Transform xf = b3InvMulTransforms( transformA, transformB );
b3Transform xfc = b3InvMulTransforms( contact->cachedTransformA, contact->cachedTransformB );
b3Transform xf = b3InvMulWorldTransforms( transformA, transformB );
b3Transform xfc = contact->cachedRelativePose;
b3Vec3 maxExtentA = isStaticA ? b3Vec3_zero : bodySimA->maxExtent;
b3Vec3 maxExtentB = isStaticB ? b3Vec3_zero : bodySimB->maxExtent;
b3Vec3 maxExtent = b3Max( maxExtentA, maxExtentB );
@ -680,13 +680,13 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
float arcSq = 4.0f * b3LengthSquared( arc );
if ( arcSq < slack * slack )
{
b3Quat dqA = b3MulQuat( transformA.q, b3Conjugate( contact->cachedTransformA.q ) );
b3Quat dqB = b3MulQuat( transformB.q, b3Conjugate( contact->cachedTransformB.q ) );
b3Quat dqA = b3MulQuat( transformA.q, b3Conjugate( contact->cachedRotationA ) );
b3Quat dqB = b3MulQuat( transformB.q, b3Conjugate( contact->cachedRotationB ) );
b3Matrix3 matrixA = b3MakeMatrixFromQuat( dqA );
b3Matrix3 matrixB = b3MakeMatrixFromQuat( dqB );
// Minimize round-off
b3Vec3 dc = b3Sub( bodySimB->center, bodySimA->center );
b3Vec3 dc = b3SubPos( bodySimB->center, bodySimA->center );
int manifoldCount = contact->manifoldCount;
for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex )
@ -723,8 +723,9 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
}
// Caching for contact recycling.
contact->cachedTransformA = transformA;
contact->cachedTransformB = transformB;
contact->cachedRotationA = transformA.q;
contact->cachedRotationB = transformB.q;
contact->cachedRelativePose = b3InvMulWorldTransforms( transformA, transformB );
contact->flags |= b3_relativeTransformValid;
// This updates solid contacts
@ -1315,8 +1316,7 @@ static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context )
if ( draw->drawBounds )
{
b3AABB aabb = shape->fatAABB;
draw->DrawBoundsFcn( aabb, b3_colorGold, draw->context );
draw->DrawBoundsFcn( shape->fatAABB, b3_colorGold, draw->context );
}
return true;
@ -1383,8 +1383,8 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
if ( draw->drawBodyNames && body->name[0] != 0 )
{
b3Vec3 offset = { 0.05f, 0.05f, 0.05f };
b3Transform transform = { bodySim->center, bodySim->transform.q };
b3Vec3 p = b3TransformPoint( transform, offset );
b3WorldTransform transform = { bodySim->center, bodySim->transform.q };
b3Pos p = b3TransformWorldPoint( transform, offset );
draw->DrawStringFcn( p, body->name, b3_colorOrange, draw->context );
}
@ -1392,9 +1392,9 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
{
b3Vec3 offset = { 0.1f, 0.1f, 0.1f };
b3Transform transform = { bodySim->center, bodySim->transform.q };
b3WorldTransform transform = { bodySim->center, bodySim->transform.q };
draw->DrawTransformFcn( transform, draw->context );
b3Vec3 p = b3TransformPoint( transform, offset );
b3Pos p = b3TransformWorldPoint( transform, offset );
char buffer[32];
snprintf( buffer, 32, " %.2f", body->mass );
@ -1451,7 +1451,10 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
B3_ASSERT( manifold->pointCount > 0 );
b3Vec3 normal = manifold->normal;
b3Vec3 frictionCenter = b3Vec3_zero;
// Average the anchors not the world points so the friction center stays exact far from the origin
b3Pos contactCenter = draw->drawAnchorA == 1 ? bodySimA->center : bodySimB->center;
b3Vec3 anchorSum = b3Vec3_zero;
const b3ManifoldPoint* points = manifold->points;
for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex )
@ -1460,22 +1463,15 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
char buffer[32];
b3Vec3 p;
if ( draw->drawAnchorA == 1 )
{
p = b3Add( bodySimA->center, mp->anchorA );
}
else
{
p = b3Add( bodySimB->center, mp->anchorB );
}
b3Vec3 anchor = draw->drawAnchorA == 1 ? mp->anchorA : mp->anchorB;
b3Pos p = b3OffsetPos( contactCenter, anchor );
frictionCenter = b3Add( frictionCenter, p );
anchorSum = b3Add( anchorSum, anchor );
if ( draw->drawContactNormals )
{
b3Vec3 p1 = p;
b3Vec3 p2 = b3MulAdd( p1, axisScale, normal );
b3Pos p1 = p;
b3Pos p2 = b3OffsetPos( p1, b3MulSV( axisScale, normal ) );
draw->DrawSegmentFcn( p1, p2, b3_colorLightGray, draw->context );
snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %.2f", 100.0f * mp->separation );
@ -1488,8 +1484,8 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
// todo validate
// multiply by one-half due to relax iteration
float force = 0.5f * mp->totalNormalImpulse * inv_dt;
b3Vec3 p1 = p;
b3Vec3 p2 = b3MulAdd( p1, draw->forceScale * force, normal );
b3Pos p1 = p;
b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale * force, normal ) );
draw->DrawSegmentFcn( p1, p2, impulseColor, draw->context );
snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %.1f", force );
draw->DrawStringFcn( p1, buffer, b3_colorWhite, draw->context );
@ -1534,9 +1530,10 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
// Hack inv_dt for single step debugging
float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f;
b3Vec3 p1 = b3MulSV( 1.0f / manifold->pointCount, frictionCenter );
b3Vec3 avgAnchor = b3MulSV( 1.0f / manifold->pointCount, anchorSum );
b3Pos p1 = b3OffsetPos( contactCenter, avgAnchor );
b3Vec3 frictionForce = b3MulSV( 0.5f * inv_dt, manifold->frictionImpulse );
b3Vec3 p2 = b3MulAdd( p1, draw->forceScale, frictionForce );
b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale, frictionForce ) );
draw->DrawSegmentFcn( p1, p2, frictionColor, draw->context );
draw->DrawPointFcn( p1, 5.0f, frictionColor, draw->context );
@ -1608,9 +1605,10 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
for ( int j = 0; j < pointCount; ++j )
{
b3DebugPoint* point = points + j;
draw->DrawPointFcn( point->p, 5.0f, point->color, draw->context );
b3Pos p = point->p;
draw->DrawPointFcn( p, 5.0f, point->color, draw->context );
snprintf( buffer, 32, " %d, %.2f", point->label, point->value );
b3Vec3 ps = b3Add( point->p, offset );
b3Pos ps = b3OffsetPos( p, offset );
draw->DrawStringFcn( ps, buffer, point->color, draw->context );
}
@ -1619,10 +1617,12 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
for ( int j = 0; j < lineCount; ++j )
{
b3DebugLine* line = lines + j;
draw->DrawSegmentFcn( line->p1, line->p2, line->color, draw->context );
draw->DrawPointFcn( line->p1, 10.0f, line->color, draw->context );
b3Pos p1 = line->p1;
b3Pos p2 = line->p2;
draw->DrawSegmentFcn( p1, p2, line->color, draw->context );
draw->DrawPointFcn( p1, 10.0f, line->color, draw->context );
snprintf( buffer, 32, "%d", line->label );
b3Vec3 ps = b3Add( line->p1, offset );
b3Pos ps = b3OffsetPos( p1, offset );
draw->DrawStringFcn( ps, buffer, line->color, draw->context );
}
}
@ -2278,8 +2278,7 @@ void b3World_DumpMemoryStats( b3WorldId worldId )
int hullBucketCount = (int)b3HullMap_bucket_count( hullDatabase );
uint64_t hullMapBytes = b3HullMapByteCount( hullDatabase );
uint64_t hullDataBytes = 0;
for ( b3HullMap_itr itr = b3HullMap_first( hullDatabase ); b3HullMap_is_end( itr ) == false;
itr = b3HullMap_next( itr ) )
for ( b3HullMap_itr itr = b3HullMap_first( hullDatabase ); b3HullMap_is_end( itr ) == false; itr = b3HullMap_next( itr ) )
{
hullDataBytes += itr.data->key->byteCount;
}
@ -2537,6 +2536,7 @@ typedef struct WorldOverlapContext
b3OverlapResultFcn* fcn;
b3QueryFilter filter;
b3ShapeProxy proxy;
b3Pos origin;
void* userContext;
} WorldOverlapContext;
@ -2558,8 +2558,9 @@ static bool b3TreeOverlapCallback( int proxyId, uint64_t userData, void* context
return true;
}
// Re-center on the query origin so the overlap test stays in float precision far from the origin
b3Body* body = b3Array_Get( world->bodies, shape->bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin );
bool overlapping = b3OverlapShape( shape, transform, &worldContext->proxy );
if ( overlapping == false )
@ -2572,8 +2573,8 @@ static bool b3TreeOverlapCallback( int proxyId, uint64_t userData, void* context
return result;
}
b3TreeStats b3World_OverlapShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3QueryFilter filter, b3OverlapResultFcn* fcn,
void* context )
b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter,
b3OverlapResultFcn* fcn, void* context )
{
b3TreeStats treeStats = { 0 };
@ -2583,9 +2584,12 @@ b3TreeStats b3World_OverlapShape( b3WorldId worldId, const b3ShapeProxy* proxy,
return treeStats;
}
b3AABB aabb = b3MakeAABB( proxy->points, proxy->count, proxy->radius );
B3_ASSERT( b3IsValidPosition( origin ) );
// Bound the proxy in origin relative space then lift to a conservative world float box
b3AABB aabb = b3OffsetAABB( b3MakeAABB( proxy->points, proxy->count, proxy->radius ), origin );
WorldOverlapContext worldContext = {
world, fcn, filter, *proxy, context,
world, fcn, filter, *proxy, origin, context,
};
for ( int i = 0; i < b3_bodyTypeCount; ++i )
@ -2606,6 +2610,7 @@ typedef struct WorldMoverContext
b3PlaneResultFcn* fcn;
b3QueryFilter filter;
b3Capsule mover;
b3Pos origin;
void* userContext;
} WorldMoverContext;
@ -2627,8 +2632,10 @@ static bool TreeCollideCallback( int proxyId, uint64_t userData, void* context )
return true;
}
// Re-center on the query origin, the mover and the resulting planes are origin relative
b3Body* body = b3Array_Get( world->bodies, shape->bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body );
b3Transform transform = b3ToRelativeTransform( bodyTransform, worldContext->origin );
b3PlaneResult buffer[64];
int count = b3CollideMover( buffer, 64, shape, transform, &worldContext->mover );
@ -2644,7 +2651,8 @@ static bool TreeCollideCallback( int proxyId, uint64_t userData, void* context )
// It is tempting to use a shape proxy for the mover, but this makes handling deep overlap difficult and the generality may
// not be worth it.
void b3World_CollideMover( b3WorldId worldId, const b3Capsule* mover, b3QueryFilter filter, b3PlaneResultFcn* fcn, void* context )
void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, b3PlaneResultFcn* fcn,
void* context )
{
b3World* world = b3GetUnlockedWorldFromId( worldId );
if ( world == NULL )
@ -2652,14 +2660,18 @@ void b3World_CollideMover( b3WorldId worldId, const b3Capsule* mover, b3QueryFil
return;
}
B3_ASSERT( b3IsValidPosition( origin ) );
b3Vec3 r = { mover->radius, mover->radius, mover->radius };
b3AABB aabb;
aabb.lowerBound = b3Sub( b3Min( mover->center1, mover->center2 ), r );
aabb.upperBound = b3Add( b3Max( mover->center1, mover->center2 ), r );
// Relative box lifted to world float with outward rounding, conservative for the tree
b3AABB relBox;
relBox.lowerBound = b3Sub( b3Min( mover->center1, mover->center2 ), r );
relBox.upperBound = b3Add( b3Max( mover->center1, mover->center2 ), r );
b3AABB aabb = b3OffsetAABB( relBox, origin );
WorldMoverContext worldContext = {
world, fcn, filter, *mover, context,
world, fcn, filter, *mover, origin, context,
};
for ( int i = 0; i < b3_bodyTypeCount; ++i )
@ -2674,6 +2686,7 @@ typedef struct WorldRayCastContext
b3CastResultFcn* fcn;
b3QueryFilter filter;
float fraction;
b3Pos origin;
void* userContext;
} WorldRayCastContext;
@ -2695,22 +2708,26 @@ static float RayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t
}
b3Body* body = b3Array_Get( world->bodies, shape->bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3CastOutput output = b3RayCastShape( shape, transform, input );
b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body );
b3Transform transform = b3ToRelativeTransform( bodyTransform, worldContext->origin );
b3RayCastInput localInput = *input;
localInput.origin = b3Vec3_zero;
b3CastOutput output = b3RayCastShape( shape, transform, &localInput );
if ( output.hit )
{
B3_ASSERT( output.fraction <= input->maxFraction );
b3ShapeId id = { shapeId + 1, world->worldId, shape->generation };
b3Pos point = b3OffsetPos( worldContext->origin, output.point );
int materialIndex = b3ClampInt( output.materialIndex, 0, shape->materialCount - 1 );
uint64_t userMaterialId = shape->materials[materialIndex].userMaterialId;
int triangleIndex = output.triangleIndex;
int childIndex = output.childIndex;
float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, userMaterialId, triangleIndex,
childIndex, worldContext->userContext );
float fraction = worldContext->fcn( id, point, output.normal, output.fraction, userMaterialId, triangleIndex, childIndex,
worldContext->userContext );
// The user may return -1 to skip this shape
if ( 0.0f <= fraction && fraction <= 1.0f )
@ -2724,7 +2741,7 @@ static float RayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t
return input->maxFraction;
}
b3TreeStats b3World_CastRay( b3WorldId worldId, b3Vec3 origin, b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn,
b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn,
void* context )
{
b3TreeStats treeStats = { 0 };
@ -2735,12 +2752,14 @@ b3TreeStats b3World_CastRay( b3WorldId worldId, b3Vec3 origin, b3Vec3 translatio
return treeStats;
}
B3_ASSERT( b3IsValidVec3( origin ) );
B3_ASSERT( b3IsValidPosition( origin ) );
B3_ASSERT( b3IsValidVec3( translation ) );
b3RayCastInput input = { origin, translation, 1.0f };
// The tree traverses in float relative to the world origin. Each shape is then re-differenced at
// full precision against the origin, so a hit stays accurate far from the origin.
b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f };
WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context };
WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, origin, context };
for ( int i = 0; i < b3_bodyTypeCount; ++i )
{
@ -2761,7 +2780,7 @@ b3TreeStats b3World_CastRay( b3WorldId worldId, b3Vec3 origin, b3Vec3 translatio
}
// This callback finds the closest hit. This is the most common callback used in games.
static float b3RayCastClosestFcn( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t userMaterialId,
static float b3RayCastClosestFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId,
int triangleIndex, int childIndex, void* context )
{
// Ignore initial overlap
@ -2782,7 +2801,7 @@ static float b3RayCastClosestFcn( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal
return fraction;
}
b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Vec3 origin, b3Vec3 translation, b3QueryFilter filter )
b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter )
{
b3RayResult result = { 0 };
@ -2792,15 +2811,18 @@ b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Vec3 origin, b3Vec3 tra
return result;
}
B3_ASSERT( b3IsValidVec3( origin ) );
B3_ASSERT( b3IsValidPosition( origin ) );
B3_ASSERT( b3IsValidVec3( translation ) );
b3RayCastInput input = { origin, translation, 1.0f };
// The tree traverses in float relative to the world origin. Each shape is then re-differenced at
// full precision against its body, so a hit stays accurate far from the origin.
b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f };
WorldRayCastContext worldContext = {
.world = world,
.fcn = b3RayCastClosestFcn,
.filter = filter,
.fraction = 1.0f,
.origin = origin,
.userContext = &result,
};
@ -2822,12 +2844,24 @@ b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Vec3 origin, b3Vec3 tra
return result;
}
static float b3ShapeCastCallback( const b3ShapeCastInput* input, int proxyId, uint64_t userData, void* context )
typedef struct WorldShapeCastContext
{
b3World* world;
b3CastResultFcn* fcn;
b3QueryFilter filter;
float fraction;
b3Pos origin;
// origin relative input
b3ShapeCastInput input;
void* userContext;
} WorldShapeCastContext;
static float b3ShapeCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context )
{
B3_UNUSED( proxyId );
int shapeId = (int)userData;
WorldRayCastContext* worldContext = (WorldRayCastContext*)context;
WorldShapeCastContext* worldContext = (WorldShapeCastContext*)context;
b3World* world = worldContext->world;
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
@ -2839,10 +2873,16 @@ static float b3ShapeCastCallback( const b3ShapeCastInput* input, int proxyId, ui
return input->maxFraction;
}
b3Body* body = b3Array_Get( world->bodies, shape->bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
// Rebuild from the origin relative input, taking only the advancing fraction from the tree.
// The tree box is world float and would lose the cast far from the origin.
b3ShapeCastInput localInput = worldContext->input;
localInput.maxFraction = input->maxFraction;
b3CastOutput output = b3ShapeCastShape( shape, transform, input );
// Re-center on the query origin so the per-shape cast stays in float precision far from the origin
b3Body* body = b3Array_Get( world->bodies, shape->bodyId );
b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin );
b3CastOutput output = b3ShapeCastShape( shape, transform, &localInput );
if ( output.hit )
{
@ -2852,8 +2892,8 @@ static float b3ShapeCastCallback( const b3ShapeCastInput* input, int proxyId, ui
int triangleIndex = output.triangleIndex;
int childIndex = output.childIndex;
float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, userMaterialId, triangleIndex,
childIndex, worldContext->userContext );
float fraction = worldContext->fcn( id, b3OffsetPos( worldContext->origin, output.point ), output.normal, output.fraction,
userMaterialId, triangleIndex, childIndex, worldContext->userContext );
// The user may return -1 to skip this shape
if ( 0.0f <= fraction && fraction <= 1.0f )
@ -2867,8 +2907,8 @@ static float b3ShapeCastCallback( const b3ShapeCastInput* input, int proxyId, ui
return input->maxFraction;
}
b3TreeStats b3World_CastShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3Vec3 translation, b3QueryFilter filter,
b3CastResultFcn* fcn, void* context )
b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,
b3QueryFilter filter, b3CastResultFcn* fcn, void* context )
{
b3TreeStats treeStats = { 0 };
@ -2878,20 +2918,31 @@ b3TreeStats b3World_CastShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3V
return treeStats;
}
B3_ASSERT( b3IsValidPosition( origin ) );
B3_ASSERT( b3IsValidVec3( translation ) );
b3ShapeCastInput input;
input.proxy = *proxy;
input.translation = translation;
input.maxFraction = 1.0f;
input.canEncroach = false;
WorldShapeCastContext worldContext = { 0 };
worldContext.world = world;
worldContext.fcn = fcn;
worldContext.filter = filter;
worldContext.fraction = 1.0f;
worldContext.origin = origin;
worldContext.input.proxy = *proxy;
worldContext.input.translation = translation;
worldContext.input.maxFraction = 1.0f;
worldContext.input.canEncroach = false;
worldContext.userContext = context;
WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context };
// Bound the proxy in origin relative space then lift to a conservative world float box. The tree
// node boxes use the same directed rounding, so the swept box never clips a shape far from the
// origin. Per shape casts re-difference at full precision against the carried origin.
b3AABB localBox = b3MakeAABB( proxy->points, proxy->count, proxy->radius );
b3BoxCastInput treeInput = { b3OffsetAABB( localBox, origin ), translation, 1.0f };
for ( int i = 0; i < b3_bodyTypeCount; ++i )
{
b3TreeStats treeResult = b3DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, false,
b3ShapeCastCallback, &worldContext );
b3TreeStats treeResult = b3DynamicTree_BoxCast( world->broadPhase.trees + i, &treeInput, filter.maskBits, false,
b3ShapeCastCallback, &worldContext );
treeStats.nodeVisits += treeResult.nodeVisits;
treeStats.leafVisits += treeResult.leafVisits;
@ -2900,7 +2951,7 @@ b3TreeStats b3World_CastShape( b3WorldId worldId, const b3ShapeProxy* proxy, b3V
return treeStats;
}
input.maxFraction = worldContext.fraction;
treeInput.maxFraction = worldContext.fraction;
}
return treeStats;
@ -2912,10 +2963,13 @@ typedef struct WorldMoverCastContext
b3MoverFilterFcn* fcn;
b3QueryFilter filter;
float fraction;
b3Pos origin;
// origin relative input
b3ShapeCastInput input;
void* userContext;
} WorldMoverCastContext;
static float MoverCastCallback( const b3ShapeCastInput* input, int proxyId, uint64_t userData, void* context )
static float MoverCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context )
{
B3_UNUSED( proxyId );
@ -2942,10 +2996,15 @@ static float MoverCastCallback( const b3ShapeCastInput* input, int proxyId, uint
}
}
b3Body* body = b3Array_Get( world->bodies, shape->bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
// Rebuild from the origin relative input, taking only the advancing fraction from the tree
b3ShapeCastInput localInput = worldContext->input;
localInput.maxFraction = input->maxFraction;
b3CastOutput output = b3ShapeCastShape( shape, transform, input );
// Re-center on the query origin so the per-shape cast stays in float precision far from the origin
b3Body* body = b3Array_Get( world->bodies, shape->bodyId );
b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin );
b3CastOutput output = b3ShapeCastShape( shape, transform, &localInput );
if ( output.fraction == 0.0f )
{
// Ignore overlapping shapes
@ -2956,9 +3015,10 @@ static float MoverCastCallback( const b3ShapeCastInput* input, int proxyId, uint
return output.fraction;
}
float b3World_CastMover( b3WorldId worldId, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter,
float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter,
b3MoverFilterFcn* fcn, void* context )
{
B3_ASSERT( b3IsValidPosition( origin ) );
B3_ASSERT( b3IsValidVec3( translation ) );
b3World* world = b3GetUnlockedWorldFromId( worldId );
@ -2967,30 +3027,34 @@ float b3World_CastMover( b3WorldId worldId, const b3Capsule* mover, b3Vec3 trans
return 1.0f;
}
b3ShapeCastInput input;
input.proxy = (b3ShapeProxy){ &mover->center1, 2, mover->radius };
input.translation = translation;
input.maxFraction = 1.0f;
input.canEncroach = mover->radius > 0.0f;
WorldMoverCastContext worldContext = {
.world = world,
.fcn = fcn,
.filter = filter,
.fraction = 1.0f,
.origin = origin,
.userContext = context,
};
worldContext.input.proxy = (b3ShapeProxy){ &mover->center1, 2, mover->radius };
worldContext.input.translation = translation;
worldContext.input.maxFraction = 1.0f;
worldContext.input.canEncroach = mover->radius > 0.0f;
// Bound the capsule in origin relative space then lift to a conservative world float box
b3Vec3 centers[2] = { mover->center1, mover->center2 };
b3BoxCastInput treeInput = { b3OffsetAABB( b3MakeAABB( centers, 2, mover->radius ), origin ), translation, 1.0f };
for ( int i = 0; i < b3_bodyTypeCount; ++i )
{
b3DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, false, MoverCastCallback, &worldContext );
b3DynamicTree_BoxCast( world->broadPhase.trees + i, &treeInput, filter.maskBits, false, MoverCastCallback,
&worldContext );
if ( worldContext.fraction == 0.0f )
{
return 0.0f;
}
input.maxFraction = worldContext.fraction;
treeInput.maxFraction = worldContext.fraction;
}
return worldContext.fraction;
@ -3033,7 +3097,7 @@ b3Vec3 b3World_GetGravity( b3WorldId worldId )
typedef struct ExplosionContext
{
b3World* world;
b3Vec3 position;
b3Pos position;
float radius;
float falloff;
float impulsePerArea;
@ -3056,13 +3120,16 @@ static bool ExplosionCallback( int proxyId, uint64_t userData, void* context )
b3Body* body = b3Array_Get( world->bodies, shape->bodyId );
B3_ASSERT( body->type == b3_dynamicBody );
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3WorldTransform xf = b3GetBodyTransformQuick( world, body );
// Re-center the explosion into the shape local frame so distance and direction stay precise
// far from the origin. Everything below runs in that near-origin frame.
b3Vec3 localPosition = b3InvTransformWorldPoint( xf, explosionContext->position );
b3DistanceInput input;
input.proxyA = b3MakeShapeProxy( shape );
input.proxyB = (b3ShapeProxy){ &explosionContext->position, 1, 0.0f };
input.transformA = transform;
input.transformB = b3Transform_identity;
input.proxyB = (b3ShapeProxy){ &localPosition, 1, 0.0f };
input.transform = b3Transform_identity;
input.useRadii = true;
b3SimplexCache cache = { 0 };
@ -3082,14 +3149,14 @@ static bool ExplosionCallback( int proxyId, uint64_t userData, void* context )
return true;
}
// Witness point is already in the body local query frame
b3Vec3 closestPoint = output.pointA;
if ( output.distance == 0.0f )
{
b3Vec3 localCentroid = b3GetShapeCentroid( shape );
closestPoint = b3TransformPoint( transform, localCentroid );
closestPoint = b3GetShapeCentroid( shape );
}
b3Vec3 direction = b3Sub( closestPoint, explosionContext->position );
b3Vec3 direction = b3Sub( closestPoint, localPosition );
if ( b3LengthSquared( direction ) > 100.0f * FLT_EPSILON * FLT_EPSILON )
{
direction = b3Normalize( direction );
@ -3107,15 +3174,17 @@ static bool ExplosionCallback( int proxyId, uint64_t userData, void* context )
}
float magnitude = explosionContext->impulsePerArea * area * scale * shape->explosionScale;
b3Vec3 impulse = b3MulSV( magnitude, direction );
b3Vec3 impulse = b3MulSV( magnitude, b3RotateVector( xf.q, direction ) );
int localIndex = body->localIndex;
b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet );
b3BodyState* state = b3Array_Get( set->bodyStates, localIndex );
b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex );
state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse );
state->angularVelocity = b3Add(
state->angularVelocity, b3MulMV( bodySim->invInertiaWorld, b3Cross( b3Sub( closestPoint, bodySim->center ), impulse ) ) );
// Lever arm from the center of mass to the closest point, rotated to world
b3Vec3 r = b3RotateVector( xf.q, b3Sub( closestPoint, bodySim->localCenter ) );
state->angularVelocity = b3Add( state->angularVelocity, b3MulMV( bodySim->invInertiaWorld, b3Cross( r, impulse ) ) );
return true;
}
@ -3123,12 +3192,12 @@ static bool ExplosionCallback( int proxyId, uint64_t userData, void* context )
void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionDef )
{
uint64_t maskBits = explosionDef->maskBits;
b3Vec3 position = explosionDef->position;
b3Pos position = explosionDef->position;
float radius = explosionDef->radius;
float falloff = explosionDef->falloff;
float impulsePerArea = explosionDef->impulsePerArea;
B3_ASSERT( b3IsValidVec3( position ) );
B3_ASSERT( b3IsValidPosition( position ) );
B3_ASSERT( b3IsValidFloat( radius ) && radius >= 0.0f );
B3_ASSERT( b3IsValidFloat( falloff ) && falloff >= 0.0f );
B3_ASSERT( b3IsValidFloat( impulsePerArea ) );
@ -3144,9 +3213,10 @@ void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionDef )
struct ExplosionContext explosionContext = { world, position, radius, falloff, impulsePerArea };
float totalRadius = radius + falloff;
b3Vec3 r = { totalRadius, totalRadius, totalRadius };
b3AABB aabb = { b3Sub( position, r ), b3Add( position, r ) };
// The broad-phase tree is float, so translate a local query box out to world with outward rounding
float extent = radius + falloff;
b3AABB localBox = { { -extent, -extent, -extent }, { extent, extent, extent } };
b3AABB aabb = b3OffsetAABB( localBox, position );
b3DynamicTree_Query( world->broadPhase.trees + b3_dynamicBody, aabb, maskBits, false, ExplosionCallback, &explosionContext );

View file

@ -53,7 +53,7 @@ enum b3SetType
typedef struct b3DebugPoint
{
b3Vec3 p;
b3Pos p;
int label;
float value;
b3HexColor color;
@ -61,7 +61,7 @@ typedef struct b3DebugPoint
typedef struct b3DebugLine
{
b3Vec3 p1, p2;
b3Pos p1, p2;
int label;
b3HexColor color;
} b3DebugLine;

View file

@ -94,15 +94,15 @@ float b3PrismaticJoint_GetTranslation( b3JointId jointId )
{
b3World* world = b3GetWorld( jointId.world0 );
b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3Vec3 jointAxis = b3RotateVector( base->localFrameA.q, b3Vec3_axisX );
jointAxis = b3RotateVector( transformA.q, jointAxis );
b3Vec3 anchorA = b3RotateVector( transformA.q, base->localFrameA.p );
b3Vec3 anchorB = b3RotateVector( transformB.q, base->localFrameB.p );
b3Vec3 d = b3Add( b3Sub( transformB.p, transformA.p ), b3Sub( anchorB, anchorA ) );
b3Vec3 d = b3Add( b3SubPos( transformB.p, transformA.p ), b3Sub( anchorB, anchorA ) );
float translation = b3Dot( d, jointAxis );
return translation;
}
@ -206,7 +206,7 @@ float b3PrismaticJoint_GetMaxMotorForce( b3JointId jointId )
b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base )
{
b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3PrismaticJoint* joint = &base->prismaticJoint;
// impulse in joint space
@ -229,7 +229,7 @@ b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base )
b3Vec3 b3GetPrismaticJointTorque( b3World* world, b3JointSim* base )
{
b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3PrismaticJoint* joint = &base->prismaticJoint;
b3Vec3 torque = b3MulSV( world->inv_h, joint->angularImpulse );
@ -275,7 +275,7 @@ void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context )
joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center );
joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
joint->rotationMass = b3InvertMatrix( invInertiaSum );
// Initial joint axes in world space
@ -631,10 +631,10 @@ void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBi
}
}
void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale )
void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
{
b3Transform frameA = b3MulTransforms( transformA, base->localFrameA );
b3Transform frameB = b3MulTransforms( transformB, base->localFrameB );
b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
b3Matrix3 R = b3MakeMatrixFromQuat( frameA.q );
b3Vec3 axis = R.cx;
@ -642,22 +642,22 @@ void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform tran
b3Vec3 perpZ = R.cz;
float s = 0.2f * scale;
draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, s, perpY ), b3_colorGreen, draw->context );
draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, s, perpZ ), b3_colorBlue, draw->context );
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( s, perpY ) ), b3_colorGreen, draw->context );
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( s, perpZ ) ), b3_colorBlue, draw->context );
b3PrismaticJoint* joint = &base->prismaticJoint;
if ( joint->enableLimit )
{
b3Vec3 p1 = b3MulAdd( frameA.p, joint->lowerTranslation, axis );
b3Vec3 p2 = b3MulAdd( frameA.p, joint->upperTranslation, axis );
b3Pos p1 = b3OffsetPos( frameA.p, b3MulSV( joint->lowerTranslation, axis ) );
b3Pos p2 = b3OffsetPos( frameA.p, b3MulSV( joint->upperTranslation, axis ) );
draw->DrawSegmentFcn( p1, p2, b3_colorOrange, draw->context );
draw->DrawPointFcn( p1, 10.0f, b3_colorGreen, draw->context );
draw->DrawPointFcn( p2, 10.0f, b3_colorRed, draw->context );
}
else
{
b3Vec3 p1 = b3MulSub( frameA.p, 0.5f * scale, axis );
b3Vec3 p2 = b3MulAdd( frameA.p, 0.5f * scale, axis );
b3Pos p1 = b3OffsetPos( frameA.p, b3MulSV( -0.5f * scale, axis ) );
b3Pos p2 = b3OffsetPos( frameA.p, b3MulSV( 0.5f * scale, axis ) );
draw->DrawSegmentFcn( p1, p2, b3_colorOrange, draw->context );
}

View file

@ -86,8 +86,8 @@ float b3RevoluteJoint_GetAngle( b3JointId jointId )
{
b3World* world = b3GetWorld( jointId.world0 );
b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q );
b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q );
@ -208,7 +208,7 @@ b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base )
b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base )
{
b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3RevoluteJoint* joint = &base->revoluteJoint;
b3Vec3 axis = b3RotateVector( base->localFrameA.q, b3Vec3_axisZ );
axis = b3RotateVector( transformA.q, axis );
@ -274,7 +274,7 @@ void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context )
joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center );
joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
{
// Rotation axis is the z-axis of body A.
@ -587,19 +587,19 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
}
}
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale )
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
{
b3Transform frameA = b3MulTransforms( transformA, base->localFrameA );
b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
float length1 = 0.1f * scale;
draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ), b3_colorRed,
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed,
draw->context );
draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ), b3_colorGreen,
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen,
draw->context );
draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ), b3_colorBlue,
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue,
draw->context );
b3Transform frameB = b3MulTransforms( transformB, base->localFrameB );
b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
b3RevoluteJoint* joint = &base->revoluteJoint;
enum { kSliceCount = 16 };
@ -631,19 +631,19 @@ void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform trans
if ( index == 0 )
{
draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, vertex1 ), b3_colorCyan, draw->context );
draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context );
}
if ( index == kSliceCount - 1 )
{
draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );
}
draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex1 ), b3TransformPoint( frameA, vertex2 ), b3_colorCyan,
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,
draw->context );
}
float twistAngle = b3GetTwistAngle( relQ );
b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f };
draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, p2 ), b3_colorYellow, draw->context );
draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, p2 ), b3_colorYellow, draw->context );
}
}

View file

@ -148,7 +148,7 @@ static bool b3SensorQueryCallback( int proxyId, uint64_t userData, void* context
}
}
b3Transform otherTransform = b3GetBodyTransform( world, otherShape->bodyId );
b3Transform otherTransform = b3ToRelativeTransform( b3GetBodyTransform( world, otherShape->bodyId ), b3Pos_zero );
bool overlap = b3OverlapSensor( sensorShape, queryContext->transform, otherShape, otherTransform );
if ( overlap == false )
@ -217,7 +217,7 @@ static void b3SensorTask( int startIndex, int endIndex, int workerIndex, void* c
continue;
}
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), b3Pos_zero );
b3SensorQueryContext queryContext = {
.world = world,

View file

@ -77,19 +77,13 @@ static float b3ComputeShapeMargin( b3Shape* shape )
return b3MinFloat( B3_MAX_AABB_MARGIN, B3_AABB_MARGIN_FRACTION * margin );
}
static void b3UpdateShapeAABBs( b3Shape* shape, b3Transform transform, b3BodyType proxyType )
static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3BodyType proxyType )
{
// Compute a bounding box with a speculative margin
const float speculativeDistance = B3_SPECULATIVE_DISTANCE;
const float aabbMargin = shape->aabbMargin;
b3AABB aabb = b3ComputeShapeAABB( shape, transform );
aabb.lowerBound.x -= speculativeDistance;
aabb.lowerBound.y -= speculativeDistance;
aabb.lowerBound.z -= speculativeDistance;
aabb.upperBound.x += speculativeDistance;
aabb.upperBound.y += speculativeDistance;
aabb.upperBound.z += speculativeDistance;
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance );
shape->aabb = aabb;
// Smaller margin for static bodies. Cannot be zero due to TOI tolerance.
@ -102,11 +96,9 @@ static void b3UpdateShapeAABBs( b3Shape* shape, b3Transform transform, b3BodyTyp
fatAABB.upperBound.y = aabb.upperBound.y + margin;
fatAABB.upperBound.z = aabb.upperBound.z + margin;
shape->fatAABB = fatAABB;
B3_VALIDATE( b3IsSaneAABB( fatAABB ) );
}
static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3Transform bodyTransform, const b3ShapeDef* def,
static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTransform bodyTransform, const b3ShapeDef* def,
const void* geometry, b3ShapeType shapeType, b3Transform shapeTransform, b3Vec3 scale,
bool haveShapeTransform )
{
@ -297,7 +289,7 @@ static b3ShapeId b3CreateShape( b3BodyId bodyId, const b3ShapeDef* def, const vo
world->locked = true;
b3Transform bodyTransform = b3GetBodyTransformQuick( world, body );
b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body );
b3Shape* shape =
b3CreateShapeInternal( world, body, bodyTransform, def, geometry, shapeType, transform, scale, haveTransform );
@ -521,6 +513,26 @@ b3AABB b3ComputeShapeAABB( const b3Shape* shape, b3Transform transform )
}
}
b3AABB b3ComputeFatShapeAABB( const b3Shape* shape, b3WorldTransform transform, float extra )
{
b3Vec3 r = { extra, extra, extra };
#if defined( BOX3D_DOUBLE_PRECISION )
// Build the box in the body local frame, inflate, then translate by the double origin and
// round outward. Inflating before the single rounding matters far from the origin where the
// float margin would otherwise vanish.
b3Transform rotation = { b3Vec3_zero, transform.q };
b3AABB localBox = b3ComputeShapeAABB( shape, rotation );
localBox.lowerBound = b3Sub( localBox.lowerBound, r );
localBox.upperBound = b3Add( localBox.upperBound, r );
return b3OffsetAABB( localBox, transform.p );
#else
b3AABB aabb = b3ComputeShapeAABB( shape, transform );
aabb.lowerBound = b3Sub( aabb.lowerBound, r );
aabb.upperBound = b3Add( aabb.upperBound, r );
return aabb;
#endif
}
b3AABB b3ComputeSweptShapeAABB( const b3Shape* shape, const b3Sweep* sweep, float time )
{
B3_ASSERT( 0.0f <= time && time <= 1.0f );
@ -894,7 +906,7 @@ int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* sha
return planeCount;
}
void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3Transform transform, bool forcePairCreation )
void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation )
{
B3_ASSERT( shape->proxyKey == B3_NULL_INDEX );
@ -1081,57 +1093,31 @@ const char* b3Shape_GetName( b3ShapeId shapeId )
}
// todo no tests
b3CastOutput b3Shape_RayCast( b3ShapeId shapeId, const b3RayCastInput* input )
b3WorldCastOutput b3Shape_RayCast( b3ShapeId shapeId, b3Pos origin, b3Vec3 translation )
{
B3_ASSERT( b3IsValidPosition( origin ) );
B3_ASSERT( b3IsValidVec3( translation ) );
b3World* world = b3GetWorld( shapeId.world0 );
b3Shape* shape = b3GetShape( world, shapeId );
b3Transform transform = b3GetBodyTransform( world, shape->bodyId );
// Re-center on the origin so the cast runs in float precision far from the world origin
b3Transform transform = b3ToRelativeTransform( b3GetBodyTransform( world, shape->bodyId ), origin );
// input in local coordinates
b3RayCastInput localInput;
localInput.origin = b3InvTransformPoint( transform, input->origin );
localInput.translation = b3InvRotateVector( transform.q, input->translation );
localInput.maxFraction = input->maxFraction;
// The ray starts at the origin, so its origin in the re-centered frame is zero
b3RayCastInput input = { b3Vec3_zero, translation, 1.0f };
b3CastOutput output = { 0 };
switch ( shape->type )
{
case b3_capsuleShape:
output = b3RayCastCapsule( &shape->capsule, &localInput );
break;
case b3_compoundShape:
output = b3RayCastCompound( shape->compound, &localInput );
break;
case b3_sphereShape:
output = b3RayCastSphere( &shape->sphere, &localInput );
break;
case b3_meshShape:
output = b3RayCastMesh( &shape->mesh, &localInput );
break;
case b3_hullShape:
output = b3RayCastHull( shape->hull, &localInput );
break;
case b3_heightShape:
output = b3RayCastHeightField( shape->heightField, &localInput );
break;
default:
B3_ASSERT( false );
return output;
}
if ( output.hit )
{
// convert to world coordinates
output.normal = b3RotateVector( transform.q, output.normal );
output.point = b3TransformPoint( transform, output.point );
}
// Lift the re-centered float result back to a world position
b3CastOutput local = b3RayCastShape( shape, transform, &input );
b3WorldCastOutput output;
output.normal = local.normal;
output.point = b3OffsetPos( origin, local.point );
output.fraction = local.fraction;
output.iterations = local.iterations;
output.triangleIndex = local.triangleIndex;
output.childIndex = local.childIndex;
output.materialIndex = local.materialIndex;
output.hit = local.hit;
return output;
}
@ -1280,7 +1266,7 @@ static void b3ResetProxy( b3World* world, b3Shape* shape, bool wakeBodies, bool
}
}
b3Transform transform = b3GetBodyTransformQuick( world, body );
b3WorldTransform transform = b3GetBodyTransformQuick( world, body );
if ( shape->proxyKey != B3_NULL_INDEX )
{
b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey );
@ -1739,19 +1725,20 @@ b3Vec3 b3Shape_GetClosestPoint( b3ShapeId shapeId, b3Vec3 target )
b3Shape* shape = b3GetShape( world, shapeId );
b3Body* body = b3Array_Get( world->bodies, shape->bodyId );
b3Transform transform = b3GetBodyTransformQuick( world, body );
// Low level closest point query is a documented float carve-out far from the origin
b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), b3Pos_zero );
b3DistanceInput input;
input.proxyA = b3MakeShapeProxy( shape );
input.proxyB = (b3ShapeProxy){ &target, 1, 0.0f };
input.transformA = transform;
input.transformB = b3Transform_identity;
input.transform = b3InvMulTransforms( transform, b3Transform_identity );
input.useRadii = true;
b3SimplexCache cache = { 0 };
b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 );
return output.pointA;
// Witness point comes back in frame A, lift it back to the query frame
return b3TransformPoint( transform, output.pointA );
}
#define B3_DEBUG_WIND 0
@ -1804,7 +1791,8 @@ void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift,
B3_ASSERT( body->setIndex == b3_awakeSet );
b3BodyState* state = b3GetBodyState( world, body );
b3Transform transform = sim->transform;
// Only the rotation is used below, so the demoted world transform is exact
b3Transform transform = b3ToRelativeTransform( sim->transform, b3Pos_zero );
float lengthUnits = b3GetLengthUnitsPerMeter();
float volumeUnits = lengthUnits * lengthUnits * lengthUnits;
@ -1935,8 +1923,8 @@ void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift,
if ( lineIndex < B3_DEBUG_LINE_CAPACITY )
{
b3DebugLine* line = world->taskContexts.data[0].lines + lineIndex;
line->p1 = b3Add( b3MulMV( matrix, localCenter ), transform.p );
line->p2 = b3Add( line->p1, deltaForce );
line->p1 = b3OffsetPos( sim->transform.p, b3MulMV( matrix, localCenter ) );
line->p2 = b3OffsetPos( line->p1, deltaForce );
line->label = i;
line->color = b3_colorBlanchedAlmond;
world->taskContexts.data[0].lineCount += 1;

View file

@ -57,7 +57,7 @@ typedef struct b3Shape
} b3Shape;
void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3Transform transform, bool forcePairCreation );
void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation );
void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp );
void b3DestroyShapeAllocations( b3World* world, b3Shape* shape );
@ -69,6 +69,10 @@ b3AABB b3ComputeSweptSphereAABB( const b3Sphere* shape, b3Transform xf1, b3Trans
b3AABB b3ComputeSweptCapsuleAABB( const b3Capsule* shape, b3Transform xf1, b3Transform xf2 );
b3AABB b3ComputeShapeAABB( const b3Shape* shape, b3Transform transform );
// Conservative world AABB for a shape inflated by extra margin. In double precision mode the
// box is built in the body local frame, translated by the double origin, and rounded outward.
b3AABB b3ComputeFatShapeAABB( const b3Shape* shape, b3WorldTransform transform, float extra );
b3AABB b3ComputeSweptShapeAABB( const b3Shape* shape, const b3Sweep* sweep, float time );
b3Vec3 b3GetShapeCentroid( const b3Shape* shape );
float b3GetShapeArea( const b3Shape* shape );

View file

@ -319,6 +319,8 @@ typedef struct b3ContinuousContext
b3Shape* fastShape;
b3Vec3 centroid1, centroid2;
b3Sweep sweep;
// World base for re-centering sweeps. Keeps TOI in float precision far from the origin.
b3Pos base;
float fraction;
b3SensorHit sensorHits[B2_MAX_CONTINUOUS_SENSOR_HITS];
float sensorFractions[B2_MAX_CONTINUOUS_SENSOR_HITS];
@ -406,7 +408,7 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con
}
// todo does having a sweep on shapeA help with bullets?
b3Sweep sweepA = b3MakeSweep( bodySim );
b3Sweep sweepA = b3MakeRelativeSweep( bodySim, continuousContext->base );
// Time of impact versus shape. Supports all shape types
b3TOIOutput output = b3ShapeTimeOfImpact( shape, fastShape, &sweepA, &continuousContext->sweep, continuousContext->fraction );
@ -436,7 +438,8 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con
{
b3ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->generation };
b3ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->generation };
didHit = world->preSolveFcn( shapeIdA, shapeIdB, output.point, output.normal, world->preSolveContext );
b3Pos point = b3OffsetPos( continuousContext->base, output.point );
didHit = world->preSolveFcn( shapeIdA, shapeIdB, point, output.normal, world->preSolveContext );
}
if ( didHit )
@ -462,7 +465,10 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex );
B3_ASSERT( fastBodySim->flags & b3_isFast );
b3Sweep sweep = b3MakeSweep( fastBodySim );
// Re-center the sweep on the fast body so the TOI and the swept query stay in float precision
b3Pos base = fastBodySim->center0;
b3Sweep sweep = b3MakeRelativeSweep( fastBodySim, base );
b3Transform xf1;
xf1.q = sweep.q1;
@ -480,6 +486,7 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
b3ContinuousContext context = { 0 };
context.world = world;
context.sweep = sweep;
context.base = base;
context.fastBodySim = fastBodySim;
context.fraction = 1.0f;
@ -496,7 +503,8 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
context.centroid2 = b3TransformPoint( xf2, fastShape->localCentroid );
b3AABB box1 = fastShape->aabb;
b3AABB box2 = b3ComputeShapeAABB( fastShape, xf2 );
// xf2 is relative to the base, so translate the box back to world space, rounding outward
b3AABB box2 = b3OffsetAABB( b3ComputeShapeAABB( fastShape, xf2 ), base );
// Store this to avoid double computation in the case there is no impact event
fastShape->aabb = box2;
@ -524,21 +532,22 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
}
const float speculativeScalar = B3_SPECULATIVE_DISTANCE;
const b3Vec3 speculativeMargin = { speculativeScalar, speculativeScalar, speculativeScalar };
if ( context.fraction < 1.0f )
{
// Handle time of impact event
// Handle time of impact event. The sweep is relative to the base, so re-add the base
// to return the advanced pose to world space.
b3Quat q = b3NLerp( sweep.q1, sweep.q2, context.fraction );
b3Vec3 c = b3Lerp( sweep.c1, sweep.c2, context.fraction );
b3Vec3 origin = b3Sub( c, b3RotateVector( q, sweep.localCenter ) );
// Advance body
b3Transform transform = { origin, q };
b3WorldTransform transform = { b3OffsetPos( base, origin ), q };
b3Pos center = b3OffsetPos( base, c );
fastBodySim->transform = transform;
fastBodySim->center = c;
fastBodySim->center = center;
fastBodySim->rotation0 = q;
fastBodySim->center0 = c;
fastBodySim->center0 = center;
// Prepare AABBs for broad-phase.
// Even though a body is fast, it may not move much. So the AABB may not need enlargement.
@ -549,9 +558,7 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
// Must recompute aabb at the interpolated transform
b3AABB aabb = b3ComputeShapeAABB( shape, transform );
aabb.lowerBound = b3Sub( aabb.lowerBound, speculativeMargin );
aabb.upperBound = b3Add( aabb.upperBound, speculativeMargin );
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar );
shape->aabb = aabb;
if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )
@ -592,12 +599,6 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
.upperBound = b3Add( shape->aabb.upperBound, aabbMargin ),
};
if ( b3IsSaneAABB( shape->fatAABB ) == false )
{
b3Vec3 c = fastBodySim->center;
b3Log( "body %s out of bounds at %g %g %g", fastBody->name, c.x, c.y, c.z );
}
shape->enlargedAABB = true;
fastBodySim->flags |= b3_enlargeBounds;
}
@ -650,7 +651,6 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
b3BitSet* awakeIslandBitSet = &taskContext->awakeIslandBitSet;
const float speculativeScalar = B3_SPECULATIVE_DISTANCE;
const b3Vec3 speculativeMargin = { speculativeScalar, speculativeScalar, speculativeScalar };
for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex )
{
@ -670,7 +670,7 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
B3_ASSERT( b3IsValidVec3( v ) );
B3_ASSERT( b3IsValidVec3( w ) );
sim->center = b3Add( sim->center, state->deltaPosition );
sim->center = b3OffsetPos( sim->center, state->deltaPosition );
sim->transform.q = b3NormalizeQuat( b3MulQuat( state->deltaRotation, sim->transform.q ) );
// Use the velocity of the farthest point on the body to account for rotation.
@ -691,7 +691,7 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
state->deltaPosition = b3Vec3_zero;
state->deltaRotation = b3Quat_identity;
sim->transform.p = b3Sub( sim->center, b3RotateVector( sim->transform.q, sim->localCenter ) );
sim->transform.p = b3OffsetPos( sim->center, b3Neg( b3RotateVector( sim->transform.q, sim->localCenter ) ) );
// cache miss here, however I need the shape list below
b3Body* body = bodies + sim->bodyId;
@ -777,7 +777,7 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
}
// Update shapes AABBs
b3Transform transform = sim->transform;
b3WorldTransform transform = sim->transform;
bool isFast = ( sim->flags & b3_isFast ) != 0;
int shapeId = body->headShapeId;
while ( shapeId != B3_NULL_INDEX )
@ -795,9 +795,7 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
}
else
{
b3AABB aabb = b3ComputeShapeAABB( shape, transform );
aabb.lowerBound = b3Sub( aabb.lowerBound, speculativeMargin );
aabb.upperBound = b3Add( aabb.upperBound, speculativeMargin );
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar );
shape->aabb = aabb;
B3_ASSERT( shape->enlargedAABB == false );
@ -1972,7 +1970,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId );
b3BodySim* simA = b3GetBodySim( world, bodyA );
b3BodySim* simB = b3GetBodySim( world, bodyB );
b3Vec3 midCenter = b3Lerp( simA->center, simB->center, 0.5f );
b3Pos midCenter = b3LerpPosition( simA->center, simB->center, 0.5f );
b3ContactHitEvent event = { 0 };
event.approachSpeed = threshold;
@ -1993,7 +1991,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
if ( approachSpeed > event.approachSpeed && mp->totalNormalImpulse > 0.0f )
{
event.approachSpeed = approachSpeed;
event.point = b3Add( midCenter, b3Lerp( mp->anchorA, mp->anchorB, 0.5f ) );
event.point = b3OffsetPos( midCenter, b3Lerp( mp->anchorA, mp->anchorB, 0.5f ) );
event.normal = manifold->normal;
triangleIndex = mp->triangleIndex;
found = true;
@ -2106,8 +2104,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
// A fast body may have been flagged as enlarged despite having no shapes enlarged.
if ( shape->enlargedAABB )
{
B3_VALIDATE( b3IsSaneAABB( shape->fatAABB ) );
b3BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB );
shape->enlargedAABB = false;
}

View file

@ -22,7 +22,9 @@ b3MassData b3ComputeSphereMass( const b3Sphere* shape, float density )
b3MassData out;
out.mass = mass;
out.center = center;
out.inertia = b3AddMM( b3MakeDiagonalMatrix( ixx, ixx, ixx ), b3Steiner( mass, center ) );
// Inertia about the center of mass
out.inertia = b3MakeDiagonalMatrix( ixx, ixx, ixx );
return out;
}
@ -47,8 +49,7 @@ bool b3OverlapSphere( const b3Sphere* shape, b3Transform shapeTransform, const b
b3DistanceInput input;
input.proxyA = ( b3ShapeProxy ){ &shape->center, 1, shape->radius };
input.proxyB = *proxy;
input.transformA = shapeTransform;
input.transformB = b3Transform_identity;
input.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity );
input.useRadii = true;
b3SimplexCache cache = { 0 };
@ -206,8 +207,7 @@ b3CastOutput b3ShapeCastSphere( const b3Sphere* sphere, const b3ShapeCastInput*
b3ShapeCastPairInput pairInput;
pairInput.proxyA = ( b3ShapeProxy ){ &sphere->center, 1, sphere->radius };
pairInput.proxyB = input->proxy;
pairInput.transformA = b3Transform_identity;
pairInput.transformB = b3Transform_identity;
pairInput.transform = b3Transform_identity;
pairInput.translationB = input->translation;
pairInput.maxFraction = input->maxFraction;
pairInput.canEncroach = input->canEncroach;

View file

@ -46,8 +46,8 @@ float b3SphericalJoint_GetConeAngle( b3JointId jointId )
{
b3World* world = b3GetWorld( jointId.world0 );
b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q );
b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q );
@ -109,8 +109,8 @@ float b3SphericalJoint_GetTwistAngle( b3JointId jointId )
{
b3World* world = b3GetWorld( jointId.world0 );
b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
b3Transform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3Transform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q );
b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q );
@ -231,8 +231,8 @@ b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base )
b3Vec3 b3GetSphericalJointTorque( b3World* world, b3JointSim* base )
{
b3Transform xfA = b3GetBodyTransform( world, base->bodyIdA );
b3Transform xfB = b3GetBodyTransform( world, base->bodyIdB );
b3WorldTransform xfA = b3GetBodyTransform( world, base->bodyIdA );
b3WorldTransform xfB = b3GetBodyTransform( world, base->bodyIdB );
b3Quat qA = b3MulQuat( xfA.q, base->localFrameA.q );
b3Quat qB = b3MulQuat( xfB.q, base->localFrameB.q );
@ -300,7 +300,7 @@ void b3PrepareSphericalJoint( b3JointSim* base, b3StepContext* context )
joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
joint->deltaCenter = b3Sub( bodySimB->center, bodySimA->center );
joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
// Cone axis is the z-axis of body A.
b3Vec3 coneAxis = b3RotateVector( joint->frameA.q, b3Vec3_axisZ );
@ -630,22 +630,22 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi
}
}
void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale )
void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
{
b3Transform frameA = b3MulTransforms( transformA, base->localFrameA );
b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
float length1 = 0.1f * scale;
draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ), b3_colorRed,
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed,
draw->context );
draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ), b3_colorGreen,
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen,
draw->context );
draw->DrawSegmentFcn( frameA.p, b3MulAdd( frameA.p, length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ), b3_colorBlue,
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue,
draw->context );
b3Transform frameB = b3MulTransforms( transformB, base->localFrameB );
b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
float length2 = 0.2f * scale;
draw->DrawSegmentFcn( frameB.p, b3MulAdd( frameB.p, length2, b3RotateVector( frameB.q, b3Vec3_axisZ ) ), b3_colorOrange,
draw->DrawSegmentFcn( frameB.p, b3OffsetPos( frameB.p, b3MulSV( length2, b3RotateVector( frameB.q, b3Vec3_axisZ ) ) ), b3_colorOrange,
draw->context );
b3SphericalJoint* joint = &base->sphericalJoint;
@ -678,20 +678,20 @@ void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform tran
if ( index == 0 )
{
draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, vertex1 ), b3_colorCyan, draw->context );
draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context );
}
if ( index == kSliceCount - 1 )
{
draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );
}
draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex1 ), b3TransformPoint( frameA, vertex2 ), b3_colorCyan,
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,
draw->context );
}
float twistAngle = b3GetTwistAngle( relQ );
b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f };
draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, p2 ), b3_colorYellow, draw->context );
draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, p2 ), b3_colorYellow, draw->context );
}
// Swing limit
@ -709,8 +709,8 @@ void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform tran
b3Vec3 vertex1 = { coneRadius * b3Cos( phi1 ), coneRadius * b3Sin( phi1 ), coneHeight };
b3Vec3 vertex2 = { coneRadius * b3Cos( phi2 ), coneRadius * b3Sin( phi2 ), coneHeight };
draw->DrawSegmentFcn( frameA.p, b3TransformPoint( frameA, vertex1 ), b3_colorCyan, draw->context );
draw->DrawSegmentFcn( b3TransformPoint( frameA, vertex1 ), b3TransformPoint( frameA, vertex2 ), b3_colorCyan,
draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context );
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,
draw->context );
}
}

View file

@ -346,8 +346,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const
b3DistanceInput distanceInput;
distanceInput.proxyA = (b3ShapeProxy){ triangleB, 3, 0.0f };
distanceInput.proxyB = (b3ShapeProxy){ &capsuleA->center1, 2, 0.0f };
distanceInput.transformA = b3Transform_identity;
distanceInput.transformB = b3Transform_identity;
distanceInput.transform = b3Transform_identity;
distanceInput.useRadii = false;
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 );
@ -1230,8 +1229,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3
.radius = 0.0f,
};
input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullA->vertexCount, .radius = 0.0f };
input.transformA = b3Transform_identity;
input.transformB = b3Transform_identity;
input.transform = b3Transform_identity;
input.useRadii = false;
b3SimplexCache simplexCache = { 0 };

Some files were not shown because too many files have changed in this diff Show more