From f0b1ed2fca31ad0ae0e8db2257572e28286a1601 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Tue, 20 Oct 2026 23:33:18 -0700 Subject: [PATCH] dynamic mover --- include/box3d/box3d.h | 71 +++ include/box3d/math_functions.h | 14 + include/box3d/types.h | 59 +++ samples/CMakeLists.txt | 6 +- samples/dynamic_mover.cpp | 505 +++++++++++++++++++++ samples/dynamic_mover.h | 161 +++++++ samples/geometric_mover.cpp | 351 +++++++++++++++ samples/geometric_mover.h | 114 +++++ samples/gfx/keycodes.h | 3 + samples/main.cpp | 15 +- samples/mover.cpp | 353 --------------- samples/mover.h | 53 --- samples/sample.cpp | 45 ++ samples/sample.h | 15 + samples/sample_character.cpp | 801 ++++++++++++++++++++++++++++++++- samples/sample_compound.cpp | 36 +- samples/sample_replay.cpp | 4 + src/CMakeLists.txt | 2 + src/joint.c | 143 ++++++ src/joint.h | 41 ++ src/mover_joint.c | 173 +++++++ src/pogo_joint.c | 281 ++++++++++++ src/recording.c | 24 + src/recording.h | 6 +- src/recording_ops.inl | 12 + src/replay.c | 72 +++ src/replay.h | 2 + test/test_joint.c | 98 ++++ test/test_recording.c | 50 ++ 29 files changed, 3078 insertions(+), 432 deletions(-) create mode 100644 samples/dynamic_mover.cpp create mode 100644 samples/dynamic_mover.h create mode 100644 samples/geometric_mover.cpp create mode 100644 samples/geometric_mover.h delete mode 100644 samples/mover.cpp delete mode 100644 samples/mover.h create mode 100644 src/mover_joint.c create mode 100644 src/pogo_joint.c diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index 54d0baf..cdf085a 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -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. diff --git a/include/box3d/math_functions.h b/include/box3d/math_functions.h index a12f250..8f81d93 100644 --- a/include/box3d/math_functions.h +++ b/include/box3d/math_functions.h @@ -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 { diff --git a/include/box3d/types.h b/include/box3d/types.h index 2cbbbeb..cf24480 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -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 diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 257de2e..d9d29de 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -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 diff --git a/samples/dynamic_mover.cpp b/samples/dynamic_mover.cpp new file mode 100644 index 0000000..6386253 --- /dev/null +++ b/samples/dynamic_mover.cpp @@ -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 + +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 ) ); +} diff --git a/samples/dynamic_mover.h b/samples/dynamic_mover.h new file mode 100644 index 0000000..88a959c --- /dev/null +++ b/samples/dynamic_mover.h @@ -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 ); +}; diff --git a/samples/geometric_mover.cpp b/samples/geometric_mover.cpp new file mode 100644 index 0000000..3d6a038 --- /dev/null +++ b/samples/geometric_mover.cpp @@ -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 +#include + +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( context ); + float maxPush = FLT_MAX; + bool clipVelocity = true; + MoverShapeUserData* userData = + static_cast( 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( 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 ) ); +} diff --git a/samples/geometric_mover.h b/samples/geometric_mover.h new file mode 100644 index 0000000..e8adac1 --- /dev/null +++ b/samples/geometric_mover.h @@ -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(); +}; diff --git a/samples/gfx/keycodes.h b/samples/gfx/keycodes.h index a2a62c8..e238fb8 100644 --- a/samples/gfx/keycodes.h +++ b/samples/gfx/keycodes.h @@ -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, diff --git a/samples/main.cpp b/samples/main.cpp index a92df26..84d373d 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -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 diff --git a/samples/mover.cpp b/samples/mover.cpp deleted file mode 100644 index 4db9a13..0000000 --- a/samples/mover.cpp +++ /dev/null @@ -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 -#include - -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( context ); - float maxPush = FLT_MAX; - bool clipVelocity = true; - MoverShapeUserData* userData = static_cast( (void*)b3Shape_GetUserData( shapeId ) ); - if ( userData != nullptr ) - { - maxPush = userData->maxPush; - clipVelocity = userData->clipVelocity; - } - - for ( int i = 0; i < planeCount && self->m_planeCount < CharacterMover::m_planeCapacity; ++i ) - { - assert( b3IsValidPlane( planeResults[i].plane ) ); - self->m_planes[self->m_planeCount] = { - .plane = planeResults[i].plane, - .pushLimit = maxPush, - .push = 0.0f, - .clipVelocity = clipVelocity, - }; - self->m_planeExtras[self->m_planeCount] = { - .point = 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( 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; -} diff --git a/samples/mover.h b/samples/mover.h deleted file mode 100644 index 724b59b..0000000 --- a/samples/mover.h +++ /dev/null @@ -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; -}; diff --git a/samples/sample.cpp b/samples/sample.cpp index 5abea6b..a299724 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -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 ) { diff --git a/samples/sample.h b/samples/sample.h index 6d09f41..f9a8466 100644 --- a/samples/sample.h +++ b/samples/sample.h @@ -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(); diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index 77b1c86..6700430 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -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 { diff --git a/samples/sample_compound.cpp b/samples/sample_compound.cpp index 363dd37..e0e7694 100644 --- a/samples/sample_compound.cpp +++ b/samples/sample_compound.cpp @@ -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; }; diff --git a/samples/sample_replay.cpp b/samples/sample_replay.cpp index 0a363fb..4879c1d 100644 --- a/samples/sample_replay.cpp +++ b/samples/sample_replay.cpp @@ -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 "?"; } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index aa65d79..eaa22b7 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -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 diff --git a/src/joint.c b/src/joint.c index 068a29d..aa48179 100644 --- a/src/joint.c +++ b/src/joint.c @@ -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 ); diff --git a/src/joint.h b/src/joint.h index 9a8c347..e633f84 100644 --- a/src/joint.h +++ b/src/joint.h @@ -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 ); diff --git a/src/mover_joint.c b/src/mover_joint.c new file mode 100644 index 0000000..8407450 --- /dev/null +++ b/src/mover_joint.c @@ -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; + } +} diff --git a/src/pogo_joint.c b/src/pogo_joint.c new file mode 100644 index 0000000..5306632 --- /dev/null +++ b/src/pogo_joint.c @@ -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; + } +} diff --git a/src/recording.c b/src/recording.c index 7e9bc03..8eb4f01 100644 --- a/src/recording.c +++ b/src/recording.c @@ -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 diff --git a/src/recording.h b/src/recording.h index 6f9df4d..1afd800 100644 --- a/src/recording.h +++ b/src/recording.h @@ -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 ); diff --git a/src/recording_ops.inl b/src/recording_ops.inl index ed4b7d6..9905ebd 100644 --- a/src/recording_ops.inl +++ b/src/recording_ops.inl @@ -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 ) ) diff --git a/src/replay.c b/src/replay.c index 38bda1a..20d0c60 100644 --- a/src/replay.c +++ b/src/replay.c @@ -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 ); diff --git a/src/replay.h b/src/replay.h index ea2bae8..8aeddd9 100644 --- a/src/replay.h +++ b/src/replay.h @@ -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 ); diff --git a/test/test_joint.c b/test/test_joint.c index 4e38d75..9c196ca 100644 --- a/test/test_joint.c +++ b/test/test_joint.c @@ -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 ); diff --git a/test/test_recording.c b/test/test_recording.c index 12f7316..663b9a0 100644 --- a/test/test_recording.c +++ b/test/test_recording.c @@ -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 ) {