dynamic mover

This commit is contained in:
Erin Catto 2026-10-20 23:33:18 -07:00
commit f0b1ed2fca
29 changed files with 3078 additions and 432 deletions

View file

@ -1358,6 +1358,77 @@ B3_API float b3MotorJoint_GetMaxSpringTorque( b3JointId jointId );
/**@}*/ // motor_joint
/**
* @defgroup mover_joint Mover Joint
* @brief Functions for the mover joint.
*
* The mover joint is designed for a dynamic character mover.
*
* @warning Experimental. The mover joint may change in future versions.
* @{
*/
/// Create a mover joint
/// @see b3MoverJointDef for details
B3_API b3JointId b3CreateMoverJoint( b3WorldId worldId, const b3MoverJointDef* def );
/// Set the desired relative linear velocity in meters per second
B3_API void b3MoverJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity );
/// Get the desired relative linear velocity in meters per second
B3_API b3Vec3 b3MoverJoint_GetLinearVelocity( b3JointId jointId );
/// Set the joint maximum force, usually in newtons
B3_API void b3MoverJoint_SetMaxVelocityForce( b3JointId jointId, b3Vec3 maxForce );
/// Get the joint maximum force, usually in newtons
B3_API b3Vec3 b3MoverJoint_GetMaxVelocityForce( b3JointId jointId );
/**@}*/ // mover_joint
/**
* @defgroup pogo_joint Pogo Joint
* @brief Functions for the pogo joint. Used for dynamic character movers.
*
* @warning Experimental. The pogo joint may change in future versions.
* @{
*/
/// Create a pogo joint
/// @see b3PogoJointDef for details
B3_API b3JointId b3CreatePogoJoint( b3WorldId worldId, const b3PogoJointDef* def );
/// Set the rest length of a pogo joint
/// @param jointId The id for a pogo joint
/// @param length The new pogo joint length
B3_API void b3PogoJoint_SetRestLength( b3JointId jointId, float length );
/// Get the rest length of a pogo joint
B3_API float b3PogoJoint_GetRestLength( b3JointId jointId );
/// Set the spring stiffness in Hertz
B3_API void b3PogoJoint_SetSpringHertz( b3JointId jointId, float hertz );
/// Get the spring Hertz
B3_API float b3PogoJoint_GetSpringHertz( b3JointId jointId );
/// Set the spring damping ratio, non-dimensional
B3_API void b3PogoJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio );
/// Get the spring damping ratio
B3_API float b3PogoJoint_GetSpringDampingRatio( b3JointId jointId );
/// Get the current length of the pogo.
B3_API float b3PogoJoint_GetCurrentLength( b3JointId jointId );
/// Get the internal pogo velocity.
B3_API float b3PogoJoint_GetCurrentSpeed( b3JointId jointId );
/// Get the internal pogo impulse.
B3_API float b3PogoJoint_GetImpulse( b3JointId jointId );
/**@}*/ // pogo_joint
/**
* @defgroup filter_joint Filter Joint
* @brief Functions for the filter joint.

View file

@ -1053,6 +1053,20 @@ B3_INLINE b3Vec3 b3ClosestPointToAABB( b3Vec3 point, b3AABB a )
return b3Clamp( point, a.lowerBound, a.upperBound );
}
/// One-dimensional mass-spring-damper simulation. Returns the new velocity given the position and
/// time step. You can then compute the new position using:
/// position += timeStep * newVelocity
/// This drives towards a zero position. By using implicit integration we get a stable solution that
/// doesn't require transcendental functions.
B3_INLINE float b3SpringDamper( float hertz, float dampingRatio, float position, float velocity,
float timeStep )
{
float omega = 2.0f * B3_PI * hertz;
float omegaH = omega * timeStep;
return ( velocity - omega * omegaH * position ) /
( 1.0f + 2.0f * dampingRatio * omegaH + omegaH * omegaH );
}
/// The closest points between to segments or infinite lines.
typedef struct b3SegmentDistanceResult
{

View file

@ -628,6 +628,8 @@ typedef enum b3JointType
b3_sphericalJoint,
b3_weldJoint,
b3_wheelJoint,
b3_moverJoint,
b3_pogoJoint,
} b3JointType;
/// Base joint definition used by all joint types. The local frames are measured from the
@ -767,6 +769,63 @@ typedef struct b3MotorJointDef
/// @ingroup motor_joint
B3_API b3MotorJointDef b3DefaultMotorJointDef( void );
/// A mover joint is used to move a dynamic character mover through velocity commands. The x, y, and
/// z directions are handled separately. Does not affect rotation.
/// @ingroup mover_joint
typedef struct b3MoverJointDef
{
/// Base joint definition
b3JointDef base;
/// The desired linear velocity
b3Vec3 linearVelocity;
/// The maximum motor force in newtons.
b3Vec3 maxVelocityForce;
} b3MoverJointDef;
/// Use this to initialize your joint definition
/// @ingroup mover_joint
B3_API b3MoverJointDef b3DefaultMoverJointDef( void );
/// Pogo joint definition
/// @ingroup pogo_joint
typedef struct b3PogoJointDef
{
/// Base joint definition
b3JointDef base;
/// Normal vector at the contact
b3Vec3 normal;
/// The spring stiffness Hertz, cycles per second
float hertz;
/// The spring damping ratio, non-dimensional
float dampingRatio;
/// Spring length.
float restLength;
/// Maximum tension force. Controls how sticky the mover is to ground.
float maxTensionForce;
/// Maximum compression force. How hard the pogo pushes the mover up from the ground.
float maxCompressionForce;
/// The initial pogo internal impulse. The pogo is typically recreated every frame. This gives a
/// way to move the internal state forward for smoother spring behavior.
float impulse;
/// The initial pogo internal velocity. The pogo is typically recreated every frame. This gives
/// a way to move the internal state forward for smoother spring behavior.
float velocity;
} b3PogoJointDef;
/// Use this to initialize your joint definition
/// @ingroup pogo_joint
B3_API b3PogoJointDef b3DefaultPogoJointDef( void );
/// A filter joint is used to disable collision between two specific bodies.
/// @ingroup filter_joint
typedef struct b3FilterJointDef

View file

@ -169,8 +169,10 @@ set(SAMPLE_FILES
sample.h
mesh_loader.cpp
mesh_loader.h
mover.cpp
mover.h
dynamic_mover.cpp
dynamic_mover.h
geometric_mover.cpp
geometric_mover.h
jsmn.h
tiny_obj_loader.h
earcut.h

505
samples/dynamic_mover.cpp Normal file
View file

@ -0,0 +1,505 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#include "dynamic_mover.h"
#include "gfx/draw.h"
#include "box3d/box3d.h"
#include "box3d/math_functions.h"
#include <float.h>
struct CastContext
{
DynamicMoverCastResult result;
b3BodyId moverId;
};
static float CastCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction,
uint64_t userMaterialId, int triangleIndex, int childIndex,
void* context )
{
(void)triangleIndex;
(void)childIndex;
CastContext* castContext = (CastContext*)context;
b3BodyId bodyId = b3Shape_GetBody( shapeId );
// The probes start inside or on the capsule. Sensors can't hold up the mover.
if ( B3_ID_EQUALS( bodyId, castContext->moverId ) || b3Shape_IsSensor( shapeId ) )
{
return -1.0f;
}
DynamicMoverCastResult* result = &castContext->result;
result->point = point;
result->normal = normal;
result->bodyId = bodyId;
result->userMaterialId = userMaterialId;
result->fraction = fraction;
result->hit = true;
return fraction;
}
DynamicMover::DynamicMover()
{
DynamicMoverDef def;
m_jumpSpeed = def.jumpSpeed;
m_maxSpeed = def.maxSpeed;
m_minSpeed = def.minSpeed;
m_stopSpeed = def.stopSpeed;
m_accelerate = def.accelerate;
m_airSteer = def.airSteer;
m_friction = def.friction;
m_gravityScale = def.gravityScale;
m_maxGroundForce = def.maxGroundForce;
m_maxAirForce = def.maxAirForce;
m_pogoRestLength = def.pogoRestLength;
m_pogoHertz = def.pogoHertz;
m_pogoDampingRatio = def.pogoDampingRatio;
m_pogoCompressionScale = def.pogoCompressionScale;
m_pogoTensionScale = def.pogoTensionScale;
m_enableSkirtPogo = def.enableSkirtPogo;
m_platformMaterialId = def.platformMaterialId;
m_minGroundNormalY = 0.7f;
m_capsule = def.capsule;
m_filter = def.filter;
m_worldId = b3_nullWorldId;
m_groundId = b3_nullBodyId;
m_moverId = b3_nullBodyId;
m_moverJointId = b3_nullJointId;
m_pogoJointId = b3_nullJointId;
m_castResult = {};
m_velocity = b3Vec3_zero;
m_onGround = false;
m_walkable = false;
m_jumping = false;
m_jumpTicks = 0;
m_pogoImpulse = 0.0f;
m_pogoVelocity = 0.0f;
m_pogoLength = 0.0f;
for ( DynamicMoverProbe& probe : m_probes )
{
probe = { b3Pos_zero, b3Vec3_zero, 1.0f, false };
}
m_probeCount = 0;
m_selectedProbe = 0;
}
void DynamicMover::Create( b3WorldId worldId, const DynamicMoverDef* def )
{
m_capsule = def->capsule;
m_filter = def->filter;
m_jumpSpeed = def->jumpSpeed;
m_maxSpeed = def->maxSpeed;
m_minSpeed = def->minSpeed;
m_stopSpeed = def->stopSpeed;
m_accelerate = def->accelerate;
m_airSteer = def->airSteer;
m_friction = def->friction;
m_gravityScale = def->gravityScale;
m_maxGroundForce = def->maxGroundForce;
m_maxAirForce = def->maxAirForce;
m_pogoRestLength = def->pogoRestLength;
m_pogoHertz = def->pogoHertz;
m_pogoDampingRatio = def->pogoDampingRatio;
m_pogoCompressionScale = def->pogoCompressionScale;
m_pogoTensionScale = def->pogoTensionScale;
m_enableSkirtPogo = def->enableSkirtPogo;
m_platformMaterialId = def->platformMaterialId;
m_worldId = worldId;
m_velocity = b3Vec3_zero;
m_onGround = false;
m_walkable = false;
m_jumping = false;
m_jumpTicks = 0;
m_pogoImpulse = 0.0f;
m_pogoVelocity = 0.0f;
m_pogoLength = 0.0f;
m_pogoJointId = b3_nullJointId;
b3BodyDef bodyDef = b3DefaultBodyDef();
m_groundId = b3CreateBody( worldId, &bodyDef );
// Position is the center of the capsule.
bodyDef.type = b3_dynamicBody;
bodyDef.position = def->position;
bodyDef.gravityScale = m_gravityScale;
bodyDef.motionLocks.angularX = true;
bodyDef.motionLocks.angularY = true;
bodyDef.motionLocks.angularZ = true;
bodyDef.enableSleep = false;
bodyDef.name = "mover";
m_moverId = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.density = def->density;
// The mover joint and the pogo joint handle all the movement, surface friction would fight them
shapeDef.baseMaterial.friction = 0.0f;
shapeDef.baseMaterial.restitution = 0.0f;
shapeDef.filter = def->filter;
shapeDef.enablePreSolveEvents = def->enablePreSolveEvents;
shapeDef.enableSensorEvents = def->enableSensorEvents;
b3CreateCapsuleShape( m_moverId, &shapeDef, &m_capsule );
CreateMoverJoint( m_groundId, { m_maxGroundForce, 0.0f, m_maxGroundForce } );
}
void DynamicMover::CreateMoverJoint( b3BodyId baseId, b3Vec3 maxForce )
{
b3MoverJointDef moverDef = b3DefaultMoverJointDef();
moverDef.linearVelocity = m_velocity;
moverDef.maxVelocityForce = maxForce;
moverDef.base.bodyIdA = baseId;
moverDef.base.bodyIdB = m_moverId;
moverDef.base.collideConnected = true;
m_moverJointId = b3CreateMoverJoint( m_worldId, &moverDef );
}
void DynamicMover::Destroy()
{
if ( B3_IS_NON_NULL( m_moverId ) )
{
b3DestroyBody( m_moverId );
}
if ( B3_IS_NON_NULL( m_groundId ) )
{
b3DestroyBody( m_groundId );
}
m_groundId = b3_nullBodyId;
m_moverId = b3_nullBodyId;
// Joints are implicitly destroyed.
m_moverJointId = b3_nullJointId;
m_pogoJointId = b3_nullJointId;
}
void DynamicMover::SetGravityScale( float gravityScale )
{
m_gravityScale = gravityScale;
if ( B3_IS_NON_NULL( m_moverId ) )
{
b3Body_SetGravityScale( m_moverId, gravityScale );
}
}
bool DynamicMover::Jump()
{
if ( m_onGround == false || m_walkable == false )
{
return false;
}
float surfaceVelocity = 0.0f;
if ( b3Body_IsValid( m_castResult.bodyId ) )
{
b3Vec3 v = b3Body_GetWorldPointVelocity( m_castResult.bodyId, m_castResult.point );
surfaceVelocity = v.y;
}
float mass = b3Body_GetMass( m_moverId );
float vy = b3Body_GetLinearVelocity( m_moverId ).y;
// Remove the pogo constraint velocity but add the surface velocity.
float dv = b3MaxFloat( 0.0f, m_jumpSpeed - vy ) + surfaceVelocity;
b3Body_ApplyLinearImpulseToCenter( m_moverId, { 0.0f, mass * dv, 0.0f }, true );
Launch();
return true;
}
void DynamicMover::Launch()
{
// This removes the pogo step down on the next update.
m_onGround = false;
m_walkable = false;
m_jumping = true;
m_jumpTicks = 0;
}
// Movement follows the Quake ground and air model:
// https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390
// onGround - controls friction, mover max force, and pogo step-down extension
// walkable - controls air steer
// jumping - blocks the pogo from pulling down
void DynamicMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle,
float speedScale )
{
// Reach further while grounded so the spring can find the ground over a step
float stepDownLength = m_pogoRestLength;
float rayLength = m_onGround ? m_pogoRestLength + stepDownLength : m_pogoRestLength;
b3Vec3 translation = { 0.0f, -rayLength, 0.0f };
b3Pos position = b3Body_GetPosition( m_moverId );
b3Pos origin = position + m_capsule.center1;
b3QueryFilter queryFilter = {
.categoryBits = m_filter.categoryBits,
.maskBits = m_filter.maskBits,
.id = 0,
.name = "dynamic_mover_pogo",
};
CastContext castContext = { {}, m_moverId };
b3World_CastRay( m_worldId, origin, translation, queryFilter, CastCallback, &castContext );
m_castResult = castContext.result;
m_probes[0] = { origin, translation, m_castResult.hit ? m_castResult.fraction : 1.0f,
m_castResult.hit };
m_probeCount = 1;
m_selectedProbe = 0;
b3Vec3 pogoOffset = b3Vec3_zero;
bool centerWalkable = m_castResult.hit && m_castResult.normal.y >= m_minGroundNormalY;
if ( centerWalkable == false && m_enableSkirtPogo )
{
// The skirt turns with the direction of travel so ties go to the leading probe
b3Vec3 moverVelocity = b3Body_GetLinearVelocity( m_moverId );
b3Vec3 heading = { moverVelocity.x, 0.0f, moverVelocity.z };
if ( b3LengthSquared( heading ) < FLT_EPSILON )
{
heading = b3Vec3_axisX;
}
heading = b3Normalize( heading );
b3Vec3 side = { -heading.z, 0.0f, heading.x };
float bestDisplacement = FLT_MAX;
for ( int i = 0; i < m_skirtCount; ++i )
{
b3CosSin cs = b3ComputeCosSin( 2.0f * B3_PI * i / m_skirtCount );
b3Vec3 offset = m_capsule.radius * ( cs.cosine * heading + cs.sine * side );
b3Pos skirtOrigin = origin + offset;
CastContext skirtContext = { {}, m_moverId };
b3World_CastRay( m_worldId, skirtOrigin, translation, queryFilter, CastCallback,
&skirtContext );
const DynamicMoverCastResult& result = skirtContext.result;
m_probes[m_probeCount] = { skirtOrigin, translation,
result.hit ? result.fraction : 1.0f, result.hit };
// Use the walkable result that causes the least movement. With none the mover keeps
// the central hit and slides off the steep surface.
if ( result.hit && result.normal.y >= m_minGroundNormalY )
{
float displacement = b3AbsFloat( result.fraction * rayLength - m_pogoRestLength );
if ( displacement < bestDisplacement )
{
bestDisplacement = displacement;
m_castResult = result;
m_selectedProbe = m_probeCount;
pogoOffset = offset;
}
}
m_probeCount += 1;
}
}
// Should jumping end?
if ( m_jumping )
{
m_jumpTicks += 1;
// Jump ticks allow time for the jump to leave the ground. Otherwise jumping ends
// when the character approaches the current ground.
if ( m_jumpTicks > 2 && m_castResult.hit &&
b3Dot( m_velocity, m_castResult.normal ) <= 0.0f )
{
m_jumping = false;
}
}
if ( m_castResult.hit )
{
// The cast can still hit while jumping.
m_onGround = m_jumping == false;
// Is the ground too steep to walk?
m_walkable = m_castResult.normal.y >= m_minGroundNormalY;
}
else
{
m_onGround = false;
m_walkable = false;
}
// Connect the mover joint to platform. Acts like friction.
b3BodyId baseId = m_groundId;
if ( m_castResult.hit && m_platformMaterialId != 0 &&
m_castResult.userMaterialId == m_platformMaterialId &&
b3Body_GetType( m_castResult.bodyId ) == b3_kinematicBody )
{
baseId = m_castResult.bodyId;
}
if ( B3_ID_EQUALS( b3Joint_GetBodyA( m_moverJointId ), baseId ) == false )
{
b3Vec3 maxForce = b3MoverJoint_GetMaxVelocityForce( m_moverJointId );
b3DestroyJoint( m_moverJointId, false );
CreateMoverJoint( baseId, maxForce );
}
m_velocity = b3Body_GetLinearVelocity( m_moverId ) - b3Body_GetLinearVelocity( baseId );
// Friction acts in the horizontal plane. The pogo owns the vertical motion.
if ( m_onGround )
{
float speed = sqrtf( m_velocity.x * m_velocity.x + m_velocity.z * m_velocity.z );
if ( speed < m_minSpeed )
{
m_velocity.x = 0.0f;
m_velocity.z = 0.0f;
}
else
{
// Linear damping above stopSpeed and fixed reduction below stopSpeed
float control = speed < m_stopSpeed ? m_stopSpeed : speed;
// friction has units of 1/time
float drop = control * m_friction * timeStep;
float newSpeed = b3MaxFloat( 0.0f, speed - drop );
float ratio = newSpeed / speed;
m_velocity.x *= ratio;
m_velocity.z *= ratio;
}
}
// Mover force
float maxForce;
if ( m_onGround )
{
maxForce = m_walkable ? m_maxGroundForce : 0.0f;
}
else
{
maxForce = m_maxAirForce;
}
b3MoverJoint_SetMaxVelocityForce( m_moverJointId, { maxForce, 0.0f, maxForce } );
float maxSpeed = speedScale * m_maxSpeed;
b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right;
desiredVelocity.y = 0.0f;
float desiredSpeed;
b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity );
if ( desiredSpeed > maxSpeed )
{
desiredSpeed = maxSpeed;
}
// Accelerate
float currentSpeed = b3Dot( m_velocity, desiredDirection );
float addSpeed = desiredSpeed - currentSpeed;
if ( addSpeed > 0.0f )
{
float steer = m_walkable ? 1.0f : m_airSteer;
float accelSpeed = steer * m_accelerate * maxSpeed * timeStep;
if ( accelSpeed > addSpeed )
{
accelSpeed = addSpeed;
}
m_velocity.x += accelSpeed * desiredDirection.x;
m_velocity.z += accelSpeed * desiredDirection.z;
}
// The pogo joint is rebuilt every update because it may land on a different body
if ( b3Joint_IsValid( m_pogoJointId ) )
{
m_pogoLength = b3PogoJoint_GetCurrentLength( m_pogoJointId );
m_pogoImpulse = b3PogoJoint_GetImpulse( m_pogoJointId );
m_pogoVelocity = b3PogoJoint_GetCurrentSpeed( m_pogoJointId );
b3DestroyJoint( m_pogoJointId, false );
m_pogoJointId = b3_nullJointId;
}
if ( m_castResult.hit == true )
{
float moverMass = b3Body_GetMass( m_moverId );
float moverWeight =
m_gravityScale * b3Length( b3World_GetGravity( m_worldId ) ) * moverMass;
b3PogoJointDef pogoDef = b3DefaultPogoJointDef();
pogoDef.base.localFrameA.p =
b3Body_GetLocalPoint( m_castResult.bodyId, m_castResult.point );
pogoDef.base.localFrameB.p = m_capsule.center1 + pogoOffset;
pogoDef.normal = m_castResult.normal;
pogoDef.base.bodyIdA = m_castResult.bodyId;
pogoDef.base.bodyIdB = m_moverId;
pogoDef.base.collideConnected = true;
pogoDef.restLength = m_pogoRestLength;
pogoDef.hertz = m_pogoHertz;
pogoDef.dampingRatio = m_pogoDampingRatio;
pogoDef.maxCompressionForce = m_pogoCompressionScale * moverWeight;
// Warm start from the joint that was just destroyed
pogoDef.impulse = m_pogoImpulse;
pogoDef.velocity = m_pogoVelocity;
if ( m_jumping )
{
// Don't allow the pogo to pull down
pogoDef.maxTensionForce = 0.0f;
}
else
{
// The pogo can pull down at a multiple of the gravity force.
pogoDef.maxTensionForce = m_pogoTensionScale * moverWeight;
}
m_pogoJointId = b3CreatePogoJoint( m_worldId, &pogoDef );
}
else
{
m_pogoImpulse = 0.0f;
m_pogoVelocity = 0.0f;
}
b3MoverJoint_SetLinearVelocity( m_moverJointId, m_velocity );
}
void DynamicMover::Draw() const
{
for ( int i = 0; i < m_probeCount; ++i )
{
const DynamicMoverProbe& probe = m_probes[i];
b3Pos end = b3OffsetPos( probe.origin, probe.fraction * probe.translation );
if ( probe.hit == false )
{
DrawLine( probe.origin, end, MakeColor( b3_colorGray ) );
continue;
}
b3HexColor color = i == m_selectedProbe ? b3_colorGreen : b3_colorYellow;
DrawLine( probe.origin, end, MakeColor( color ) );
DrawPoint( end, 6.0f, MakeColor( color ) );
}
b3Pos position = b3Body_GetPosition( m_moverId );
b3HexColor color = m_onGround ? b3_colorOrange : b3_colorAquamarine;
DrawLine( position, position + m_velocity, MakeColor( color ) );
}

161
samples/dynamic_mover.h Normal file
View file

@ -0,0 +1,161 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#pragma once
#include "box3d/collision.h"
#include "box3d/types.h"
struct DynamicMoverDef
{
b3Pos position = {};
b3Capsule capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f };
// About 80 kilograms for the default capsule
float density = 200.0f;
b3Filter filter = b3DefaultFilter();
float jumpSpeed = 7.0f;
float maxSpeed = 6.0f;
float minSpeed = 0.1f;
float stopSpeed = 3.0f;
float accelerate = 20.0f;
float airSteer = 0.5f;
float friction = 8.0f;
float gravityScale = 1.5f;
float maxGroundForce = 6000.0f;
float maxAirForce = 1500.0f;
float pogoRestLength = 0.9f;
float pogoHertz = 3.0f;
float pogoDampingRatio = 0.8f;
float pogoCompressionScale = 100.0f;
float pogoTensionScale = 100.0f;
uint64_t platformMaterialId = 0;
bool enableSkirtPogo = true;
bool enablePreSolveEvents = false;
bool enableSensorEvents = false;
};
struct DynamicMoverCastResult
{
b3Pos point;
b3Vec3 normal;
b3BodyId bodyId;
uint64_t userMaterialId;
float fraction;
bool hit;
};
struct DynamicMoverProbe
{
b3Pos origin;
b3Vec3 translation;
float fraction;
bool hit;
};
// Dynamic character mover. The capsule is a rigid body with locked rotation, so it
// collides and is pushed like everything else in the world. A mover joint drives it to
// the target velocity and a pogo joint holds it above the ground so it can climb
// steps without touching the capsule.
//
// The dynamic mover should be vendored into your project so you can alter it to meet
// your game's specific needs.
//
// This system is inspired by Tom Waterson's presentation:
// https://www.youtube.com/watch?v=_jRLlTDqoGI
class DynamicMover
{
public:
DynamicMover();
void Create( b3WorldId worldId, const DynamicMoverDef* def );
void Destroy();
// Advance the mover. Throttle is the input along the forward and right directions on [-1, 1].
void Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, float speedScale );
// Jump if grounded. Returns false when airborne.
bool Jump();
// Release the ground so an external impulse can carry the mover into the air. Otherwise the
// pogo pulls it back down.
void Launch();
// Gravity scale is used to size the pogo forces, so change it here.
void SetGravityScale( float gravityScale );
void Draw() const;
static constexpr int m_skirtCount = 8;
static constexpr int m_probeCapacity = m_skirtCount + 1;
float m_jumpSpeed;
float m_maxSpeed;
float m_minSpeed;
float m_stopSpeed;
float m_accelerate;
// Fraction of the ground acceleration available in the air.
float m_airSteer;
// Ground friction in units of 1/time.
float m_friction;
// A character usually wants to fall faster than the rest of the world.
float m_gravityScale;
// Force budget the mover joint has for reaching the target velocity. The air value
// is small so the character keeps its momentum while jumping.
float m_maxGroundForce;
float m_maxAirForce;
float m_pogoRestLength;
float m_pogoHertz;
float m_pogoDampingRatio;
// Pogo force limits as a multiple of the mover weight.
float m_pogoCompressionScale;
float m_pogoTensionScale;
// Casts an octagon skirt of rays around the capsule when the central ray misses or
// lands on a surface too steep to walk, so the mover can stand on a ledge or a thin
// rail, or step off the foot of a steep slope.
bool m_enableSkirtPogo;
// Used for the pogo to detect moving platforms.
uint64_t m_platformMaterialId;
// Surfaces steeper than this are not ground.
float m_minGroundNormalY;
b3Capsule m_capsule;
b3Filter m_filter;
b3WorldId m_worldId;
b3BodyId m_groundId;
b3BodyId m_moverId;
b3JointId m_moverJointId;
b3JointId m_pogoJointId;
DynamicMoverCastResult m_castResult;
b3Vec3 m_velocity;
bool m_onGround;
bool m_walkable;
bool m_jumping;
int m_jumpTicks;
// Pogo joint state, carried across the joint being recreated each update.
float m_pogoImpulse;
float m_pogoVelocity;
float m_pogoLength;
// Pogo ray cast results for drawing.
DynamicMoverProbe m_probes[m_probeCapacity];
int m_probeCount;
int m_selectedProbe;
private:
void CreateMoverJoint( b3BodyId baseId, b3Vec3 maxForce );
};

351
samples/geometric_mover.cpp Normal file
View file

@ -0,0 +1,351 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#include "geometric_mover.h"
#include "gfx/draw.h"
#include "box3d/box3d.h"
#include "box3d/math_functions.h"
#include <assert.h>
#include <float.h>
static bool MoverFilterCallback( b3ShapeId shapeId, void* context )
{
GeometricMover* self = (GeometricMover*)context;
for ( int i = 0; i < self->m_ignoreCount; ++i )
{
if ( B3_ID_EQUALS( shapeId, self->m_ignoreShapeIds[i] ) )
{
return false;
}
}
return true;
}
static bool PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* planeResults, int planeCount,
void* context )
{
if ( MoverFilterCallback( shapeId, context ) == false )
{
// ignore these planes but continue looking for more
return true;
}
GeometricMover* self = static_cast<GeometricMover*>( context );
float maxPush = FLT_MAX;
bool clipVelocity = true;
MoverShapeUserData* userData =
static_cast<MoverShapeUserData*>( b3Shape_GetUserData( shapeId ) );
if ( userData != nullptr )
{
maxPush = userData->maxPush;
clipVelocity = userData->clipVelocity;
}
for ( int i = 0; i < planeCount && self->m_planeCount < GeometricMover::m_planeCapacity; ++i )
{
assert( b3IsValidPlane( planeResults[i].plane ) );
self->m_planes[self->m_planeCount] = {
.plane = planeResults[i].plane,
.pushLimit = maxPush,
.push = 0.0f,
.clipVelocity = clipVelocity,
};
// The plane point is relative to the origin passed to the collide call
self->m_planeExtras[self->m_planeCount] = {
.point = b3OffsetPos( self->m_position, planeResults[i].point ),
.shapeId = shapeId,
};
self->m_planeCount += 1;
}
return true;
}
GeometricMover::GeometricMover()
{
GeometricMoverDef def;
m_jumpSpeed = def.jumpSpeed;
m_maxSpeed = def.maxSpeed;
m_minSpeed = def.minSpeed;
m_stopSpeed = def.stopSpeed;
m_accelerate = def.accelerate;
m_friction = def.friction;
m_gravity = def.gravity;
m_pogoRestLength = def.pogoRestLength;
m_pogoHertz = def.pogoHertz;
m_pogoDampingRatio = def.pogoDampingRatio;
m_clipVelocity = true;
m_ignoreShapeIds = nullptr;
m_ignoreCount = 0;
m_capsule = def.capsule;
m_worldId = b3_nullWorldId;
m_position = def.position;
m_velocity = b3Vec3_zero;
m_pogoVelocity = 0.0f;
m_onGround = false;
m_planeCount = 0;
m_totalIterations = 0;
m_pogoOrigin = b3Pos_zero;
m_pogoTranslation = b3Vec3_zero;
m_pogoPoint = b3Pos_zero;
m_pogoHit = false;
}
void GeometricMover::Create( b3WorldId worldId, const GeometricMoverDef* def )
{
m_capsule = def->capsule;
m_jumpSpeed = def->jumpSpeed;
m_maxSpeed = def->maxSpeed;
m_minSpeed = def->minSpeed;
m_stopSpeed = def->stopSpeed;
m_accelerate = def->accelerate;
m_friction = def->friction;
m_gravity = def->gravity;
m_pogoRestLength = def->pogoRestLength;
m_pogoHertz = def->pogoHertz;
m_pogoDampingRatio = def->pogoDampingRatio;
m_worldId = worldId;
m_position = def->position;
m_velocity = b3Vec3_zero;
m_pogoVelocity = 0.0f;
m_onGround = false;
m_planeCount = 0;
m_totalIterations = 0;
m_pogoHit = false;
}
bool GeometricMover::Jump()
{
if ( m_onGround == false )
{
return false;
}
m_velocity.y = m_jumpSpeed;
m_onGround = false;
return true;
}
// Solve a normal constraint between the mover and each touched rigid body. This is the
// same velocity constraint the solver uses for a contact, except the mover is treated as
// having infinite mass, so all of the impulse lands on the body.
void GeometricMover::PushBodies()
{
for ( int i = 0; i < m_planeCount; ++i )
{
b3BodyId bodyId = b3Shape_GetBody( m_planeExtras[i].shapeId );
if ( b3Body_GetType( bodyId ) != b3_dynamicBody )
{
continue;
}
MoverShapeUserData* userData =
static_cast<MoverShapeUserData*>( b3Shape_GetUserData( m_planeExtras[i].shapeId ) );
if ( userData != nullptr && userData->canMoverPush == false )
{
continue;
}
// Plane normals point at the mover
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 );
b3Pos centerB = b3Body_GetWorldCenter( bodyId );
b3Vec3 rB = b3SubPos( point, centerB );
b3Vec3 rnB = b3Cross( rB, normal );
float kNormal = invMassA + invMassB + b3Dot( rnB, b3MulMV( invIB, rnB ) );
float normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
b3Vec3 vB = b3Body_GetLinearVelocity( bodyId );
b3Vec3 omegaB = b3Body_GetAngularVelocity( bodyId );
b3Vec3 vrB = b3Add( vB, b3Cross( omegaB, rB ) );
float vn = b3Dot( b3Sub( vrB, m_velocity ), normal );
// Push only, a separating body is left alone
float impulse = b3MaxFloat( -normalMass * vn, 0.0f );
b3Vec3 P = b3MulSV( impulse, normal );
m_velocity = b3MulSub( m_velocity, invMassA, P );
b3Body_ApplyLinearImpulse( bodyId, P, point, true );
}
}
// Movement follows the Quake ground and air model:
// https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390
void GeometricMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle,
float speedScale )
{
// Friction
float speed = b3Length( m_velocity );
if ( speed < m_minSpeed )
{
m_velocity.x = 0.0f;
m_velocity.z = 0.0f;
}
else
{
// Linear damping above stopSpeed and fixed reduction below stopSpeed
float control = speed < m_stopSpeed ? m_stopSpeed : speed;
// friction has units of 1/time
float drop = control * m_friction * timeStep;
float newSpeed = b3MaxFloat( 0.0f, speed - drop );
float ratio = newSpeed / speed;
m_velocity.x *= ratio;
m_velocity.z *= ratio;
}
float maxSpeed = speedScale * m_maxSpeed;
b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right;
float desiredSpeed;
b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity );
if ( desiredSpeed > maxSpeed )
{
desiredSpeed = maxSpeed;
}
if ( m_onGround )
{
m_velocity.y = 0.0f;
}
// Accelerate
float currentSpeed = b3Dot( m_velocity, desiredDirection );
float addSpeed = desiredSpeed - currentSpeed;
if ( addSpeed > 0.0f )
{
float accelSpeed = m_accelerate * maxSpeed * timeStep;
if ( accelSpeed > addSpeed )
{
accelSpeed = addSpeed;
}
m_velocity += accelSpeed * desiredDirection;
}
m_velocity.y -= m_gravity * timeStep;
// The probe reaches past the rest length so the spring can be stretched
float rayLength = m_pogoRestLength + m_capsule.radius;
b3Pos rayOrigin = m_position + m_capsule.center1;
b3Vec3 rayTranslation = -rayLength * b3Vec3_axisY;
b3QueryFilter pogoFilter = { 1, ~2u, 0, "pogo" };
b3RayResult rayResult =
b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, pogoFilter );
m_pogoOrigin = rayOrigin;
m_pogoTranslation = rayTranslation;
m_pogoPoint = rayResult.point;
// After gravity was applied, disable pogo when still moving up.
// Avoids getting pulled back to the ground when jumping.
bool suppressPogo = m_velocity.y > 0.0f;
if ( rayResult.hit == false || suppressPogo )
{
m_onGround = false;
m_pogoVelocity = 0.0f;
}
else
{
m_onGround = true;
float pogoCurrentLength = rayResult.fraction * rayLength;
float offset = pogoCurrentLength - m_pogoRestLength;
m_pogoVelocity =
b3SpringDamper( m_pogoHertz, m_pogoDampingRatio, offset, m_pogoVelocity, timeStep );
}
m_pogoHit = m_onGround;
b3Pos startPosition = m_position;
b3Pos target = m_position + timeStep * m_velocity + timeStep * m_pogoVelocity * b3Vec3_axisY;
// Want the mover to collide with allies
b3QueryFilter moverFilter = {
.categoryBits = 1, .maskBits = ~0u, .id = 1, .name = "mover_collide" };
// The cast should ignore allies
b3QueryFilter castFilter = {
.categoryBits = 1, .maskBits = ~2u, .id = 1, .name = "mover_cast" };
m_totalIterations = 0;
float tolerance = 0.01f;
for ( int iteration = 0; iteration < 5; ++iteration )
{
m_planeCount = 0;
b3World_CollideMover( m_worldId, m_position, &m_capsule, moverFilter, PlaneResultFcn,
this );
b3Vec3 targetDelta = target - m_position;
b3PlaneSolverResult result = b3SolvePlanes( targetDelta, m_planes, m_planeCount );
m_totalIterations += result.iterationCount;
float fraction = b3World_CastMover( m_worldId, m_position, &m_capsule, result.delta,
castFilter, MoverFilterCallback, this );
b3Vec3 delta = fraction * result.delta;
m_position = m_position + delta;
if ( b3LengthSquared( delta ) < tolerance * tolerance )
{
break;
}
}
PushBodies();
if ( m_clipVelocity )
{
// Stop pushing into surfaces so speed can't accumulate against a wall
m_velocity = b3ClipVector( m_velocity, m_planes, m_planeCount );
}
else if ( timeStep > 0.0f )
{
m_velocity = ( 1.0f / timeStep ) * ( m_position - startPosition );
}
}
void GeometricMover::Draw() const
{
if ( m_pogoHit )
{
DrawLine( m_pogoOrigin, m_pogoPoint, MakeColor( b3_colorGreen ) );
}
else
{
DrawLine( m_pogoOrigin, b3OffsetPos( m_pogoOrigin, m_pogoTranslation ),
MakeColor( b3_colorGray ) );
}
for ( int i = 0; i < m_planeCount; ++i )
{
b3Plane plane = m_planes[i].plane;
b3Pos p1 = m_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_position, b3Quat_identity }, m_capsule, MakeColor( b3_colorBlue ) );
DrawLine( m_position, m_position + m_velocity, MakeColor( b3_colorPurple ) );
}

114
samples/geometric_mover.h Normal file
View file

@ -0,0 +1,114 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#pragma once
#include "box3d/collision.h"
#include "box3d/types.h"
struct GeometricMoverDef
{
b3Pos position = {};
b3Capsule capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f };
float jumpSpeed = 5.0f;
float maxSpeed = 6.0f;
float minSpeed = 0.01f;
float stopSpeed = 1.0f;
float accelerate = 30.0f;
float friction = 4.0f;
float gravity = 15.0f;
float pogoRestLength = 0.9f;
float pogoHertz = 4.0f;
float pogoDampingRatio = 0.7f;
};
// Optional shape user data read by GeometricMover when it gathers collision planes.
// Shapes without user data are treated as rigid, fully clipping, and pushable.
struct MoverShapeUserData
{
// Distance limit on the push this surface may apply to the mover. FLT_MAX is rigid.
float maxPush;
// False keeps the velocity along this surface, which is what soft collision needs.
bool clipVelocity;
// False stops the mover from shoving this body.
bool canMoverPush;
};
// Extra data kept alongside each collision plane so the mover can push rigid bodies.
struct PlaneExtra
{
b3Pos point;
b3ShapeId shapeId;
};
// Geometric character mover. The capsule is moved by casting and by solving collision
// planes, so the game keeps full control of the motion.
//
// A pogo spring holds the capsule above the ground so it can move up steps.
class GeometricMover
{
public:
GeometricMover();
void Create( b3WorldId worldId, const GeometricMoverDef* def );
// Advance the mover. Throttle is the input along the forward and right directions on [-1, 1].
void Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, float speedScale );
// Jump if grounded. Returns false when airborne.
bool Jump();
void Draw() const;
static constexpr int m_planeCapacity = 8;
float m_jumpSpeed;
float m_maxSpeed;
float m_minSpeed;
float m_stopSpeed;
float m_accelerate;
// Friction in units of 1/time.
float m_friction;
// The mover is not simulated so it needs its own gravity.
float m_gravity;
float m_pogoRestLength;
float m_pogoHertz;
float m_pogoDampingRatio;
// The clipper avoids picking up velocity from soft collision depenetration. Otherwise the
// velocity comes from the position delta, which is more intuitive in some cases.
bool m_clipVelocity;
// Shapes the mover passes through. Set before calling Update.
const b3ShapeId* m_ignoreShapeIds;
int m_ignoreCount;
// The capsule is relative to the mover position.
b3Capsule m_capsule;
b3WorldId m_worldId;
b3Pos m_position;
b3Vec3 m_velocity;
float m_pogoVelocity;
bool m_onGround;
// Planes from the last solve, kept for debug drawing.
b3CollisionPlane m_planes[m_planeCapacity] = {};
PlaneExtra m_planeExtras[m_planeCapacity] = {};
int m_planeCount;
int m_totalIterations;
b3Pos m_pogoOrigin;
b3Vec3 m_pogoTranslation;
b3Pos m_pogoPoint;
bool m_pogoHit;
private:
void PushBodies();
};

View file

@ -32,6 +32,9 @@ enum
KEY_ESCAPE = SAPP_KEYCODE_ESCAPE,
KEY_TAB = SAPP_KEYCODE_TAB,
KEY_LEFT_SHIFT = SAPP_KEYCODE_LEFT_SHIFT,
KEY_RIGHT_SHIFT = SAPP_KEYCODE_RIGHT_SHIFT,
KEY_LEFT_CONTROL = SAPP_KEYCODE_LEFT_CONTROL,
KEY_RIGHT_CONTROL = SAPP_KEYCODE_RIGHT_CONTROL,
KEY_LEFT_BRACKET = SAPP_KEYCODE_LEFT_BRACKET,
KEY_RIGHT_BRACKET = SAPP_KEYCODE_RIGHT_BRACKET,
KEY_PAUSE = SAPP_KEYCODE_PAUSE,

View file

@ -160,6 +160,17 @@ static void OnEvent( const sapp_event* e )
s_context.camera.OnEvent( e );
}
// Releases go to the key table even when the UI captures them. Otherwise Ctrl let go
// while the Ctrl+O picker is open stays down in the next sample.
if ( e->type == SAPP_EVENTTYPE_KEY_UP )
{
SetKeyDown( e->key_code, false );
}
else if ( e->type == SAPP_EVENTTYPE_UNFOCUSED )
{
ClearKeysDown();
}
if ( uiCaptured )
{
return;
@ -267,10 +278,6 @@ static void OnEvent( const sapp_event* e )
}
break;
case SAPP_EVENTTYPE_KEY_UP:
SetKeyDown( e->key_code, false );
break;
case SAPP_EVENTTYPE_CHAR:
// ? toggles the controls window. Read the typed character so it works
// on any layout and a text field can still type a literal ?, since the

View file

@ -1,353 +0,0 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#include "mover.h"
#include "gfx/draw.h"
#include "sample.h"
#include "box3d/box3d.h"
#include <assert.h>
#include <gfx/keycodes.h>
static bool MoverFilterCallback( b3ShapeId shapeId, void* context )
{
CharacterMover* self = (CharacterMover*)context;
for ( int i = 0; i < self->m_ignoreCount; ++i )
{
if ( B3_ID_EQUALS( shapeId, self->m_ignoreShapeIds[i] ) )
{
return false;
}
}
return true;
}
void CharacterMover::Initialize( Sample* sample, b3Pos position )
{
m_sample = sample;
m_transform.p = position;
m_transform.q = b3Quat_identity;
m_velocity = { 0.0f, 0.0f, 0.0f };
m_capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f };
m_planeCount = 0;
m_totalIterations = 0;
m_pogoVelocity = 0.0f;
m_onGround = false;
m_sprint = false;
m_ignoreShapeIds = nullptr;
m_ignoreCount = 0;
}
static bool PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* planeResults, int planeCount, void* context )
{
if ( MoverFilterCallback( shapeId, context ) == false )
{
// ignore these planes but continue looking for more
return true;
}
CharacterMover* self = static_cast<CharacterMover*>( context );
float maxPush = FLT_MAX;
bool clipVelocity = true;
MoverShapeUserData* userData = static_cast<MoverShapeUserData*>( (void*)b3Shape_GetUserData( shapeId ) );
if ( userData != nullptr )
{
maxPush = userData->maxPush;
clipVelocity = userData->clipVelocity;
}
for ( int i = 0; i < planeCount && self->m_planeCount < CharacterMover::m_planeCapacity; ++i )
{
assert( b3IsValidPlane( planeResults[i].plane ) );
self->m_planes[self->m_planeCount] = {
.plane = planeResults[i].plane,
.pushLimit = maxPush,
.push = 0.0f,
.clipVelocity = clipVelocity,
};
self->m_planeExtras[self->m_planeCount] = {
.point = b3OffsetPos( self->m_transform.p, planeResults[i].point ),
.shapeId = shapeId,
};
self->m_planeCount += 1;
}
return true;
}
void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, bool clipVelocity )
{
// Friction
float speed = b3Length( m_velocity );
if ( speed < m_minSpeed )
{
m_velocity.x = 0.0f;
m_velocity.z = 0.0f;
}
else
{
// Linear damping above stopSpeed and fixed reduction below stopSpeed
float control = speed < m_stopSpeed ? m_stopSpeed : speed;
// friction has units of 1/time
float drop = control * m_friction * timeStep;
float newSpeed = b3MaxFloat( 0.0f, speed - drop );
float ratio = newSpeed / speed;
m_velocity.x *= ratio;
m_velocity.z *= ratio;
}
float maxSpeed = m_sprint ? 1.5f * m_maxSpeed : m_maxSpeed;
b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right;
float desiredSpeed;
b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity );
if ( desiredSpeed > maxSpeed )
{
desiredVelocity *= maxSpeed / desiredSpeed;
desiredSpeed = maxSpeed;
}
if ( m_onGround )
{
m_velocity.y = 0.0f;
}
// Accelerate
float currentSpeed = b3Dot( m_velocity, desiredDirection );
float addSpeed = desiredSpeed - currentSpeed;
if ( addSpeed > 0.0f )
{
float accelSpeed = m_accelerate * maxSpeed * timeStep;
if ( accelSpeed > addSpeed )
{
accelSpeed = addSpeed;
}
m_velocity += accelSpeed * desiredDirection;
}
m_velocity.y -= m_gravity * timeStep;
b3WorldId worldId = m_sample->m_worldId;
float pogoRestLength = 3.0f * m_capsule.radius;
float rayLength = pogoRestLength + m_capsule.radius;
b3Pos rayOrigin = b3TransformWorldPoint( m_transform, m_capsule.center1 );
b3Vec3 rayTranslation = -rayLength * b3Vec3_axisY;
b3QueryFilter skipTeamFilter = { 1, ~2u };
skipTeamFilter.name = "pogo";
b3RayResult rayResult = b3World_CastRayClosest( worldId, rayOrigin, rayTranslation, skipTeamFilter );
// After gravity was applied, disable pogo when still moving up.
// Avoids getting pulled back to the ground when jumping.
bool suppressPogo = m_velocity.y > 0.0f;
if ( rayResult.hit == false || suppressPogo )
{
m_onGround = false;
m_pogoVelocity = 0.0f;
DrawLine( rayOrigin, b3OffsetPos( rayOrigin, rayTranslation ), MakeColor( b3_colorGray ) );
}
else
{
m_onGround = true;
float pogoCurrentLength = rayResult.fraction * rayLength;
float zeta = 0.7f;
float hertz = 4.0f;
float omega = 2.0f * B3_PI * hertz;
float omegaH = omega * timeStep;
m_pogoVelocity = ( m_pogoVelocity - omega * omegaH * ( pogoCurrentLength - pogoRestLength ) ) /
( 1.0f + 2.0f * zeta * omegaH + omegaH * omegaH );
DrawLine( rayOrigin, rayResult.point, MakeColor( b3_colorGreen ) );
}
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, .id = 1, .name = "mover_collide" };
// The cast should ignore allies
b3QueryFilter castFilter = { .categoryBits = 1, .maskBits = ~2u, .id = 1, .name = "mover_cast" };
m_totalIterations = 0;
float tolerance = 0.01f;
for ( int iteration = 0; iteration < 5; ++iteration )
{
m_planeCount = 0;
b3Capsule mover;
mover.center1 = m_capsule.center1;
mover.center2 = m_capsule.center2;
mover.radius = m_capsule.radius;
b3World_CollideMover( worldId, m_transform.p, &mover, moverFilter, PlaneResultFcn, this );
b3Vec3 targetDelta = target - m_transform.p;
b3PlaneSolverResult result = b3SolvePlanes( targetDelta, m_planes, m_planeCount );
m_totalIterations += result.iterationCount;
b3Vec3 delta = result.delta;
float fraction = b3World_CastMover( worldId, m_transform.p, &mover, delta, castFilter, MoverFilterCallback, this );
delta *= fraction;
m_transform.p = m_transform.p + delta;
if ( b3LengthSquared( delta ) < tolerance * tolerance )
{
break;
}
}
for ( int i = 0; i < m_planeCount; ++i )
{
b3BodyId bodyId = b3Shape_GetBody( m_planeExtras[i].shapeId );
b3BodyType bodyType = b3Body_GetType( bodyId );
if ( bodyType != b3_dynamicBody )
{
continue;
}
MoverShapeUserData* userData = static_cast<MoverShapeUserData*>( b3Shape_GetUserData( m_planeExtras[i].shapeId ) );
if ( userData != nullptr && userData->canMoverPush == false )
{
continue;
}
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 );
b3Pos pB = b3Body_GetWorldCenter( bodyId );
b3Vec3 rB = b3SubPos( point, pB );
b3Vec3 rnB = b3Cross( rB, normal );
float kNormal = invMassA + invMassB + b3Dot( rnB, b3MulMV( invIB, rnB ) );
float normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
b3Vec3 vB = b3Body_GetLinearVelocity( bodyId );
b3Vec3 omegaB = b3Body_GetAngularVelocity( bodyId );
b3Vec3 vrB = b3Add( vB, b3Cross( omegaB, rB ) );
float vn = b3Dot( b3Sub( vrB, m_velocity ), normal );
float impulse = b3MaxFloat( -normalMass * vn, 0.0f );
b3Vec3 P = b3MulSV( impulse, normal );
m_velocity = b3MulSub( m_velocity, invMassA, P );
b3Body_ApplyLinearImpulse( bodyId, P, point, true );
}
if ( clipVelocity )
{
// Using the velocity clipper can avoid picking up velocity from depenetration.
// This allows the mover to avoid velocity from soft collision depenetration.
m_velocity = b3ClipVector( m_velocity, m_planes, m_planeCount );
}
else if ( timeStep > 0.0f )
{
// Using the position delta is more holistic and intuitive in some cases.
m_velocity = ( 1.0f / timeStep ) * ( m_transform.p - startPosition );
}
}
void CharacterMover::Step( b3ShapeId* ignoreShapes, int ignoreCount, bool clipVelocity )
{
m_ignoreShapeIds = ignoreShapes;
m_ignoreCount = ignoreCount;
b3Vec2 throttle = { 0.0f, 0.0f };
b3Vec3 forward = -m_sample->m_camera->GetForward();
b3Vec3 right = m_sample->m_camera->GetRight();
forward.y = 0.0f;
forward = b3Normalize( forward );
if ( m_sample->m_camera->m_thirdPerson )
{
if ( IsKeyDown( KEY_W ) )
{
throttle.x += 1.0f;
}
if ( IsKeyDown( KEY_S ) )
{
throttle.x -= 1.0f;
}
if ( IsKeyDown( KEY_A ) )
{
throttle.y -= 1.0f;
}
if ( IsKeyDown( KEY_D ) )
{
throttle.y += 1.0f;
}
if ( IsKeyDown( KEY_SPACE ) && m_onGround == true )
{
m_velocity.y = m_jumpSpeed;
m_onGround = false;
}
if ( m_onGround == true )
{
m_sprint = IsKeyDown( KEY_LEFT_SHIFT );
}
else
{
m_sprint = false;
}
}
float hertz = m_sample->m_context->hertz;
float timeStep = hertz > 0.0f ? 1.0f / hertz : 0.0f;
// throttle = { 0.0f, 0.0f, -1.0f };
SolveMove( timeStep, forward, right, throttle, clipVelocity );
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->DrawOrigin() );
int count = m_planeCount;
for ( int i = 0; i < count; ++i )
{
b3Plane plane = m_planes[i].plane;
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( position, position + m_velocity, MakeColor( b3_colorPurple ) );
m_ignoreShapeIds = nullptr;
m_ignoreCount = 0;
}

View file

@ -1,53 +0,0 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#pragma once
#include "box3d/types.h"
class Sample;
struct MoverShapeUserData
{
float maxPush;
bool clipVelocity;
bool canMoverPush;
};
struct PlaneExtra
{
b3Pos point;
b3ShapeId shapeId;
};
struct CharacterMover
{
static constexpr int m_planeCapacity = 8;
static constexpr float m_jumpSpeed = 5.0f;
static constexpr float m_maxSpeed = 6.0f;
static constexpr float m_minSpeed = 0.01f;
static constexpr float m_stopSpeed = 1.0f;
static constexpr float m_accelerate = 30.0f;
static constexpr float m_friction = 4.0f;
static constexpr float m_gravity = 15.0f;
void Initialize( Sample* sample, 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;
b3WorldTransform m_transform;
b3Vec3 m_velocity;
b3Capsule m_capsule;
b3CollisionPlane m_planes[m_planeCapacity] = {};
PlaneExtra m_planeExtras[m_planeCapacity] = {};
int m_planeCount;
int m_totalIterations;
float m_pogoVelocity;
bool m_onGround;
bool m_sprint;
// Transient
b3ShapeId* m_ignoreShapeIds;
int m_ignoreCount;
};

View file

@ -97,6 +97,11 @@ void SetKeyDown( int key, bool down )
}
}
void ClearKeysDown()
{
memset( s_keyDown, 0, sizeof( s_keyDown ) );
}
void SampleContext::Save()
{
const b3DebugDraw* gd = GetGuiDraw();
@ -2308,6 +2313,46 @@ void Sample::ToggleThirdPerson()
}
}
MoverInput Sample::GetMoverInput() const
{
MoverInput input = {};
input.forward = -m_camera->GetForward();
input.right = m_camera->GetRight();
input.forward.y = 0.0f;
input.forward = b3Normalize( input.forward );
if ( m_camera->m_thirdPerson == false )
{
return input;
}
if ( IsKeyDown( KEY_W ) )
{
input.throttle.x += 1.0f;
}
if ( IsKeyDown( KEY_S ) )
{
input.throttle.x -= 1.0f;
}
if ( IsKeyDown( KEY_A ) )
{
input.throttle.y -= 1.0f;
}
if ( IsKeyDown( KEY_D ) )
{
input.throttle.y += 1.0f;
}
input.jump = IsKeyDown( KEY_SPACE );
input.sprint = IsKeyDown( KEY_LEFT_SHIFT ) || IsKeyDown( KEY_RIGHT_SHIFT );
input.walk = IsKeyDown( KEY_LEFT_CONTROL ) || IsKeyDown( KEY_RIGHT_CONTROL );
return input;
}
float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex,
int childIndex, void* context )
{

View file

@ -11,6 +11,7 @@
// Fed from the host event loop; read with the KEY_* aliases in gfx/keycodes.h.
bool IsKeyDown( int key );
void SetKeyDown( int key, bool down );
void ClearKeysDown();
// Key action passed to Keyboard, matching the old GLFW press/release values so
// sample code reads naturally without leaking sokol_app event types.
@ -20,6 +21,17 @@ enum
ACTION_PRESS = 1,
};
// Character input relative to the camera. Throttle x is forward and y is right.
struct MoverInput
{
b3Vec3 forward;
b3Vec3 right;
b3Vec2 throttle;
bool jump;
bool sprint;
bool walk;
};
struct SampleContext
{
void Save();
@ -211,6 +223,9 @@ public:
void ToggleThirdPerson();
// WASD, space, shift and ctrl, active only in third person so the free camera keeps its keys.
MoverInput GetMoverInput() const;
B3_PRINTF_FORMAT( 2, 3 )
void DrawTextLine( const char* text, ... );
void ResetProfile();

View file

@ -1,10 +1,11 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "dynamic_mover.h"
#include "geometric_mover.h"
#include "gfx/draw.h"
#include "gfx/keycodes.h"
#include "mesh_loader.h"
#include "mover.h"
#include "sample.h"
#include "box3d/box3d.h"
@ -321,10 +322,10 @@ struct MoverImpact
bool valid;
};
class GeometricMover : public Sample
class GeometricMoverSample : public Sample
{
public:
explicit GeometricMover( SampleContext* context )
explicit GeometricMoverSample( SampleContext* context )
: Sample( context )
{
b3Pos moverPosition = { 7.5f, 0.75f, 9.0f };
@ -334,7 +335,11 @@ public:
m_camera->SetView( 120.0f, 30.0f, 5.0f, moverPosition );
}
m_mover.Initialize( this, moverPosition );
GeometricMoverDef moverDef;
moverDef.position = moverPosition;
m_mover.Create( m_worldId, &moverDef );
m_mover.m_ignoreShapeIds = &m_ignoreShapeId;
m_mover.m_ignoreCount = 1;
{
m_levelMesh = CreateMeshData( "data/meshes/test_map01.obj", 1.0f, false, false, true, true );
@ -557,7 +562,6 @@ public:
//}
m_camera->m_thirdPerson = false;
m_clipVelocity = true;
// sapp_lock_mouse( true );
// m_haveMouseLast = false;
@ -565,7 +569,7 @@ public:
// m_mouseDelta = { 0.0f, 0.0f };
}
~GeometricMover() override
~GeometricMoverSample() override
{
m_camera->m_thirdPerson = false;
sapp_lock_mouse( false );
@ -589,7 +593,7 @@ public:
ToggleThirdPerson();
}
ImGui::Checkbox( "Clip Velocity", &m_clipVelocity );
ImGui::Checkbox( "Clip Velocity", &m_mover.m_clipVelocity );
return true;
}
@ -691,7 +695,7 @@ public:
void UpdateFallingTree( b3Pos moverStart, b3WorldTransform treeStart )
{
b3WorldTransform treeEnd = b3Body_GetTransform( m_treeId );
b3Vec3 moverTranslation = b3SubPos( m_mover.m_transform.p, moverStart );
b3Vec3 moverTranslation = b3SubPos( m_mover.m_position, moverStart );
b3BodyTOIResult toi = b3Body_TimeOfImpactMover( m_treeId, moverStart, &m_mover.m_capsule, moverTranslation,
b3DefaultQueryFilter(), treeStart, treeEnd );
@ -712,7 +716,7 @@ public:
void UpdateFallingBox( b3Pos moverStart, b3WorldTransform boxStart )
{
b3WorldTransform boxEnd = b3Body_GetTransform( m_fallingBoxId );
b3Vec3 moverTranslation = b3SubPos( m_mover.m_transform.p, moverStart );
b3Vec3 moverTranslation = b3SubPos( m_mover.m_position, moverStart );
b3BodyTOIResult toi = b3Body_TimeOfImpactMover( m_fallingBoxId, moverStart, &m_mover.m_capsule, moverTranslation,
b3DefaultQueryFilter(), boxStart, boxEnd );
@ -742,11 +746,37 @@ public:
void Step() override
{
b3Pos moverStart = m_mover.m_transform.p;
b3Pos moverStart = m_mover.m_position;
b3WorldTransform boxStart = b3Body_GetTransform( m_fallingBoxId );
b3WorldTransform treeStart = b3Body_GetTransform( m_treeId );
m_mover.Step( &m_ignoreShapeId, 1, m_clipVelocity );
MoverInput input = GetMoverInput();
if ( input.jump )
{
m_mover.Jump();
}
float speedScale = 1.0f;
if ( input.walk )
{
speedScale = 0.25f;
}
else if ( m_mover.m_onGround && input.sprint )
{
speedScale = 1.5f;
}
float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f;
m_mover.Update( timeStep, input.forward, input.right, input.throttle, speedScale );
if ( m_camera->m_thirdPerson )
{
m_camera->m_pivot = m_mover.m_position;
m_camera->UpdateTransform();
}
SetDrawOrigin( m_camera->DrawOrigin() );
m_mover.Draw();
DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson );
DrawTextLine( "deltaX = %g, deltaY = %g", m_mouseDelta.x, m_mouseDelta.y );
@ -769,10 +799,10 @@ public:
static Sample* Create( SampleContext* context )
{
return new GeometricMover( context );
return new GeometricMoverSample( context );
}
CharacterMover m_mover;
GeometricMover m_mover;
MoverShapeUserData m_enemyShape;
MoverShapeUserData m_friendlyShape;
MoverShapeUserData m_treeShapeUserData;
@ -793,10 +823,751 @@ public:
b3Vec3 m_fallingExtent;
b3Vec3 m_fallingSpin;
float m_respawnTimer;
bool m_clipVelocity;
};
static int sampleMover = RegisterSample( "Character", "Geometric Mover", GeometricMover::Create );
static int sampleMover =
RegisterSample( "Character", "Geometric Mover", GeometricMoverSample::Create );
// A playground for the dynamic mover. Collect every gem to finish a lap. The course has stairs, a
// rope bridge, an elevator and a shuttle to a high route, a seesaw, crates to shove, a sweeper arm
// to jump over, and a row of thin pillars that only the skirt probes can stand on.
class DynamicMoverSample : public Sample
{
public:
explicit DynamicMoverSample( SampleContext* context )
: Sample( context )
{
b3Pos startPosition = { 0.0f, 1.5f, 0.0f };
if ( m_context->restart == false )
{
m_camera->SetView( 180.0f, 20.0f, 6.0f, startPosition );
}
b3BodyId groundId = AddGroundBox( 80.0f );
{
DynamicMoverDef def;
def.position = startPosition;
def.filter = { MoverBit, AllBits, 0 };
def.platformMaterialId = m_platformMaterial;
def.enableSensorEvents = true;
m_mover.Create( m_worldId, &def );
}
b3ShapeDef shapeDef = b3DefaultShapeDef();
// Stairs up to the bridge
{
b3BodyDef bodyDef = b3DefaultBodyDef();
b3BodyId stairsId = b3CreateBody( m_worldId, &bodyDef );
shapeDef.baseMaterial.customColor = b3_colorCornflowerBlue;
for ( int i = 0; i < 10; ++i )
{
float halfHeight = 0.1f * ( i + 1 );
b3Transform transform = { { -6.0f, halfHeight, 3.3f + 0.6f * i }, b3Quat_identity };
b3BoxHull box = b3MakeTransformedBoxHull( 1.5f, halfHeight, 0.3f, transform );
b3CreateHullShape( stairsId, &shapeDef, &box.base );
}
}
// Towers at each end of the bridge
b3BodyId towerIds[2];
for ( int i = 0; i < 2; ++i )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = { i == 0 ? -6.0f : 6.0f, 1.0f, 11.0f };
towerIds[i] = b3CreateBody( m_worldId, &bodyDef );
shapeDef.baseMaterial.customColor = b3_colorSlateGray;
b3BoxHull box = b3MakeBoxHull( 1.5f, 1.0f, 2.0f );
b3CreateHullShape( towerIds[i], &shapeDef, &box.base );
}
// The pogo presses down on the planks, so the bridge sags and sways underfoot
{
int plankCount = 18;
float plankLength = 9.0f / plankCount;
float xBase = -4.5f;
float y = 1.95f;
float z = 11.0f;
b3ShapeDef plankShapeDef = b3DefaultShapeDef();
plankShapeDef.density = 300.0f;
plankShapeDef.baseMaterial.customColor = b3_colorBurlywood;
b3BoxHull plank = b3MakeBoxHull( 0.5f * plankLength - 0.01f, 0.05f, 0.75f );
// The hinge is the local z-axis, across the bridge
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
jointDef.enableMotor = true;
jointDef.maxMotorTorque = 50.0f;
b3BodyId prevId = towerIds[0];
for ( int i = 0; i < plankCount; ++i )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = { xBase + plankLength * ( i + 0.5f ), y, z };
bodyDef.angularDamping = 0.5f;
b3BodyId plankId = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( plankId, &plankShapeDef, &plank.base );
b3Pos pivot = { xBase + plankLength * i, y, z };
jointDef.base.bodyIdA = prevId;
jointDef.base.bodyIdB = plankId;
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( prevId, pivot );
jointDef.base.localFrameB.p = b3Body_GetLocalPoint( plankId, pivot );
b3CreateRevoluteJoint( m_worldId, &jointDef );
prevId = plankId;
}
b3Pos pivot = { xBase + plankLength * plankCount, y, z };
jointDef.base.bodyIdA = prevId;
jointDef.base.bodyIdB = towerIds[1];
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( prevId, pivot );
jointDef.base.localFrameB.p = b3Body_GetLocalPoint( towerIds[1], pivot );
b3CreateRevoluteJoint( m_worldId, &jointDef );
}
// Ramps from the east tower down to the ground, one walkable and one too steep
AddRamp( { 6.0f, 2.0f, 9.0f }, { 6.0f, 0.0f, 3.5f }, 1.5f, b3_colorOliveDrab );
AddRamp( { 12.0f, 3.0f, -12.0f }, { 12.0f, 0.0f, -9.5f }, 3.5f, b3_colorIndianRed );
// High route: an elevator up to a ledge, then a shuttle across to a second ledge
for ( int i = 0; i < 2; ++i )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = { i == 0 ? 18.0f : 32.0f, 2.0f, 0.0f };
b3BodyId ledgeId = b3CreateBody( m_worldId, &bodyDef );
shapeDef.baseMaterial.customColor = b3_colorDarkSlateGray;
b3BoxHull box = b3MakeBoxHull( 2.0f, 2.0f, 2.0f );
b3CreateHullShape( ledgeId, &shapeDef, &box.base );
}
for ( int i = 0; i < m_platformCount; ++i )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_kinematicBody;
bodyDef.position = GetPlatformPosition( i, 0.0f );
m_platformIds[i] = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef platformShapeDef = b3DefaultShapeDef();
platformShapeDef.baseMaterial.userMaterialId = m_platformMaterial;
platformShapeDef.baseMaterial.customColor = b3_colorGoldenRod;
b3BoxHull box = b3MakeBoxHull( 1.5f, 0.1f, 1.5f );
b3CreateHullShape( m_platformIds[i], &platformShapeDef, &box.base );
}
// Seesaw
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = { -14.0f, 0.4f, 0.0f };
b3BodyId fulcrumId = b3CreateBody( m_worldId, &bodyDef );
shapeDef.baseMaterial.customColor = b3_colorSlateGray;
b3BoxHull fulcrum = b3MakeBoxHull( 0.25f, 0.4f, 0.75f );
b3CreateHullShape( fulcrumId, &shapeDef, &fulcrum.base );
bodyDef.type = b3_dynamicBody;
bodyDef.position = { -14.0f, 0.9f, 0.0f };
b3BodyId plankId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef plankShapeDef = b3DefaultShapeDef();
plankShapeDef.density = 200.0f;
plankShapeDef.baseMaterial.customColor = b3_colorBurlywood;
b3BoxHull plank = b3MakeBoxHull( 3.5f, 0.1f, 0.8f );
b3CreateHullShape( plankId, &plankShapeDef, &plank.base );
b3Pos pivot = { -14.0f, 0.8f, 0.0f };
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
jointDef.base.bodyIdA = fulcrumId;
jointDef.base.bodyIdB = plankId;
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( fulcrumId, pivot );
jointDef.base.localFrameB.p = b3Body_GetLocalPoint( plankId, pivot );
jointDef.enableLimit = true;
jointDef.lowerAngle = -0.3f;
jointDef.upperAngle = 0.3f;
b3CreateRevoluteJoint( m_worldId, &jointDef );
}
// Large pyramid past the seesaw, standing in the yz plane
{
int baseCount = m_isDebug ? 20 : 100;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
b3ShapeDef boxShapeDef = b3DefaultShapeDef();
boxShapeDef.density = 50.0f;
float h = 0.25f;
b3BoxHull box = b3MakeCubeHull( h );
for ( int i = 0; i < baseCount; ++i )
{
float y = ( 2.0f * i + 1.0f ) * h;
for ( int j = i; j < baseCount; ++j )
{
float z = ( i + 1.0f ) * h + 2.0f * ( j - i ) * h - h * baseCount;
bodyDef.position = { -50.0f, y, z };
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( bodyId, &boxShapeDef, &box.base );
}
}
}
// Crates to shove around
{
b3ShapeDef crateShapeDef = b3DefaultShapeDef();
crateShapeDef.density = 100.0f;
crateShapeDef.baseMaterial.friction = 0.6f;
crateShapeDef.baseMaterial.customColor = b3_colorPeru;
b3BoxHull crate = b3MakeCubeHull( 0.4f );
for ( int row = 0; row < 4; ++row )
{
for ( int i = 0; i < 4 - row; ++i )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = { -10.0f + 0.82f * ( i - 0.5f * ( 3 - row ) ),
0.4f + 0.8f * row, -8.0f };
b3BodyId crateId = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( crateId, &crateShapeDef, &crate.base );
}
}
}
// Chest high spheres on soft welds the mover can push aside
{
b3ShapeDef sphereShapeDef = b3DefaultShapeDef();
sphereShapeDef.density = 100.0f;
sphereShapeDef.baseMaterial.customColor = b3_colorLightSeaGreen;
sphereShapeDef.filter = { SphereBit, AllBits, 0 };
b3Sphere sphere = { b3Vec3_zero, 0.35f };
b3WeldJointDef jointDef = b3DefaultWeldJointDef();
jointDef.linearHertz = 0.5f;
jointDef.linearDampingRatio = 0.2f;
jointDef.base.bodyIdA = groundId;
jointDef.base.collideConnected = true;
int gridCount = 20;
for ( int i = 0; i < gridCount; ++i )
{
for ( int j = 0; j < gridCount; ++j )
{
b3Pos position = { -9.5f + 1.0f * i, 1.7f, 17.0f + 1.0f * j };
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = position;
bodyDef.gravityScale = 0.0f;
b3BodyId sphereId = b3CreateBody( m_worldId, &bodyDef );
b3CreateSphereShape( sphereId, &sphereShapeDef, &sphere );
jointDef.base.bodyIdB = sphereId;
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, position );
jointDef.base.localFrameB.p = b3Vec3_zero;
b3CreateWeldJoint( m_worldId, &jointDef );
}
}
}
// Pressure plate under the middle of the sphere grid
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = m_plateCenter;
b3BodyId plateId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef plateShapeDef = b3DefaultShapeDef();
plateShapeDef.density = 20.0f;
plateShapeDef.baseMaterial.customColor = b3_colorFireBrick;
plateShapeDef.filter = { PlateBit, MoverBit, 0 };
// The blast would only slam the plate into its lower limit
plateShapeDef.explosionScale = 0.0f;
b3BoxHull plate = b3MakeBoxHull( 1.5f, 0.1f, 1.5f );
b3CreateHullShape( plateId, &plateShapeDef, &plate.base );
// The motor force works like a preloaded spring. The plate holds its own weight but
// sinks under the mover.
float gravity = b3Length( b3World_GetGravity( m_worldId ) );
float plateWeight = gravity * b3Body_GetMass( plateId );
float moverWeight = m_mover.m_gravityScale * gravity * b3Body_GetMass( m_mover.m_moverId );
// The prismatic axis is the local x-axis, so turn it upright
b3Quat upright = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI );
b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = plateId;
jointDef.base.localFrameA = { b3Body_GetLocalPoint( groundId, m_plateCenter ), upright };
jointDef.base.localFrameB = { b3Vec3_zero, upright };
jointDef.enableLimit = true;
jointDef.lowerTranslation = -m_plateTravel;
jointDef.upperTranslation = 0.0f;
jointDef.enableMotor = true;
jointDef.motorSpeed = 0.5f;
jointDef.maxMotorForce = plateWeight + 0.5f * moverWeight;
m_plateJointId = b3CreatePrismaticJoint( m_worldId, &jointDef );
m_plateArmed = true;
}
// Sleeping boxes overhead that the blast wakes, kept inside its reach
if ( m_isDebug == false )
{
b3ShapeDef boxShapeDef = b3DefaultShapeDef();
boxShapeDef.density = 100.0f;
boxShapeDef.baseMaterial.customColor = b3_colorPeru;
// The blast should wake these rather than launch them
boxShapeDef.explosionScale = 0.05f;
b3BoxHull box = b3MakeCubeHull( 0.1f );
int gridCount = 32;
int layerCount = 3;
float spacing = 0.5f;
float offset = -0.5f * spacing * ( gridCount - 1 );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.isAwake = false;
for ( int layer = 0; layer < layerCount; ++layer )
{
for ( int i = 0; i < gridCount; ++i )
{
for ( int j = 0; j < gridCount; ++j )
{
bodyDef.position = { m_plateCenter.x + offset + spacing * i, 8.0f + spacing * layer,
m_plateCenter.z + offset + spacing * j };
b3BodyId boxId = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( boxId, &boxShapeDef, &box.base );
}
}
}
}
// A big ball to roll
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = { 6.0f, 1.0f, -6.0f };
b3BodyId ballId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef ballShapeDef = b3DefaultShapeDef();
ballShapeDef.density = 30.0f;
ballShapeDef.baseMaterial.rollingResistance = 0.05f;
ballShapeDef.baseMaterial.customColor = b3_colorTomato;
b3Sphere sphere = { b3Vec3_zero, 1.0f };
b3CreateSphereShape( ballId, &ballShapeDef, &sphere );
}
// The sweeper arms are at waist height, so time a jump to get past
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = m_sweeperCenter;
//bodyDef.angularVelocity = { 0.0f, 1.2f, 0.0f };
b3BodyId sweeperId = b3CreateBody( m_worldId, &bodyDef );
shapeDef.baseMaterial.customColor = b3_colorCrimson;
b3BoxHull post = b3MakeTransformedBoxHull( 0.3f, 0.6f, 0.3f,
{ { 0.0f, 0.6f, 0.0f }, b3Quat_identity } );
b3CreateHullShape( sweeperId, &shapeDef, &post.base );
b3BoxHull armX = b3MakeTransformedBoxHull( 5.0f, 0.15f, 0.15f,
{ { 0.0f, 1.0f, 0.0f }, b3Quat_identity } );
b3CreateHullShape( sweeperId, &shapeDef, &armX.base );
b3BoxHull armZ = b3MakeTransformedBoxHull( 0.15f, 0.15f, 5.0f,
{ { 0.0f, 1.0f, 0.0f }, b3Quat_identity } );
b3CreateHullShape( sweeperId, &shapeDef, &armZ.base );
b3Quat upright = b3MakeQuatFromAxisAngle( b3Vec3_axisX, -0.5f * B3_PI );
b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef();
jointDef.base.bodyIdA = groundId;
jointDef.base.bodyIdB = sweeperId;
jointDef.base.localFrameA = { b3Body_GetLocalPoint( groundId, m_sweeperCenter ),
upright };
jointDef.base.localFrameB = { b3Vec3_zero, upright };
jointDef.enableMotor = true;
jointDef.motorSpeed = 1.0f;
jointDef.maxMotorTorque = 10000.0f;
b3CreateRevoluteJoint( m_worldId, &jointDef );
}
shapeDef.baseMaterial.customColor = b3_colorMediumPurple;
for ( int i = 0; i < m_pillarCount; ++i )
{
b3Pos top = GetPillarTop( i );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.position = { top.x, 0.0f, top.z };
b3BodyId pillarId = b3CreateBody( m_worldId, &bodyDef );
b3HullData* pillar = b3CreateCylinder( (float)top.y, 0.75f, 0.0f, 12 );
b3CreateHullShape( pillarId, &shapeDef, pillar );
b3DestroyHull( pillar );
}
SpawnGems();
m_camera->m_thirdPerson = true;
sapp_lock_mouse( true );
m_jumpReleased = true;
m_drawProbes = true;
m_time = 0.0f;
m_lapTime = 0.0f;
m_bestLapTime = 0.0f;
m_lapCount = 0;
}
~DynamicMoverSample() override
{
m_camera->m_thirdPerson = false;
sapp_lock_mouse( false );
}
// A thin static slab whose top face runs from the high point to the low point
void AddRamp( b3Vec3 high, b3Vec3 low, float halfWidth, b3HexColor color )
{
b3Vec3 axis = b3Sub( high, low );
float length = b3Length( axis );
axis = b3MulSV( 1.0f / length, axis );
b3Quat rotation = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, axis );
b3Vec3 normal = b3RotateVector( rotation, b3Vec3_axisY );
float halfThickness = 0.1f;
b3Vec3 center = b3MulAdd( b3Lerp( low, high, 0.5f ), -halfThickness, normal );
b3BodyDef bodyDef = b3DefaultBodyDef();
b3BodyId rampId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.customColor = color;
b3BoxHull box = b3MakeTransformedBoxHull( halfWidth, halfThickness, 0.5f * length,
{ center, rotation } );
b3CreateHullShape( rampId, &shapeDef, &box.base );
}
b3Pos GetPlatformPosition( int index, float time ) const
{
if ( index == 0 )
{
// Elevator, the top face rises from the ground to the first ledge
float y = 1.95f - 1.95f * cosf( 0.6f * time );
return { 14.0f, y, 0.0f };
}
// Shuttle between the two ledges
float x = 25.0f - 3.5f * cosf( 0.5f * time );
return { x, 3.9f, 0.0f };
}
b3Pos GetPillarTop( int index ) const
{
return { -20.0f, 0.5f + 0.6f * index, -3.0f - 5.0f * index };
}
void SpawnGems()
{
b3Pos positions[m_gemCount] = {
{ 0.0f, 1.2f, 5.0f }, { -6.0f, 2.2f, 6.0f }, { -6.0f, 3.2f, 11.0f },
{ 0.0f, 3.2f, 11.0f }, { 6.0f, 3.2f, 11.0f }, { 6.0f, 2.2f, 6.0f },
{ 18.0f, 5.2f, 0.0f }, { 25.0f, 5.2f, 0.0f }, { 32.0f, 5.2f, 0.0f },
{ -17.0f, 2.2f, 0.0f }, { -10.0f, 4.4f, -8.0f }, { 6.0f, 1.2f, -9.5f },
{ 3.0f, 1.2f, -16.0f }, { -3.0f, 1.2f, -16.0f }, { 0.0f, 1.2f, -19.0f },
{ 0.0f, 1.2f, -13.0f }, { -20.0f, 0.0f, 0.0f },
};
// The last gem waits on the tallest pillar
positions[m_gemCount - 1] = GetPillarTop( m_pillarCount - 1 );
positions[m_gemCount - 1].y += 1.2f;
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.isSensor = true;
shapeDef.enableSensorEvents = true;
shapeDef.filter = { GemBit, MoverBit, 0 };
shapeDef.baseMaterial.customColor = b3_colorGold;
// Stand a cube on a corner so it reads as a gem
b3Quat tilt = b3MakeQuatFromAxisAngle( b3Normalize( { 1.0f, 0.0f, -1.0f } ), 0.9553f );
b3BoxHull gem = b3MakeTransformedBoxHull( 0.2f, 0.2f, 0.2f, { b3Vec3_zero, tilt } );
for ( int i = 0; i < m_gemCount; ++i )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_kinematicBody;
bodyDef.position = positions[i];
bodyDef.angularVelocity = { 0.0f, 2.0f, 0.0f };
m_gemIds[i] = b3CreateBody( m_worldId, &bodyDef );
b3CreateHullShape( m_gemIds[i], &shapeDef, &gem.base );
}
m_gemsLeft = m_gemCount;
}
void UpdatePressurePlate()
{
float translation = b3PrismaticJoint_GetTranslation( m_plateJointId );
// Fire once per press. The plate has to spring back up to rearm.
if ( m_plateArmed && translation < -m_plateTravel + 0.01f )
{
b3ExplosionDef def = b3DefaultExplosionDef();
def.position = b3OffsetPos( m_plateCenter, b3Vec3{ 0.0f, 0.1f, 0.0f } );
def.radius = 1.0f;
def.falloff = 14.0f;
def.impulsePerArea = 3000.0f;
b3World_Explode( m_worldId, &def );
m_mover.Launch();
m_plateArmed = false;
}
else if ( m_plateArmed == false && translation > -0.01f )
{
m_plateArmed = true;
}
}
void CollectGems()
{
b3SensorEvents events = b3World_GetSensorEvents( m_worldId );
for ( int i = 0; i < events.beginCount; ++i )
{
b3SensorBeginTouchEvent* event = events.beginEvents + i;
if ( b3Shape_IsValid( event->visitorShapeId ) == false ||
b3Shape_IsValid( event->sensorShapeId ) == false )
{
continue;
}
b3BodyId visitorId = b3Shape_GetBody( event->visitorShapeId );
if ( B3_ID_EQUALS( visitorId, m_mover.m_moverId ) == false )
{
continue;
}
b3BodyId gemId = b3Shape_GetBody( event->sensorShapeId );
for ( b3BodyId& id : m_gemIds )
{
if ( B3_ID_EQUALS( id, gemId ) )
{
b3DestroyBody( id );
id = b3_nullBodyId;
m_gemsLeft -= 1;
break;
}
}
}
if ( m_gemsLeft == 0 )
{
m_lapCount += 1;
if ( m_bestLapTime == 0.0f || m_lapTime < m_bestLapTime )
{
m_bestLapTime = m_lapTime;
}
m_lapTime = 0.0f;
SpawnGems();
}
}
// Pull the boom in when the view to the mover is blocked
void FollowMover( b3Pos position )
{
if ( m_camera->m_thirdPerson == false )
{
return;
}
m_camera->m_pivot = position;
m_camera->UpdateTransform();
b3Vec3 translation = b3SubPos( m_camera->m_worldEye, position );
float distance = b3Length( translation );
if ( distance < 0.01f )
{
return;
}
b3QueryFilter filter = { StaticBit, StaticBit, 0, "camera" };
b3RayResult result = b3World_CastRayClosest( m_worldId, position, translation, filter );
if ( result.hit )
{
float savedRadius = m_camera->m_radius;
m_camera->m_radius = b3MaxFloat( 0.1f, result.fraction * distance - 0.15f );
m_camera->UpdateTransform();
m_camera->m_radius = savedRadius;
}
}
void Keyboard( int key, int action, int mods ) override
{
if ( key == KEY_T && action == ACTION_PRESS )
{
ToggleThirdPerson();
}
}
bool DrawControls() override
{
bool thirdPerson = m_camera->m_thirdPerson;
if ( ImGui::Checkbox( "Third Person (T)", &thirdPerson ) )
{
ToggleThirdPerson();
}
ImGui::Checkbox( "Skirt Pogo", &m_mover.m_enableSkirtPogo );
ImGui::Checkbox( "Draw Probes", &m_drawProbes );
ImGui::Separator();
ImGui::SliderFloat( "Jump Speed", &m_mover.m_jumpSpeed, 0.0f, 20.0f, "%.1f" );
ImGui::SliderFloat( "Max Speed", &m_mover.m_maxSpeed, 0.0f, 20.0f, "%.0f" );
ImGui::SliderFloat( "Accelerate", &m_mover.m_accelerate, 0.0f, 100.0f, "%.0f" );
ImGui::SliderFloat( "Friction", &m_mover.m_friction, 0.0f, 20.0f, "%.1f" );
ImGui::SliderFloat( "Air Steer", &m_mover.m_airSteer, 0.0f, 1.0f, "%.2f" );
float gravityScale = m_mover.m_gravityScale;
if ( ImGui::SliderFloat( "Gravity Scale", &gravityScale, 0.0f, 4.0f, "%.1f" ) )
{
m_mover.SetGravityScale( gravityScale );
}
ImGui::SliderFloat( "Pogo Hertz", &m_mover.m_pogoHertz, 0.0f, 30.0f, "%.0f" );
ImGui::SliderFloat( "Pogo Damping", &m_mover.m_pogoDampingRatio, 0.0f, 4.0f, "%.1f" );
ImGui::SliderFloat( "Ground Force", &m_mover.m_maxGroundForce, 0.0f, 20000.0f, "%.0f" );
return true;
}
void Step() override
{
bool pause = m_context->pause && m_context->singleStep == 0;
float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f;
if ( pause == false && timeStep > 0.0f )
{
m_time += timeStep;
m_lapTime += timeStep;
for ( int i = 0; i < m_platformCount; ++i )
{
b3WorldTransform target = { GetPlatformPosition( i, m_time ), b3Quat_identity };
b3Body_SetTargetTransform( m_platformIds[i], target, timeStep, true );
}
MoverInput input = GetMoverInput();
// Jumping again needs a fresh press
if ( input.jump )
{
if ( m_jumpReleased && m_mover.Jump() )
{
m_jumpReleased = false;
}
}
else
{
m_jumpReleased = true;
}
float speedScale = 1.0f;
if ( input.walk )
{
speedScale = 0.25f;
}
else if ( input.sprint )
{
speedScale = 1.5f;
}
m_mover.Update( timeStep, input.forward, input.right, input.throttle, speedScale );
}
Sample::Step();
if ( m_didStep )
{
CollectGems();
UpdatePressurePlate();
}
b3Pos position = b3Body_GetPosition( m_mover.m_moverId );
FollowMover( position );
SetDrawOrigin( m_camera->DrawOrigin() );
if ( m_drawProbes )
{
m_mover.Draw();
}
DrawTextLine( "WASD = move, Space = jump, Shift = sprint, Ctrl = walk, T = camera" );
DrawTextLine( "gems %d / %d, lap time %.1f s", m_gemCount - m_gemsLeft, m_gemCount,
m_lapTime );
if ( m_lapCount > 0 )
{
DrawTextLine( "laps %d, best %.1f s", m_lapCount, m_bestLapTime );
}
float normalY = m_mover.m_castResult.hit ? m_mover.m_castResult.normal.y : 0.0f;
DrawTextLine( "on ground %d, walkable %d, normal y %.2f", (int)m_mover.m_onGround,
(int)m_mover.m_walkable, normalY );
DrawTextLine( "pogo length %.2f / %.2f", m_mover.m_pogoLength, m_mover.m_pogoRestLength );
}
static Sample* Create( SampleContext* context )
{
return new DynamicMoverSample( context );
}
static constexpr uint64_t StaticBit = 0x1;
static constexpr uint64_t MoverBit = 0x2;
static constexpr uint64_t GemBit = 0x4;
static constexpr uint64_t PlateBit = 0x8;
static constexpr uint64_t SphereBit = 0x10;
static constexpr uint64_t AllBits = ~0ull;
static constexpr uint64_t m_platformMaterial = 7;
static constexpr int m_platformCount = 2;
static constexpr int m_pillarCount = 6;
static constexpr int m_gemCount = 17;
static constexpr b3Pos m_sweeperCenter = { 0.0f, 0.0f, -16.0f };
static constexpr b3Pos m_plateCenter = { 0.0f, 0.05f, 26.5f };
static constexpr float m_plateTravel = 0.1f;
DynamicMover m_mover;
b3JointId m_plateJointId;
bool m_plateArmed;
b3BodyId m_platformIds[m_platformCount];
b3BodyId m_gemIds[m_gemCount];
int m_gemsLeft;
int m_lapCount;
float m_time;
float m_lapTime;
float m_bestLapTime;
bool m_jumpReleased;
bool m_drawProbes;
};
static int sampleDynamicMover =
RegisterSample( "Character", "Dynamic Mover", DynamicMoverSample::Create );
struct ClosestShapeCastContext
{

View file

@ -1,11 +1,11 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "geometric_mover.h"
#include "gfx/debug_adapter.h"
#include "gfx/draw.h"
#include "human.h"
#include "mesh_loader.h"
#include "mover.h"
#include "sample.h"
#include "utils.h"
@ -501,7 +501,9 @@ public:
}
m_launchSpeedScale = 2.0f;
m_mover.Initialize( this, position );
GeometricMoverDef moverDef;
moverDef.position = position;
m_mover.Create( m_worldId, &moverDef );
{
constexpr int capsuleCapacity = gridCount * gridCount / 8 + 1;
@ -711,7 +713,33 @@ public:
void Step() override
{
m_mover.Step( nullptr, 0, true );
MoverInput input = GetMoverInput();
if ( input.jump )
{
m_mover.Jump();
}
float speedScale = 1.0f;
if ( input.walk )
{
speedScale = 0.25f;
}
else if ( m_mover.m_onGround && input.sprint )
{
speedScale = 1.5f;
}
float moverTimeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f;
m_mover.Update( moverTimeStep, input.forward, input.right, input.throttle, speedScale );
if ( m_camera->m_thirdPerson )
{
m_camera->m_pivot = m_mover.m_position;
m_camera->UpdateTransform();
}
SetDrawOrigin( m_camera->DrawOrigin() );
m_mover.Draw();
DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson );
b3Vec3 translation = { 10.0f, -40.0f, -5.0f };
@ -801,7 +829,7 @@ public:
}
b3CompoundData* m_compound;
CharacterMover m_mover;
GeometricMover m_mover;
float m_worldWidth;
b3Pos m_rayOrigin;
};

View file

@ -94,6 +94,10 @@ static const char* ReplayJointTypeName( b3JointType type )
return "weld";
case b3_wheelJoint:
return "wheel";
case b3_moverJoint:
return "mover";
case b3_pogoJoint:
return "pogo";
default:
return "?";
}

View file

@ -51,6 +51,7 @@ set(BOX3D_SOURCE_FILES
mesh.c
motor_joint.c
mover.c
mover_joint.c
name_cache.c
name_cache.h
parallel_for.c
@ -59,6 +60,7 @@ set(BOX3D_SOURCE_FILES
physics_world.c
physics_world.h
platform.h
pogo_joint.c
prismatic_joint.c
qsort.h
rapidhash.h

View file

@ -62,6 +62,21 @@ b3MotorJointDef b3DefaultMotorJointDef( void )
return def;
}
b3MoverJointDef b3DefaultMoverJointDef( void )
{
b3MoverJointDef def = { 0 };
def.base = b3DefaultJointDef();
return def;
}
b3PogoJointDef b3DefaultPogoJointDef( void )
{
b3PogoJointDef def = { 0 };
def.base = b3DefaultJointDef();
def.normal = b3Vec3_axisY;
return def;
}
b3FilterJointDef b3DefaultFilterJointDef( void )
{
b3FilterJointDef def = { 0 };
@ -511,6 +526,66 @@ b3JointId b3CreateMotorJoint( b3WorldId worldId, const b3MotorJointDef* def )
return jointId;
}
b3JointId b3CreateMoverJoint( b3WorldId worldId, const b3MoverJointDef* def )
{
B3_CHECK_JOINT_DEF( def );
B3_ASSERT( b3IsValidVec3( def->linearVelocity ) );
B3_ASSERT( b3IsValidVec3( def->maxVelocityForce ) );
b3World* world = b3GetUnlockedWorldFromId( worldId );
if ( world == NULL )
{
return (b3JointId){ 0 };
}
b3JointPair pair = b3CreateJoint( world, &def->base, b3_moverJoint );
b3JointSim* joint = pair.jointSim;
joint->moverJoint = (b3MoverJoint){ 0 };
joint->moverJoint.linearVelocity = def->linearVelocity;
joint->moverJoint.maxVelocityForce = def->maxVelocityForce;
b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation };
B3_REC_CREATE( world, CreateMoverJoint, jointId, worldId, *def );
return jointId;
}
b3JointId b3CreatePogoJoint( b3WorldId worldId, const b3PogoJointDef* def )
{
B3_CHECK_JOINT_DEF( def );
B3_ASSERT( b3IsValidVec3( def->normal ) );
B3_ASSERT( b3IsValidFloat( def->restLength ) && def->restLength >= 0.0f );
B3_ASSERT( b3IsValidFloat( def->hertz ) && def->hertz >= 0.0f );
B3_ASSERT( b3IsValidFloat( def->dampingRatio ) && def->dampingRatio >= 0.0f );
B3_ASSERT( b3IsValidFloat( def->maxTensionForce ) && def->maxTensionForce >= 0.0f );
B3_ASSERT( b3IsValidFloat( def->maxCompressionForce ) && def->maxCompressionForce >= 0.0f );
B3_ASSERT( b3IsValidFloat( def->impulse ) );
B3_ASSERT( b3IsValidFloat( def->velocity ) );
b3World* world = b3GetUnlockedWorldFromId( worldId );
if ( world == NULL )
{
return (b3JointId){ 0 };
}
b3JointPair pair = b3CreateJoint( world, &def->base, b3_pogoJoint );
b3JointSim* joint = pair.jointSim;
joint->pogoJoint = (b3PogoJoint){ 0 };
joint->pogoJoint.normal = def->normal;
joint->pogoJoint.restLength = def->restLength;
joint->pogoJoint.hertz = def->hertz;
joint->pogoJoint.dampingRatio = def->dampingRatio;
joint->pogoJoint.maxTensionForce = def->maxTensionForce;
joint->pogoJoint.maxCompressionForce = def->maxCompressionForce;
joint->pogoJoint.impulse = def->impulse;
joint->pogoJoint.velocity = def->velocity;
b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation };
B3_REC_CREATE( world, CreatePogoJoint, jointId, worldId, *def );
return jointId;
}
b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def )
{
B3_CHECK_JOINT_DEF( def );
@ -1120,6 +1195,20 @@ void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, flo
}
break;
case b3_moverJoint:
{
b3MoverJoint* joint = &sim->moverJoint;
linearImpulse = b3Length( joint->linearVelocityImpulse );
}
break;
case b3_pogoJoint:
{
b3PogoJoint* joint = &sim->pogoJoint;
linearImpulse = b3AbsFloat( joint->impulse );
}
break;
default:
break;
}
@ -1161,6 +1250,12 @@ static b3Vec3 b3GetJointConstraintForce( b3World* world, b3Joint* joint )
case b3_wheelJoint:
return b3GetWheelJointForce( world, base );
case b3_moverJoint:
return b3GetMoverJointForce( world, base );
case b3_pogoJoint:
return b3GetPogoJointForce( world, base );
default:
B3_ASSERT( false );
return b3Vec3_zero;
@ -1200,6 +1295,10 @@ static b3Vec3 b3GetJointConstraintTorque( b3World* world, b3Joint* joint )
case b3_wheelJoint:
return b3GetWheelJointTorque( world, base );
case b3_moverJoint:
case b3_pogoJoint:
return b3Vec3_zero;
default:
B3_ASSERT( false );
return b3Vec3_zero;
@ -1268,6 +1367,12 @@ float b3Joint_GetLinearSeparation( b3JointId jointId )
case b3_motorJoint:
return 0.0f;
case b3_moverJoint:
return 0.0f;
case b3_pogoJoint:
return 0.0f;
case b3_filterJoint:
return 0.0f;
@ -1370,6 +1475,12 @@ float b3Joint_GetAngularSeparation( b3JointId jointId )
case b3_motorJoint:
return 0.0f;
case b3_moverJoint:
return 0.0f;
case b3_pogoJoint:
return 0.0f;
case b3_filterJoint:
return 0.0f;
@ -1522,6 +1633,14 @@ void b3PrepareJoint( b3JointSim* joint, b3StepContext* context )
b3PrepareWheelJoint( joint, context );
break;
case b3_moverJoint:
b3PrepareMoverJoint( joint, context );
break;
case b3_pogoJoint:
b3PreparePogoJoint( joint, context );
break;
default:
B3_ASSERT( false );
}
@ -1566,6 +1685,14 @@ void b3WarmStartJoint( b3JointSim* joint, b3StepContext* context )
b3WarmStartWheelJoint( joint, context );
break;
case b3_moverJoint:
b3WarmStartMoverJoint( joint, context );
break;
case b3_pogoJoint:
b3WarmStartPogoJoint( joint, context );
break;
default:
B3_ASSERT( false );
}
@ -1612,6 +1739,14 @@ void b3SolveJoint( b3JointSim* joint, b3StepContext* context, bool useBias )
b3SolveWheelJoint( joint, context, useBias );
break;
case b3_moverJoint:
b3SolveMoverJoint( joint, context );
break;
case b3_pogoJoint:
b3SolvePogoJoint( joint, context, useBias );
break;
default:
B3_ASSERT( false );
}
@ -1728,6 +1863,14 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint )
b3DrawWheelJoint( draw, jointSim, transformA, transformB, scale );
break;
case b3_moverJoint:
draw->DrawPointFcn( pB, 8.0f, b3_colorPlum, draw->context );
break;
case b3_pogoJoint:
draw->DrawSegmentFcn( pA, pB, b3_colorLightGray, draw->context );
break;
default:
draw->DrawSegmentFcn( transformA.p, pA, color, draw->context );
draw->DrawSegmentFcn( pA, pB, color, draw->context );

View file

@ -121,6 +121,37 @@ typedef struct b3MotorJoint
b3Matrix3 angularMass;
} b3MotorJoint;
typedef struct b3MoverJoint
{
b3Vec3 linearVelocity;
b3Vec3 maxVelocityForce;
b3Vec3 linearVelocityImpulse;
int indexA;
int indexB;
float linearMass;
} b3MoverJoint;
typedef struct b3PogoJoint
{
b3Vec3 normal;
float restLength;
float hertz;
float dampingRatio;
float maxTensionForce;
float maxCompressionForce;
float impulse;
int indexA;
int indexB;
b3Transform frameA;
b3Transform frameB;
b3Vec3 deltaCenter;
float linearMass;
float velocity;
} b3PogoJoint;
typedef struct b3ParallelJoint
{
float hertz;
@ -336,7 +367,9 @@ typedef struct b3JointSim
{
b3DistanceJoint distanceJoint;
b3MotorJoint motorJoint;
b3MoverJoint moverJoint;
b3ParallelJoint parallelJoint;
b3PogoJoint pogoJoint;
b3RevoluteJoint revoluteJoint;
b3SphericalJoint sphericalJoint;
b3PrismaticJoint prismaticJoint;
@ -365,6 +398,8 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint );
b3Vec3 b3GetDistanceJointForce( b3World* world, b3JointSim* base );
b3Vec3 b3GetMotorJointForce( b3World* world, b3JointSim* base );
b3Vec3 b3GetMoverJointForce( b3World* world, b3JointSim* base );
b3Vec3 b3GetPogoJointForce( b3World* world, b3JointSim* base );
b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base );
b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base );
b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base );
@ -381,6 +416,8 @@ b3Vec3 b3GetWheelJointTorque( b3World* world, b3JointSim* base );
void b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context );
void b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context );
void b3PrepareMoverJoint( b3JointSim* base, b3StepContext* context );
void b3PreparePogoJoint( b3JointSim* base, b3StepContext* context );
void b3PrepareParallelJoint( b3JointSim* base, b3StepContext* context );
void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context );
void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context );
@ -390,6 +427,8 @@ void b3PrepareWheelJoint( b3JointSim* base, b3StepContext* context );
void b3WarmStartDistanceJoint( b3JointSim* base, b3StepContext* context );
void b3WarmStartMotorJoint( b3JointSim* base, b3StepContext* context );
void b3WarmStartMoverJoint( b3JointSim* base, b3StepContext* context );
void b3WarmStartPogoJoint( b3JointSim* base, b3StepContext* context );
void b3WarmStartParallelJoint( b3JointSim* base, b3StepContext* context );
void b3WarmStartPrismaticJoint( b3JointSim* base, b3StepContext* context );
void b3WarmStartRevoluteJoint( b3JointSim* base, b3StepContext* context );
@ -399,6 +438,8 @@ void b3WarmStartWheelJoint( b3JointSim* base, b3StepContext* context );
void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBias );
void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context );
void b3SolveMoverJoint( b3JointSim* base, b3StepContext* context );
void b3SolvePogoJoint( b3JointSim* base, b3StepContext* context, bool useBias );
void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context );
void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBias );
void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBias );

173
src/mover_joint.c Normal file
View file

@ -0,0 +1,173 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#include "body.h"
#include "core.h"
#include "joint.h"
#include "math_internal.h"
#include "physics_world.h"
#include "recording.h"
#include "solver.h"
#include "solver_set.h"
// needed for dll export
#include "box3d/box3d.h"
void b3MoverJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity )
{
B3_ASSERT( b3IsValidVec3( velocity ) );
b3World* world = b3GetWorld( jointId.world0 );
B3_REC( world, MoverJointSetLinearVelocity, jointId, velocity );
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint );
joint->moverJoint.linearVelocity = velocity;
}
b3Vec3 b3MoverJoint_GetLinearVelocity( b3JointId jointId )
{
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint );
return joint->moverJoint.linearVelocity;
}
void b3MoverJoint_SetMaxVelocityForce( b3JointId jointId, b3Vec3 maxForce )
{
B3_ASSERT( b3IsValidVec3( maxForce ) );
b3World* world = b3GetWorld( jointId.world0 );
B3_REC( world, MoverJointSetMaxVelocityForce, jointId, maxForce );
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint );
joint->moverJoint.maxVelocityForce = maxForce;
}
b3Vec3 b3MoverJoint_GetMaxVelocityForce( b3JointId jointId )
{
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint );
return joint->moverJoint.maxVelocityForce;
}
b3Vec3 b3GetMoverJointForce( b3World* world, b3JointSim* base )
{
return b3MulSV( world->inv_h, base->moverJoint.linearVelocityImpulse );
}
void b3PrepareMoverJoint( b3JointSim* base, b3StepContext* context )
{
B3_ASSERT( base->type == b3_moverJoint );
int idA = base->bodyIdA;
int idB = base->bodyIdB;
b3World* world = context->world;
b3Body* bodyA = b3Array_Get( world->bodies, idA );
b3Body* bodyB = b3Array_Get( world->bodies, idB );
B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex );
b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex );
int localIndexA = bodyA->localIndex;
int localIndexB = bodyB->localIndex;
b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA );
b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB );
float mA = bodySimA->invMass;
float mB = bodySimB->invMass;
base->invMassA = mA;
base->invMassB = mB;
base->invIA = b3Mat3_zero;
base->invIB = b3Mat3_zero;
b3MoverJoint* joint = &base->moverJoint;
joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
float k = mA + mB;
joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f;
if ( context->enableWarmStarting == false )
{
joint->linearVelocityImpulse = b3Vec3_zero;
}
}
void b3WarmStartMoverJoint( b3JointSim* base, b3StepContext* context )
{
B3_ASSERT( base->type == b3_moverJoint );
float mA = base->invMassA;
float mB = base->invMassB;
b3MoverJoint* joint = &base->moverJoint;
// dummy state for static bodies
b3BodyState dummyState = b3_identityBodyState;
b3BodyState* stateA =
joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
b3BodyState* stateB =
joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
if ( stateA->flags & b3_dynamicFlag )
{
stateA->linearVelocity =
b3MulSub( stateA->linearVelocity, mA, joint->linearVelocityImpulse );
}
if ( stateB->flags & b3_dynamicFlag )
{
stateB->linearVelocity =
b3MulAdd( stateB->linearVelocity, mB, joint->linearVelocityImpulse );
}
}
void b3SolveMoverJoint( b3JointSim* base, b3StepContext* context )
{
B3_ASSERT( base->type == b3_moverJoint );
float mA = base->invMassA;
float mB = base->invMassB;
// dummy state for static bodies
b3BodyState dummyState = b3_identityBodyState;
b3MoverJoint* joint = &base->moverJoint;
b3BodyState* stateA =
joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
b3BodyState* stateB =
joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
b3Vec3 vA = stateA->linearVelocity;
b3Vec3 vB = stateB->linearVelocity;
b3Vec3 maxForce = joint->maxVelocityForce;
if ( maxForce.x > 0.0f || maxForce.y > 0.0f || maxForce.z > 0.0f )
{
b3Vec3 cdot = b3Sub( b3Sub( vB, vA ), joint->linearVelocity );
b3Vec3 impulse = b3MulSV( -joint->linearMass, cdot );
b3Vec3 oldImpulse = joint->linearVelocityImpulse;
b3Vec3 maxImpulse = b3MulSV( context->h, maxForce );
joint->linearVelocityImpulse =
b3Clamp( b3Add( oldImpulse, impulse ), b3Neg( maxImpulse ), maxImpulse );
impulse = b3Sub( joint->linearVelocityImpulse, oldImpulse );
vA = b3MulSub( vA, mA, impulse );
vB = b3MulAdd( vB, mB, impulse );
}
else
{
joint->linearVelocityImpulse = b3Vec3_zero;
}
if ( stateA->flags & b3_dynamicFlag )
{
stateA->linearVelocity = vA;
}
if ( stateB->flags & b3_dynamicFlag )
{
stateB->linearVelocity = vB;
}
}

281
src/pogo_joint.c Normal file
View file

@ -0,0 +1,281 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#include "body.h"
#include "core.h"
#include "joint.h"
#include "math_internal.h"
#include "physics_world.h"
#include "recording.h"
#include "solver.h"
#include "solver_set.h"
// needed for dll export
#include "box3d/box3d.h"
void b3PogoJoint_SetRestLength( b3JointId jointId, float length )
{
B3_ASSERT( b3IsValidFloat( length ) && length >= 0.0f );
b3World* world = b3GetWorld( jointId.world0 );
B3_REC( world, PogoJointSetRestLength, jointId, length );
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
joint->pogoJoint.restLength = length;
}
float b3PogoJoint_GetRestLength( b3JointId jointId )
{
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
return joint->pogoJoint.restLength;
}
void b3PogoJoint_SetSpringHertz( b3JointId jointId, float hertz )
{
B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );
b3World* world = b3GetWorld( jointId.world0 );
B3_REC( world, PogoJointSetSpringHertz, jointId, hertz );
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
joint->pogoJoint.hertz = hertz;
}
float b3PogoJoint_GetSpringHertz( b3JointId jointId )
{
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
return joint->pogoJoint.hertz;
}
void b3PogoJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio )
{
B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
b3World* world = b3GetWorld( jointId.world0 );
B3_REC( world, PogoJointSetSpringDampingRatio, jointId, dampingRatio );
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
joint->pogoJoint.dampingRatio = dampingRatio;
}
float b3PogoJoint_GetSpringDampingRatio( b3JointId jointId )
{
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
return joint->pogoJoint.dampingRatio;
}
float b3PogoJoint_GetCurrentLength( b3JointId jointId )
{
b3World* world = b3GetWorld( jointId.world0 );
b3JointSim* base = b3GetJointSimCheckType( jointId, b3_pogoJoint );
b3WorldTransform worldTransformA = b3GetBodyTransform( world, base->bodyIdA );
b3Transform transformA = b3ToRelativeTransform( worldTransformA, worldTransformA.p );
b3Transform transformB =
b3ToRelativeTransform( b3GetBodyTransform( world, base->bodyIdB ), worldTransformA.p );
b3Vec3 axis = b3RotateVector( b3MulQuat( transformB.q, base->localFrameB.q ), b3Vec3_axisY );
b3Vec3 pA = b3TransformPoint( transformA, base->localFrameA.p );
b3Vec3 pB = b3TransformPoint( transformB, base->localFrameB.p );
return b3Dot( b3Sub( pB, pA ), axis );
}
float b3PogoJoint_GetImpulse( b3JointId jointId )
{
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
return joint->pogoJoint.impulse;
}
float b3PogoJoint_GetCurrentSpeed( b3JointId jointId )
{
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
return joint->pogoJoint.velocity;
}
b3Vec3 b3GetPogoJointForce( b3World* world, b3JointSim* base )
{
return b3MulSV( world->inv_h * base->pogoJoint.impulse, base->pogoJoint.normal );
}
void b3PreparePogoJoint( b3JointSim* base, b3StepContext* context )
{
B3_ASSERT( base->type == b3_pogoJoint );
b3World* world = context->world;
// chase body id to the solver set where the body lives
int idA = base->bodyIdA;
int idB = base->bodyIdB;
b3Body* bodyA = b3Array_Get( world->bodies, idA );
b3Body* bodyB = b3Array_Get( world->bodies, idB );
B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex );
b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex );
int localIndexA = bodyA->localIndex;
int localIndexB = bodyB->localIndex;
b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA );
b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB );
float mA = bodySimA->invMass;
float mB = bodySimB->invMass;
b3Matrix3 iA = bodySimA->invInertiaWorld;
b3Matrix3 iB = bodySimB->invInertiaWorld;
base->invMassA = mA;
base->invMassB = mB;
base->invIA = iA;
base->invIB = iB;
b3PogoJoint* joint = &base->pogoJoint;
joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
// Compute joint anchor frames with world space rotation, relative to center of mass
joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q );
joint->frameA.p = b3RotateVector( bodySimA->transform.q,
b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
joint->frameB.p = b3RotateVector( bodySimB->transform.q,
b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
// Compute the initial center delta. Incremental position updates are relative to this.
joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
b3Vec3 rnA = b3Cross( joint->frameA.p, joint->normal );
b3Vec3 rnB = b3Cross( joint->frameB.p, joint->normal );
float k = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) );
joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f;
if ( context->enableWarmStarting == false )
{
joint->impulse = 0.0f;
}
}
void b3WarmStartPogoJoint( b3JointSim* base, b3StepContext* context )
{
B3_ASSERT( base->type == b3_pogoJoint );
float mA = base->invMassA;
float mB = base->invMassB;
b3Matrix3 iA = base->invIA;
b3Matrix3 iB = base->invIB;
b3PogoJoint* joint = &base->pogoJoint;
if ( joint->hertz == 0.0f )
{
joint->impulse = 0.0f;
return;
}
// dummy state for static bodies
b3BodyState dummyState = b3_identityBodyState;
b3BodyState* stateA =
joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
b3BodyState* stateB =
joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
b3Vec3 linearImpulse = b3MulSV( joint->impulse, joint->normal );
if ( stateA->flags & b3_dynamicFlag )
{
stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse );
stateA->angularVelocity =
b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Cross( rA, linearImpulse ) ) );
}
if ( stateB->flags & b3_dynamicFlag )
{
stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, linearImpulse );
stateB->angularVelocity =
b3Add( stateB->angularVelocity, b3MulMV( iB, b3Cross( rB, linearImpulse ) ) );
}
}
void b3SolvePogoJoint( b3JointSim* base, b3StepContext* context, bool useBias )
{
B3_ASSERT( base->type == b3_pogoJoint );
float mA = base->invMassA;
float mB = base->invMassB;
b3Matrix3 iA = base->invIA;
b3Matrix3 iB = base->invIB;
// dummy state for static bodies
b3BodyState dummyState = b3_identityBodyState;
b3PogoJoint* joint = &base->pogoJoint;
if ( joint->hertz == 0.0f )
{
joint->impulse = 0.0f;
return;
}
b3BodyState* stateA =
joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
b3BodyState* stateB =
joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
b3Vec3 vA = stateA->linearVelocity;
b3Vec3 wA = stateA->angularVelocity;
b3Vec3 vB = stateB->linearVelocity;
b3Vec3 wB = stateB->angularVelocity;
b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
float bias = 0.0f;
if ( useBias )
{
// This is a custom pogo position correction designed to avoid feeding velocity into
// the mover body. Otherwise the mover can make huge jumps going up stairs. This still
// works with continuous collision.
b3Vec3 dcA = stateA->deltaPosition;
b3Vec3 dcB = stateB->deltaPosition;
b3Vec3 d = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter );
// The pogo axis is usually fixed as the world y-axis. Different than the ground normal.
b3Vec3 pogoAxis = b3RotateVector( joint->frameB.q, b3Vec3_axisY );
float c = b3Dot( pogoAxis, d ) - joint->restLength;
// This could be divided by dot(joint->normal, pogoAxis) to account for the constraint
// direction, but I'd rather diminish the pogo recovery rate than risk making it huge.
joint->velocity =
b3SpringDamper( joint->hertz, joint->dampingRatio, c, joint->velocity, context->h );
bias = -joint->velocity;
}
b3Vec3 vr = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) );
float cdot = b3Dot( joint->normal, vr );
float maxTensionImpulse = context->h * joint->maxTensionForce;
float maxCompressionImpulse = context->h * joint->maxCompressionForce;
float oldImpulse = joint->impulse;
float impulse = -joint->linearMass * ( cdot + bias );
joint->impulse =
b3ClampFloat( joint->impulse + impulse, -maxTensionImpulse, maxCompressionImpulse );
impulse = joint->impulse - oldImpulse;
b3Vec3 P = b3MulSV( impulse, joint->normal );
vA = b3MulSub( vA, mA, P );
wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) );
vB = b3MulAdd( vB, mB, P );
wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) );
if ( stateA->flags & b3_dynamicFlag )
{
stateA->linearVelocity = vA;
stateA->angularVelocity = wA;
}
if ( stateB->flags & b3_dynamicFlag )
{
stateB->linearVelocity = vB;
stateB->angularVelocity = wB;
}
}

View file

@ -342,6 +342,10 @@ _Static_assert( sizeof( void* ) != 8 || sizeof( b3WeldJointDef ) == 128,
"b3WeldJointDef changed: update b3RecW_WELDJOINTDEF and its reader together" );
_Static_assert( sizeof( void* ) != 8 || sizeof( b3WheelJointDef ) == 184,
"b3WheelJointDef changed: update b3RecW_WHEELJOINTDEF and its reader together" );
_Static_assert( sizeof( void* ) != 8 || sizeof( b3MoverJointDef ) == 136,
"b3MoverJointDef changed: update b3RecW_MOVERJOINTDEF and its reader together" );
_Static_assert( sizeof( void* ) != 8 || sizeof( b3PogoJointDef ) == 152,
"b3PogoJointDef changed: update b3RecW_POGOJOINTDEF and its reader together" );
void b3RecW_EXPLOSIONDEF( b3RecBuffer* buf, b3ExplosionDef v )
{
@ -548,6 +552,26 @@ void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v )
b3RecW_F32( buf, v.upperSteeringLimit );
}
void b3RecW_MOVERJOINTDEF( b3RecBuffer* buf, b3MoverJointDef v )
{
b3RecW_JointBase( buf, &v.base );
b3RecW_VEC3( buf, v.linearVelocity );
b3RecW_VEC3( buf, v.maxVelocityForce );
}
void b3RecW_POGOJOINTDEF( b3RecBuffer* buf, b3PogoJointDef v )
{
b3RecW_JointBase( buf, &v.base );
b3RecW_VEC3( buf, v.normal );
b3RecW_F32( buf, v.hertz );
b3RecW_F32( buf, v.dampingRatio );
b3RecW_F32( buf, v.restLength );
b3RecW_F32( buf, v.maxTensionForce );
b3RecW_F32( buf, v.maxCompressionForce );
b3RecW_F32( buf, v.impulse );
b3RecW_F32( buf, v.velocity );
}
// Query recording. A query collects a variable number of hits through a user callback, so the count
// is not known until the callback stops firing. The record is built in a local buffer with a
// reserved hit-count slot, then committed whole under the lock so concurrent query threads never

View file

@ -52,7 +52,7 @@ typedef struct b3World b3World;
// Minor tracks op-stream additions that keep the 48 byte header shape.
// Minor version 7 made the contact body sim locators persistent
#define B3_REC_VERSION_MINOR 9
#define B3_REC_VERSION_MINOR 10
// File header, fixed 48 bytes. Contains the registry locator so the player
// can load geometry before replaying any ops.
@ -196,6 +196,8 @@ typedef b3RevoluteJointDef b3RecCType_REVOLUTEJOINTDEF;
typedef b3SphericalJointDef b3RecCType_SPHERICALJOINTDEF;
typedef b3WeldJointDef b3RecCType_WELDJOINTDEF;
typedef b3WheelJointDef b3RecCType_WHEELJOINTDEF;
typedef b3MoverJointDef b3RecCType_MOVERJOINTDEF;
typedef b3PogoJointDef b3RecCType_POGOJOINTDEF;
// Codegen pass 1a: arg structs. Generated here so call sites (body.c, shape.c, etc.) can see them.
#define ARG( TAG, field ) b3RecCType_##TAG field;
@ -266,6 +268,8 @@ void b3RecW_REVOLUTEJOINTDEF( b3RecBuffer* buf, b3RevoluteJointDef v );
void b3RecW_SPHERICALJOINTDEF( b3RecBuffer* buf, b3SphericalJointDef v );
void b3RecW_WELDJOINTDEF( b3RecBuffer* buf, b3WeldJointDef v );
void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v );
void b3RecW_MOVERJOINTDEF( b3RecBuffer* buf, b3MoverJointDef v );
void b3RecW_POGOJOINTDEF( b3RecBuffer* buf, b3PogoJointDef v );
// Record framing
void b3RecBeginRecord( b3Recording* rec, uint8_t opcode );

View file

@ -15,6 +15,7 @@
// 0x40-0x4F shape create/destroy
// 0x50-0x6F shape mutators
// 0x80 step
// 0x81-0x8F mover and pogo joints
// 0x90-0xE7 joints (create, generic, per-type)
// 0xE8-0xEF spatial queries
// 0xF0-0xFF markers
@ -211,6 +212,17 @@ B3_REC_OP( 0xE5, WheelJointEnableSteeringLimit, RET_NONE, ARG( JOINTID, joint )
B3_REC_OP( 0xE6, WheelJointSetSteeringLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )
B3_REC_OP( 0xE7, WheelJointSetTargetSteeringAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, radians ) )
// Mover joint
B3_REC_OP( 0x81, CreateMoverJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOVERJOINTDEF, def ) )
B3_REC_OP( 0x82, MoverJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, velocity ) )
B3_REC_OP( 0x83, MoverJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, maxForce ) )
// Pogo joint
B3_REC_OP( 0x84, CreatePogoJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( POGOJOINTDEF, def ) )
B3_REC_OP( 0x85, PogoJointSetRestLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) )
B3_REC_OP( 0x86, PogoJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
B3_REC_OP( 0x87, PogoJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
// Spatial queries. Inputs flow through the manifest (reader side). The hit tail and result are
// hand-written in recording.c / recording_replay.c since they are variable length.
B3_REC_OP( 0xE8, QueryOverlapAABB, RET_NONE, ARG( WORLDID, world ) ARG( AABB, aabb ) ARG( QUERYFILTER, filter ) )

View file

@ -649,6 +649,30 @@ b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr )
return def;
}
b3MoverJointDef b3RecR_MOVERJOINTDEF( b3RecReader* rdr )
{
b3MoverJointDef def = b3DefaultMoverJointDef();
b3RecR_JointBase( rdr, &def.base );
def.linearVelocity = b3RecR_VEC3( rdr );
def.maxVelocityForce = b3RecR_VEC3( rdr );
return def;
}
b3PogoJointDef b3RecR_POGOJOINTDEF( b3RecReader* rdr )
{
b3PogoJointDef def = b3DefaultPogoJointDef();
b3RecR_JointBase( rdr, &def.base );
def.normal = b3RecR_VEC3( rdr );
def.hertz = b3RecR_F32( rdr );
def.dampingRatio = b3RecR_F32( rdr );
def.restLength = b3RecR_F32( rdr );
def.maxTensionForce = b3RecR_F32( rdr );
def.maxCompressionForce = b3RecR_F32( rdr );
def.impulse = b3RecR_F32( rdr );
def.velocity = b3RecR_F32( rdr );
return def;
}
// Outliner body tracking. Defined after the player struct; forward declared here because the
// create/destroy dispatch sits above that struct.
static void b3RecTrackBodyCreate( b3RecPlayer* player, b3BodyId id );
@ -1262,6 +1286,24 @@ static void b3RecDispatch_CreateWheelJoint( const b3RecArgs_CreateWheelJoint* a,
b3RecCheckJointId( rdr, b3CreateWheelJoint( rdr->replayWorldId, &def ), recId );
}
static void b3RecDispatch_CreateMoverJoint( const b3RecArgs_CreateMoverJoint* a, b3RecReader* rdr )
{
b3JointId recId = b3RecR_JOINTID( rdr );
b3MoverJointDef def = a->def;
def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );
def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );
b3RecCheckJointId( rdr, b3CreateMoverJoint( rdr->replayWorldId, &def ), recId );
}
static void b3RecDispatch_CreatePogoJoint( const b3RecArgs_CreatePogoJoint* a, b3RecReader* rdr )
{
b3JointId recId = b3RecR_JOINTID( rdr );
b3PogoJointDef def = a->def;
def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA );
def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB );
b3RecCheckJointId( rdr, b3CreatePogoJoint( rdr->replayWorldId, &def ), recId );
}
static void b3RecDispatch_DestroyJoint( const b3RecArgs_DestroyJoint* a, b3RecReader* rdr )
{
b3DestroyJoint( b3RecMakeJointId( rdr, a->joint ), a->wakeAttached );
@ -1668,6 +1710,36 @@ static void b3RecDispatch_WheelJointSetTargetSteeringAngle( const b3RecArgs_Whee
b3WheelJoint_SetTargetSteeringAngle( b3RecMakeJointId( rdr, a->joint ), a->radians );
}
static void b3RecDispatch_MoverJointSetLinearVelocity(
const b3RecArgs_MoverJointSetLinearVelocity* a, b3RecReader* rdr )
{
b3MoverJoint_SetLinearVelocity( b3RecMakeJointId( rdr, a->joint ), a->velocity );
}
static void b3RecDispatch_MoverJointSetMaxVelocityForce(
const b3RecArgs_MoverJointSetMaxVelocityForce* a, b3RecReader* rdr )
{
b3MoverJoint_SetMaxVelocityForce( b3RecMakeJointId( rdr, a->joint ), a->maxForce );
}
static void b3RecDispatch_PogoJointSetRestLength( const b3RecArgs_PogoJointSetRestLength* a,
b3RecReader* rdr )
{
b3PogoJoint_SetRestLength( b3RecMakeJointId( rdr, a->joint ), a->length );
}
static void b3RecDispatch_PogoJointSetSpringHertz( const b3RecArgs_PogoJointSetSpringHertz* a,
b3RecReader* rdr )
{
b3PogoJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz );
}
static void b3RecDispatch_PogoJointSetSpringDampingRatio(
const b3RecArgs_PogoJointSetSpringDampingRatio* a, b3RecReader* rdr )
{
b3PogoJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );
}
static void b3RecDispatch_StateHash( const b3RecArgs_StateHash* a, b3RecReader* rdr )
{
b3World* world = b3GetWorldFromId( rdr->replayWorldId );

View file

@ -246,6 +246,8 @@ b3RevoluteJointDef b3RecR_REVOLUTEJOINTDEF( b3RecReader* rdr );
b3SphericalJointDef b3RecR_SPHERICALJOINTDEF( b3RecReader* rdr );
b3WeldJointDef b3RecR_WELDJOINTDEF( b3RecReader* rdr );
b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr );
b3MoverJointDef b3RecR_MOVERJOINTDEF( b3RecReader* rdr );
b3PogoJointDef b3RecR_POGOJOINTDEF( b3RecReader* rdr );
b3QueryFilter b3RecR_QUERYFILTER( b3RecReader* rdr );
b3ShapeProxy b3RecR_SHAPEPROXY( b3RecReader* rdr );
b3TreeStats b3RecR_TREESTATS( b3RecReader* rdr );

View file

@ -296,6 +296,102 @@ static int TestMotorJoint( void )
return FinishJoint( jointId, f.worldId );
}
static int TestMoverJoint( void )
{
JointFixture f = CreateJointFixture();
b3MoverJointDef def = b3DefaultMoverJointDef();
SetCommonFrames( &def.base, &f );
b3JointId jointId = b3CreateMoverJoint( f.worldId, &def );
if ( ExerciseJointBase( jointId, f.worldId, f.groundId, f.bodyId, b3_moverJoint ) != 0 )
{
return 1;
}
b3Vec3 velocity = { 1.0f, 2.0f, -3.0f };
b3MoverJoint_SetLinearVelocity( jointId, velocity );
b3Vec3 gotVelocity = b3MoverJoint_GetLinearVelocity( jointId );
ENSURE( gotVelocity.x == 1.0f && gotVelocity.y == 2.0f && gotVelocity.z == -3.0f );
// The y axis has no force budget, so it must stay at rest while x and z reach the target
b3Vec3 maxForce = { 1000.0f, 0.0f, 1000.0f };
b3MoverJoint_SetMaxVelocityForce( jointId, maxForce );
b3Vec3 gotForce = b3MoverJoint_GetMaxVelocityForce( jointId );
ENSURE( gotForce.x == 1000.0f && gotForce.y == 0.0f && gotForce.z == 1000.0f );
for ( int i = 0; i < 30; ++i )
{
b3World_Step( f.worldId, 1.0f / 60.0f, 4 );
}
b3Vec3 v = b3Body_GetLinearVelocity( f.bodyId );
ENSURE_SMALL( v.x - 1.0f, 1.0e-4f );
ENSURE_SMALL( v.y, 1.0e-4f );
ENSURE_SMALL( v.z + 3.0f, 1.0e-4f );
b3Vec3 w = b3Body_GetAngularVelocity( f.bodyId );
ENSURE( w.x == 0.0f && w.y == 0.0f && w.z == 0.0f );
return FinishJoint( jointId, f.worldId );
}
// A box held up by a pogo spring under gravity should settle at the rest length
static int TestPogoJoint( void )
{
JointFixture f = CreateJointFixture();
b3World_SetGravity( f.worldId, (b3Vec3){ 0.0f, -10.0f, 0.0f } );
float restLength = 2.0f;
float mass = b3Body_GetMass( f.bodyId );
b3PogoJointDef def = b3DefaultPogoJointDef();
def.base.bodyIdA = f.groundId;
def.base.bodyIdB = f.bodyId;
def.base.localFrameA.p = (b3Vec3){ 0.0f, 1.0f, 0.0f };
def.base.localFrameB.p = (b3Vec3){ 0.0f, 0.0f, 0.0f };
def.normal = b3Vec3_axisY;
def.restLength = restLength;
def.hertz = 2.0f;
def.dampingRatio = 1.0f;
def.maxCompressionForce = 100.0f * 10.0f * mass;
def.maxTensionForce = 100.0f * 10.0f * mass;
b3JointId jointId = b3CreatePogoJoint( f.worldId, &def );
if ( ExerciseJointBase( jointId, f.worldId, f.groundId, f.bodyId, b3_pogoJoint ) != 0 )
{
return 1;
}
b3PogoJoint_SetRestLength( jointId, restLength );
ENSURE( b3PogoJoint_GetRestLength( jointId ) == restLength );
b3PogoJoint_SetSpringHertz( jointId, 3.0f );
ENSURE( b3PogoJoint_GetSpringHertz( jointId ) == 3.0f );
b3PogoJoint_SetSpringDampingRatio( jointId, 0.9f );
ENSURE( b3PogoJoint_GetSpringDampingRatio( jointId ) == 0.9f );
// Starts stretched by one meter
ENSURE_SMALL( b3PogoJoint_GetCurrentLength( jointId ) - 3.0f, 1.0e-5f );
for ( int i = 0; i < 300; ++i )
{
b3World_Step( f.worldId, 1.0f / 60.0f, 4 );
}
ENSURE_SMALL( b3PogoJoint_GetCurrentLength( jointId ) - restLength, 0.01f );
ENSURE_SMALL( b3Body_GetLinearVelocity( f.bodyId ).y, 0.01f );
// Holding the weight takes an impulse of m * g * h each sub-step
ENSURE_SMALL( b3PogoJoint_GetImpulse( jointId ) - mass * 10.0f / 240.0f, 0.001f * mass );
b3Vec3 force = b3Joint_GetConstraintForce( jointId );
ENSURE_SMALL( force.y - mass * 10.0f, 0.1f * mass );
return FinishJoint( jointId, f.worldId );
}
static int TestPrismaticJoint( void )
{
JointFixture f = CreateJointFixture();
@ -673,6 +769,8 @@ int JointTest( void )
RUN_SUBTEST( TestDistanceJoint );
RUN_SUBTEST( TestFilterJoint );
RUN_SUBTEST( TestMotorJoint );
RUN_SUBTEST( TestMoverJoint );
RUN_SUBTEST( TestPogoJoint );
RUN_SUBTEST( TestPrismaticJoint );
RUN_SUBTEST( TestRevoluteJoint );
RUN_SUBTEST( TestSphericalJoint );

View file

@ -1569,6 +1569,43 @@ static int AllOps( void )
b3ParallelJoint_SetSpringDampingRatio( parallelId, 0.5f );
b3ParallelJoint_SetMaxTorque( parallelId, 20.0f );
// Dynamic mover: a rotation locked capsule driven by a mover joint and held up by a pogo joint
b3BodyDef moverBodyDef = b3DefaultBodyDef();
moverBodyDef.type = b3_dynamicBody;
moverBodyDef.position = (b3Pos){ -20.0f, 3.0f, 0.0f };
moverBodyDef.motionLocks =
(b3MotionLocks){ .angularX = true, .angularY = true, .angularZ = true };
b3BodyId moverBodyId = b3CreateBody( worldId, &moverBodyDef );
b3ShapeDef moverShapeDef = b3DefaultShapeDef();
moverShapeDef.density = 1.0f;
moverShapeDef.baseMaterial.friction = 0.0f;
b3Capsule moverCapsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f };
b3CreateCapsuleShape( moverBodyId, &moverShapeDef, &moverCapsule );
b3MoverJointDef moverDef = b3DefaultMoverJointDef();
moverDef.base.bodyIdA = groundId;
moverDef.base.bodyIdB = moverBodyId;
moverDef.base.collideConnected = true;
moverDef.linearVelocity = (b3Vec3){ 1.0f, 0.0f, 0.5f };
moverDef.maxVelocityForce = (b3Vec3){ 0.5f, 0.0f, 0.5f };
b3JointId moverJointId = b3CreateMoverJoint( worldId, &moverDef );
b3PogoJointDef pogoDef = b3DefaultPogoJointDef();
pogoDef.base.bodyIdA = groundId;
pogoDef.base.bodyIdB = moverBodyId;
pogoDef.base.localFrameA.p = (b3Vec3){ -20.0f, 1.0f, 0.0f };
pogoDef.base.localFrameB.p = moverCapsule.center1;
pogoDef.base.collideConnected = true;
pogoDef.restLength = 1.0f;
pogoDef.hertz = 5.0f;
pogoDef.dampingRatio = 0.8f;
pogoDef.maxCompressionForce = 1000.0f;
pogoDef.maxTensionForce = 1000.0f;
b3JointId pogoJointId = b3CreatePogoJoint( worldId, &pogoDef );
b3PogoJoint_SetRestLength( pogoJointId, 0.9f );
b3PogoJoint_SetSpringHertz( pogoJointId, 4.0f );
b3PogoJoint_SetSpringDampingRatio( pogoJointId, 0.7f );
// World config mutators
b3World_SetGravity( worldId, (b3Vec3){ 0.0f, -9.8f, 0.0f } );
b3World_EnableSleeping( worldId, true );
@ -1632,8 +1669,21 @@ static int AllOps( void )
b3Body_ApplyLinearImpulseToCenter( capsuleBodyId, (b3Vec3){ 2.0f, 0.0f, 0.0f }, true );
b3Body_SetGravityScale( bodyId, 1.0f );
b3Body_SetSafetyFactor( fastId, 0.4f );
b3MoverJoint_SetMaxVelocityForce( moverJointId, (b3Vec3){ 60.0f, 0.0f, 60.0f } );
}
// The dynamic mover rebuilds its pogo every step, warm started from the old one
if ( i >= 6 )
{
pogoDef.impulse = b3PogoJoint_GetImpulse( pogoJointId );
pogoDef.velocity = b3PogoJoint_GetCurrentSpeed( pogoJointId );
pogoDef.base.localFrameA.p =
b3Body_GetLocalPoint( groundId, b3Body_GetPosition( moverBodyId ) );
pogoDef.base.localFrameA.p.y = 1.0f;
b3DestroyJoint( pogoJointId, false );
pogoJointId = b3CreatePogoJoint( worldId, &pogoDef );
b3MoverJoint_SetLinearVelocity( moverJointId, (b3Vec3){ -0.5f, 0.0f, 1.0f } );
}
// Issue queries mid-loop to exercise recording across steps
if ( i == 3 )
{