dynamic mover
This commit is contained in:
parent
f36d061ba2
commit
f0b1ed2fca
29 changed files with 3078 additions and 432 deletions
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -169,8 +169,10 @@ set(SAMPLE_FILES
|
|||
sample.h
|
||||
mesh_loader.cpp
|
||||
mesh_loader.h
|
||||
mover.cpp
|
||||
mover.h
|
||||
dynamic_mover.cpp
|
||||
dynamic_mover.h
|
||||
geometric_mover.cpp
|
||||
geometric_mover.h
|
||||
jsmn.h
|
||||
tiny_obj_loader.h
|
||||
earcut.h
|
||||
|
|
|
|||
505
samples/dynamic_mover.cpp
Normal file
505
samples/dynamic_mover.cpp
Normal file
|
|
@ -0,0 +1,505 @@
|
|||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "dynamic_mover.h"
|
||||
|
||||
#include "gfx/draw.h"
|
||||
|
||||
#include "box3d/box3d.h"
|
||||
#include "box3d/math_functions.h"
|
||||
|
||||
#include <float.h>
|
||||
|
||||
struct CastContext
|
||||
{
|
||||
DynamicMoverCastResult result;
|
||||
b3BodyId moverId;
|
||||
};
|
||||
|
||||
static float CastCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction,
|
||||
uint64_t userMaterialId, int triangleIndex, int childIndex,
|
||||
void* context )
|
||||
{
|
||||
(void)triangleIndex;
|
||||
(void)childIndex;
|
||||
|
||||
CastContext* castContext = (CastContext*)context;
|
||||
b3BodyId bodyId = b3Shape_GetBody( shapeId );
|
||||
|
||||
// The probes start inside or on the capsule. Sensors can't hold up the mover.
|
||||
if ( B3_ID_EQUALS( bodyId, castContext->moverId ) || b3Shape_IsSensor( shapeId ) )
|
||||
{
|
||||
return -1.0f;
|
||||
}
|
||||
|
||||
DynamicMoverCastResult* result = &castContext->result;
|
||||
result->point = point;
|
||||
result->normal = normal;
|
||||
result->bodyId = bodyId;
|
||||
result->userMaterialId = userMaterialId;
|
||||
result->fraction = fraction;
|
||||
result->hit = true;
|
||||
return fraction;
|
||||
}
|
||||
|
||||
DynamicMover::DynamicMover()
|
||||
{
|
||||
DynamicMoverDef def;
|
||||
m_jumpSpeed = def.jumpSpeed;
|
||||
m_maxSpeed = def.maxSpeed;
|
||||
m_minSpeed = def.minSpeed;
|
||||
m_stopSpeed = def.stopSpeed;
|
||||
m_accelerate = def.accelerate;
|
||||
m_airSteer = def.airSteer;
|
||||
m_friction = def.friction;
|
||||
m_gravityScale = def.gravityScale;
|
||||
m_maxGroundForce = def.maxGroundForce;
|
||||
m_maxAirForce = def.maxAirForce;
|
||||
m_pogoRestLength = def.pogoRestLength;
|
||||
m_pogoHertz = def.pogoHertz;
|
||||
m_pogoDampingRatio = def.pogoDampingRatio;
|
||||
m_pogoCompressionScale = def.pogoCompressionScale;
|
||||
m_pogoTensionScale = def.pogoTensionScale;
|
||||
m_enableSkirtPogo = def.enableSkirtPogo;
|
||||
m_platformMaterialId = def.platformMaterialId;
|
||||
m_minGroundNormalY = 0.7f;
|
||||
|
||||
m_capsule = def.capsule;
|
||||
m_filter = def.filter;
|
||||
|
||||
m_worldId = b3_nullWorldId;
|
||||
m_groundId = b3_nullBodyId;
|
||||
m_moverId = b3_nullBodyId;
|
||||
m_moverJointId = b3_nullJointId;
|
||||
m_pogoJointId = b3_nullJointId;
|
||||
|
||||
m_castResult = {};
|
||||
|
||||
m_velocity = b3Vec3_zero;
|
||||
m_onGround = false;
|
||||
m_walkable = false;
|
||||
m_jumping = false;
|
||||
m_jumpTicks = 0;
|
||||
|
||||
m_pogoImpulse = 0.0f;
|
||||
m_pogoVelocity = 0.0f;
|
||||
m_pogoLength = 0.0f;
|
||||
|
||||
for ( DynamicMoverProbe& probe : m_probes )
|
||||
{
|
||||
probe = { b3Pos_zero, b3Vec3_zero, 1.0f, false };
|
||||
}
|
||||
|
||||
m_probeCount = 0;
|
||||
m_selectedProbe = 0;
|
||||
}
|
||||
|
||||
void DynamicMover::Create( b3WorldId worldId, const DynamicMoverDef* def )
|
||||
{
|
||||
m_capsule = def->capsule;
|
||||
m_filter = def->filter;
|
||||
m_jumpSpeed = def->jumpSpeed;
|
||||
m_maxSpeed = def->maxSpeed;
|
||||
m_minSpeed = def->minSpeed;
|
||||
m_stopSpeed = def->stopSpeed;
|
||||
m_accelerate = def->accelerate;
|
||||
m_airSteer = def->airSteer;
|
||||
m_friction = def->friction;
|
||||
m_gravityScale = def->gravityScale;
|
||||
m_maxGroundForce = def->maxGroundForce;
|
||||
m_maxAirForce = def->maxAirForce;
|
||||
m_pogoRestLength = def->pogoRestLength;
|
||||
m_pogoHertz = def->pogoHertz;
|
||||
m_pogoDampingRatio = def->pogoDampingRatio;
|
||||
m_pogoCompressionScale = def->pogoCompressionScale;
|
||||
m_pogoTensionScale = def->pogoTensionScale;
|
||||
m_enableSkirtPogo = def->enableSkirtPogo;
|
||||
m_platformMaterialId = def->platformMaterialId;
|
||||
|
||||
m_worldId = worldId;
|
||||
m_velocity = b3Vec3_zero;
|
||||
m_onGround = false;
|
||||
m_walkable = false;
|
||||
m_jumping = false;
|
||||
m_jumpTicks = 0;
|
||||
m_pogoImpulse = 0.0f;
|
||||
m_pogoVelocity = 0.0f;
|
||||
m_pogoLength = 0.0f;
|
||||
m_pogoJointId = b3_nullJointId;
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
m_groundId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
// Position is the center of the capsule.
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = def->position;
|
||||
bodyDef.gravityScale = m_gravityScale;
|
||||
bodyDef.motionLocks.angularX = true;
|
||||
bodyDef.motionLocks.angularY = true;
|
||||
bodyDef.motionLocks.angularZ = true;
|
||||
bodyDef.enableSleep = false;
|
||||
bodyDef.name = "mover";
|
||||
|
||||
m_moverId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.density = def->density;
|
||||
|
||||
// The mover joint and the pogo joint handle all the movement, surface friction would fight them
|
||||
shapeDef.baseMaterial.friction = 0.0f;
|
||||
shapeDef.baseMaterial.restitution = 0.0f;
|
||||
shapeDef.filter = def->filter;
|
||||
shapeDef.enablePreSolveEvents = def->enablePreSolveEvents;
|
||||
shapeDef.enableSensorEvents = def->enableSensorEvents;
|
||||
|
||||
b3CreateCapsuleShape( m_moverId, &shapeDef, &m_capsule );
|
||||
|
||||
CreateMoverJoint( m_groundId, { m_maxGroundForce, 0.0f, m_maxGroundForce } );
|
||||
}
|
||||
|
||||
void DynamicMover::CreateMoverJoint( b3BodyId baseId, b3Vec3 maxForce )
|
||||
{
|
||||
b3MoverJointDef moverDef = b3DefaultMoverJointDef();
|
||||
moverDef.linearVelocity = m_velocity;
|
||||
moverDef.maxVelocityForce = maxForce;
|
||||
moverDef.base.bodyIdA = baseId;
|
||||
moverDef.base.bodyIdB = m_moverId;
|
||||
moverDef.base.collideConnected = true;
|
||||
|
||||
m_moverJointId = b3CreateMoverJoint( m_worldId, &moverDef );
|
||||
}
|
||||
|
||||
void DynamicMover::Destroy()
|
||||
{
|
||||
if ( B3_IS_NON_NULL( m_moverId ) )
|
||||
{
|
||||
b3DestroyBody( m_moverId );
|
||||
}
|
||||
|
||||
if ( B3_IS_NON_NULL( m_groundId ) )
|
||||
{
|
||||
b3DestroyBody( m_groundId );
|
||||
}
|
||||
|
||||
m_groundId = b3_nullBodyId;
|
||||
m_moverId = b3_nullBodyId;
|
||||
|
||||
// Joints are implicitly destroyed.
|
||||
m_moverJointId = b3_nullJointId;
|
||||
m_pogoJointId = b3_nullJointId;
|
||||
}
|
||||
|
||||
void DynamicMover::SetGravityScale( float gravityScale )
|
||||
{
|
||||
m_gravityScale = gravityScale;
|
||||
|
||||
if ( B3_IS_NON_NULL( m_moverId ) )
|
||||
{
|
||||
b3Body_SetGravityScale( m_moverId, gravityScale );
|
||||
}
|
||||
}
|
||||
|
||||
bool DynamicMover::Jump()
|
||||
{
|
||||
if ( m_onGround == false || m_walkable == false )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
float surfaceVelocity = 0.0f;
|
||||
if ( b3Body_IsValid( m_castResult.bodyId ) )
|
||||
{
|
||||
b3Vec3 v = b3Body_GetWorldPointVelocity( m_castResult.bodyId, m_castResult.point );
|
||||
surfaceVelocity = v.y;
|
||||
}
|
||||
|
||||
float mass = b3Body_GetMass( m_moverId );
|
||||
float vy = b3Body_GetLinearVelocity( m_moverId ).y;
|
||||
|
||||
// Remove the pogo constraint velocity but add the surface velocity.
|
||||
float dv = b3MaxFloat( 0.0f, m_jumpSpeed - vy ) + surfaceVelocity;
|
||||
b3Body_ApplyLinearImpulseToCenter( m_moverId, { 0.0f, mass * dv, 0.0f }, true );
|
||||
|
||||
Launch();
|
||||
return true;
|
||||
}
|
||||
|
||||
void DynamicMover::Launch()
|
||||
{
|
||||
// This removes the pogo step down on the next update.
|
||||
m_onGround = false;
|
||||
m_walkable = false;
|
||||
m_jumping = true;
|
||||
m_jumpTicks = 0;
|
||||
}
|
||||
|
||||
// Movement follows the Quake ground and air model:
|
||||
// https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390
|
||||
|
||||
// onGround - controls friction, mover max force, and pogo step-down extension
|
||||
// walkable - controls air steer
|
||||
// jumping - blocks the pogo from pulling down
|
||||
void DynamicMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle,
|
||||
float speedScale )
|
||||
{
|
||||
// Reach further while grounded so the spring can find the ground over a step
|
||||
float stepDownLength = m_pogoRestLength;
|
||||
float rayLength = m_onGround ? m_pogoRestLength + stepDownLength : m_pogoRestLength;
|
||||
b3Vec3 translation = { 0.0f, -rayLength, 0.0f };
|
||||
|
||||
b3Pos position = b3Body_GetPosition( m_moverId );
|
||||
b3Pos origin = position + m_capsule.center1;
|
||||
|
||||
b3QueryFilter queryFilter = {
|
||||
.categoryBits = m_filter.categoryBits,
|
||||
.maskBits = m_filter.maskBits,
|
||||
.id = 0,
|
||||
.name = "dynamic_mover_pogo",
|
||||
};
|
||||
|
||||
CastContext castContext = { {}, m_moverId };
|
||||
b3World_CastRay( m_worldId, origin, translation, queryFilter, CastCallback, &castContext );
|
||||
m_castResult = castContext.result;
|
||||
|
||||
m_probes[0] = { origin, translation, m_castResult.hit ? m_castResult.fraction : 1.0f,
|
||||
m_castResult.hit };
|
||||
m_probeCount = 1;
|
||||
m_selectedProbe = 0;
|
||||
|
||||
b3Vec3 pogoOffset = b3Vec3_zero;
|
||||
|
||||
bool centerWalkable = m_castResult.hit && m_castResult.normal.y >= m_minGroundNormalY;
|
||||
if ( centerWalkable == false && m_enableSkirtPogo )
|
||||
{
|
||||
// The skirt turns with the direction of travel so ties go to the leading probe
|
||||
b3Vec3 moverVelocity = b3Body_GetLinearVelocity( m_moverId );
|
||||
b3Vec3 heading = { moverVelocity.x, 0.0f, moverVelocity.z };
|
||||
if ( b3LengthSquared( heading ) < FLT_EPSILON )
|
||||
{
|
||||
heading = b3Vec3_axisX;
|
||||
}
|
||||
|
||||
heading = b3Normalize( heading );
|
||||
b3Vec3 side = { -heading.z, 0.0f, heading.x };
|
||||
|
||||
float bestDisplacement = FLT_MAX;
|
||||
for ( int i = 0; i < m_skirtCount; ++i )
|
||||
{
|
||||
b3CosSin cs = b3ComputeCosSin( 2.0f * B3_PI * i / m_skirtCount );
|
||||
b3Vec3 offset = m_capsule.radius * ( cs.cosine * heading + cs.sine * side );
|
||||
|
||||
b3Pos skirtOrigin = origin + offset;
|
||||
CastContext skirtContext = { {}, m_moverId };
|
||||
b3World_CastRay( m_worldId, skirtOrigin, translation, queryFilter, CastCallback,
|
||||
&skirtContext );
|
||||
const DynamicMoverCastResult& result = skirtContext.result;
|
||||
|
||||
m_probes[m_probeCount] = { skirtOrigin, translation,
|
||||
result.hit ? result.fraction : 1.0f, result.hit };
|
||||
|
||||
// Use the walkable result that causes the least movement. With none the mover keeps
|
||||
// the central hit and slides off the steep surface.
|
||||
if ( result.hit && result.normal.y >= m_minGroundNormalY )
|
||||
{
|
||||
float displacement = b3AbsFloat( result.fraction * rayLength - m_pogoRestLength );
|
||||
if ( displacement < bestDisplacement )
|
||||
{
|
||||
bestDisplacement = displacement;
|
||||
m_castResult = result;
|
||||
m_selectedProbe = m_probeCount;
|
||||
pogoOffset = offset;
|
||||
}
|
||||
}
|
||||
|
||||
m_probeCount += 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Should jumping end?
|
||||
if ( m_jumping )
|
||||
{
|
||||
m_jumpTicks += 1;
|
||||
|
||||
// Jump ticks allow time for the jump to leave the ground. Otherwise jumping ends
|
||||
// when the character approaches the current ground.
|
||||
if ( m_jumpTicks > 2 && m_castResult.hit &&
|
||||
b3Dot( m_velocity, m_castResult.normal ) <= 0.0f )
|
||||
{
|
||||
m_jumping = false;
|
||||
}
|
||||
}
|
||||
|
||||
if ( m_castResult.hit )
|
||||
{
|
||||
// The cast can still hit while jumping.
|
||||
m_onGround = m_jumping == false;
|
||||
|
||||
// Is the ground too steep to walk?
|
||||
m_walkable = m_castResult.normal.y >= m_minGroundNormalY;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_onGround = false;
|
||||
m_walkable = false;
|
||||
}
|
||||
|
||||
// Connect the mover joint to platform. Acts like friction.
|
||||
b3BodyId baseId = m_groundId;
|
||||
if ( m_castResult.hit && m_platformMaterialId != 0 &&
|
||||
m_castResult.userMaterialId == m_platformMaterialId &&
|
||||
b3Body_GetType( m_castResult.bodyId ) == b3_kinematicBody )
|
||||
{
|
||||
baseId = m_castResult.bodyId;
|
||||
}
|
||||
|
||||
if ( B3_ID_EQUALS( b3Joint_GetBodyA( m_moverJointId ), baseId ) == false )
|
||||
{
|
||||
b3Vec3 maxForce = b3MoverJoint_GetMaxVelocityForce( m_moverJointId );
|
||||
b3DestroyJoint( m_moverJointId, false );
|
||||
CreateMoverJoint( baseId, maxForce );
|
||||
}
|
||||
|
||||
m_velocity = b3Body_GetLinearVelocity( m_moverId ) - b3Body_GetLinearVelocity( baseId );
|
||||
|
||||
// Friction acts in the horizontal plane. The pogo owns the vertical motion.
|
||||
if ( m_onGround )
|
||||
{
|
||||
float speed = sqrtf( m_velocity.x * m_velocity.x + m_velocity.z * m_velocity.z );
|
||||
if ( speed < m_minSpeed )
|
||||
{
|
||||
m_velocity.x = 0.0f;
|
||||
m_velocity.z = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Linear damping above stopSpeed and fixed reduction below stopSpeed
|
||||
float control = speed < m_stopSpeed ? m_stopSpeed : speed;
|
||||
|
||||
// friction has units of 1/time
|
||||
float drop = control * m_friction * timeStep;
|
||||
float newSpeed = b3MaxFloat( 0.0f, speed - drop );
|
||||
float ratio = newSpeed / speed;
|
||||
m_velocity.x *= ratio;
|
||||
m_velocity.z *= ratio;
|
||||
}
|
||||
}
|
||||
|
||||
// Mover force
|
||||
float maxForce;
|
||||
if ( m_onGround )
|
||||
{
|
||||
maxForce = m_walkable ? m_maxGroundForce : 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
maxForce = m_maxAirForce;
|
||||
}
|
||||
|
||||
b3MoverJoint_SetMaxVelocityForce( m_moverJointId, { maxForce, 0.0f, maxForce } );
|
||||
|
||||
float maxSpeed = speedScale * m_maxSpeed;
|
||||
b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right;
|
||||
desiredVelocity.y = 0.0f;
|
||||
|
||||
float desiredSpeed;
|
||||
b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity );
|
||||
|
||||
if ( desiredSpeed > maxSpeed )
|
||||
{
|
||||
desiredSpeed = maxSpeed;
|
||||
}
|
||||
|
||||
// Accelerate
|
||||
float currentSpeed = b3Dot( m_velocity, desiredDirection );
|
||||
float addSpeed = desiredSpeed - currentSpeed;
|
||||
if ( addSpeed > 0.0f )
|
||||
{
|
||||
float steer = m_walkable ? 1.0f : m_airSteer;
|
||||
float accelSpeed = steer * m_accelerate * maxSpeed * timeStep;
|
||||
if ( accelSpeed > addSpeed )
|
||||
{
|
||||
accelSpeed = addSpeed;
|
||||
}
|
||||
|
||||
m_velocity.x += accelSpeed * desiredDirection.x;
|
||||
m_velocity.z += accelSpeed * desiredDirection.z;
|
||||
}
|
||||
|
||||
// The pogo joint is rebuilt every update because it may land on a different body
|
||||
if ( b3Joint_IsValid( m_pogoJointId ) )
|
||||
{
|
||||
m_pogoLength = b3PogoJoint_GetCurrentLength( m_pogoJointId );
|
||||
m_pogoImpulse = b3PogoJoint_GetImpulse( m_pogoJointId );
|
||||
m_pogoVelocity = b3PogoJoint_GetCurrentSpeed( m_pogoJointId );
|
||||
|
||||
b3DestroyJoint( m_pogoJointId, false );
|
||||
m_pogoJointId = b3_nullJointId;
|
||||
}
|
||||
|
||||
if ( m_castResult.hit == true )
|
||||
{
|
||||
float moverMass = b3Body_GetMass( m_moverId );
|
||||
float moverWeight =
|
||||
m_gravityScale * b3Length( b3World_GetGravity( m_worldId ) ) * moverMass;
|
||||
|
||||
b3PogoJointDef pogoDef = b3DefaultPogoJointDef();
|
||||
pogoDef.base.localFrameA.p =
|
||||
b3Body_GetLocalPoint( m_castResult.bodyId, m_castResult.point );
|
||||
pogoDef.base.localFrameB.p = m_capsule.center1 + pogoOffset;
|
||||
pogoDef.normal = m_castResult.normal;
|
||||
pogoDef.base.bodyIdA = m_castResult.bodyId;
|
||||
pogoDef.base.bodyIdB = m_moverId;
|
||||
pogoDef.base.collideConnected = true;
|
||||
pogoDef.restLength = m_pogoRestLength;
|
||||
pogoDef.hertz = m_pogoHertz;
|
||||
pogoDef.dampingRatio = m_pogoDampingRatio;
|
||||
pogoDef.maxCompressionForce = m_pogoCompressionScale * moverWeight;
|
||||
|
||||
// Warm start from the joint that was just destroyed
|
||||
pogoDef.impulse = m_pogoImpulse;
|
||||
pogoDef.velocity = m_pogoVelocity;
|
||||
|
||||
if ( m_jumping )
|
||||
{
|
||||
// Don't allow the pogo to pull down
|
||||
pogoDef.maxTensionForce = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
// The pogo can pull down at a multiple of the gravity force.
|
||||
pogoDef.maxTensionForce = m_pogoTensionScale * moverWeight;
|
||||
}
|
||||
|
||||
m_pogoJointId = b3CreatePogoJoint( m_worldId, &pogoDef );
|
||||
}
|
||||
else
|
||||
{
|
||||
m_pogoImpulse = 0.0f;
|
||||
m_pogoVelocity = 0.0f;
|
||||
}
|
||||
|
||||
b3MoverJoint_SetLinearVelocity( m_moverJointId, m_velocity );
|
||||
}
|
||||
|
||||
void DynamicMover::Draw() const
|
||||
{
|
||||
for ( int i = 0; i < m_probeCount; ++i )
|
||||
{
|
||||
const DynamicMoverProbe& probe = m_probes[i];
|
||||
b3Pos end = b3OffsetPos( probe.origin, probe.fraction * probe.translation );
|
||||
|
||||
if ( probe.hit == false )
|
||||
{
|
||||
DrawLine( probe.origin, end, MakeColor( b3_colorGray ) );
|
||||
continue;
|
||||
}
|
||||
|
||||
b3HexColor color = i == m_selectedProbe ? b3_colorGreen : b3_colorYellow;
|
||||
DrawLine( probe.origin, end, MakeColor( color ) );
|
||||
DrawPoint( end, 6.0f, MakeColor( color ) );
|
||||
}
|
||||
|
||||
b3Pos position = b3Body_GetPosition( m_moverId );
|
||||
b3HexColor color = m_onGround ? b3_colorOrange : b3_colorAquamarine;
|
||||
DrawLine( position, position + m_velocity, MakeColor( color ) );
|
||||
}
|
||||
161
samples/dynamic_mover.h
Normal file
161
samples/dynamic_mover.h
Normal file
|
|
@ -0,0 +1,161 @@
|
|||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "box3d/collision.h"
|
||||
#include "box3d/types.h"
|
||||
|
||||
struct DynamicMoverDef
|
||||
{
|
||||
b3Pos position = {};
|
||||
b3Capsule capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f };
|
||||
|
||||
// About 80 kilograms for the default capsule
|
||||
float density = 200.0f;
|
||||
|
||||
b3Filter filter = b3DefaultFilter();
|
||||
float jumpSpeed = 7.0f;
|
||||
float maxSpeed = 6.0f;
|
||||
float minSpeed = 0.1f;
|
||||
float stopSpeed = 3.0f;
|
||||
float accelerate = 20.0f;
|
||||
float airSteer = 0.5f;
|
||||
float friction = 8.0f;
|
||||
float gravityScale = 1.5f;
|
||||
float maxGroundForce = 6000.0f;
|
||||
float maxAirForce = 1500.0f;
|
||||
float pogoRestLength = 0.9f;
|
||||
float pogoHertz = 3.0f;
|
||||
float pogoDampingRatio = 0.8f;
|
||||
float pogoCompressionScale = 100.0f;
|
||||
float pogoTensionScale = 100.0f;
|
||||
uint64_t platformMaterialId = 0;
|
||||
bool enableSkirtPogo = true;
|
||||
bool enablePreSolveEvents = false;
|
||||
bool enableSensorEvents = false;
|
||||
};
|
||||
|
||||
struct DynamicMoverCastResult
|
||||
{
|
||||
b3Pos point;
|
||||
b3Vec3 normal;
|
||||
b3BodyId bodyId;
|
||||
uint64_t userMaterialId;
|
||||
float fraction;
|
||||
bool hit;
|
||||
};
|
||||
|
||||
struct DynamicMoverProbe
|
||||
{
|
||||
b3Pos origin;
|
||||
b3Vec3 translation;
|
||||
float fraction;
|
||||
bool hit;
|
||||
};
|
||||
|
||||
// Dynamic character mover. The capsule is a rigid body with locked rotation, so it
|
||||
// collides and is pushed like everything else in the world. A mover joint drives it to
|
||||
// the target velocity and a pogo joint holds it above the ground so it can climb
|
||||
// steps without touching the capsule.
|
||||
//
|
||||
// The dynamic mover should be vendored into your project so you can alter it to meet
|
||||
// your game's specific needs.
|
||||
//
|
||||
// This system is inspired by Tom Waterson's presentation:
|
||||
// https://www.youtube.com/watch?v=_jRLlTDqoGI
|
||||
class DynamicMover
|
||||
{
|
||||
public:
|
||||
DynamicMover();
|
||||
|
||||
void Create( b3WorldId worldId, const DynamicMoverDef* def );
|
||||
void Destroy();
|
||||
|
||||
// Advance the mover. Throttle is the input along the forward and right directions on [-1, 1].
|
||||
void Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, float speedScale );
|
||||
|
||||
// Jump if grounded. Returns false when airborne.
|
||||
bool Jump();
|
||||
|
||||
// Release the ground so an external impulse can carry the mover into the air. Otherwise the
|
||||
// pogo pulls it back down.
|
||||
void Launch();
|
||||
|
||||
// Gravity scale is used to size the pogo forces, so change it here.
|
||||
void SetGravityScale( float gravityScale );
|
||||
|
||||
void Draw() const;
|
||||
|
||||
static constexpr int m_skirtCount = 8;
|
||||
static constexpr int m_probeCapacity = m_skirtCount + 1;
|
||||
|
||||
float m_jumpSpeed;
|
||||
float m_maxSpeed;
|
||||
float m_minSpeed;
|
||||
float m_stopSpeed;
|
||||
float m_accelerate;
|
||||
|
||||
// Fraction of the ground acceleration available in the air.
|
||||
float m_airSteer;
|
||||
|
||||
// Ground friction in units of 1/time.
|
||||
float m_friction;
|
||||
|
||||
// A character usually wants to fall faster than the rest of the world.
|
||||
float m_gravityScale;
|
||||
|
||||
// Force budget the mover joint has for reaching the target velocity. The air value
|
||||
// is small so the character keeps its momentum while jumping.
|
||||
float m_maxGroundForce;
|
||||
float m_maxAirForce;
|
||||
|
||||
float m_pogoRestLength;
|
||||
float m_pogoHertz;
|
||||
float m_pogoDampingRatio;
|
||||
|
||||
// Pogo force limits as a multiple of the mover weight.
|
||||
float m_pogoCompressionScale;
|
||||
float m_pogoTensionScale;
|
||||
|
||||
// Casts an octagon skirt of rays around the capsule when the central ray misses or
|
||||
// lands on a surface too steep to walk, so the mover can stand on a ledge or a thin
|
||||
// rail, or step off the foot of a steep slope.
|
||||
bool m_enableSkirtPogo;
|
||||
|
||||
// Used for the pogo to detect moving platforms.
|
||||
uint64_t m_platformMaterialId;
|
||||
|
||||
// Surfaces steeper than this are not ground.
|
||||
float m_minGroundNormalY;
|
||||
|
||||
b3Capsule m_capsule;
|
||||
b3Filter m_filter;
|
||||
|
||||
b3WorldId m_worldId;
|
||||
b3BodyId m_groundId;
|
||||
b3BodyId m_moverId;
|
||||
b3JointId m_moverJointId;
|
||||
b3JointId m_pogoJointId;
|
||||
|
||||
DynamicMoverCastResult m_castResult;
|
||||
|
||||
b3Vec3 m_velocity;
|
||||
bool m_onGround;
|
||||
bool m_walkable;
|
||||
bool m_jumping;
|
||||
int m_jumpTicks;
|
||||
|
||||
// Pogo joint state, carried across the joint being recreated each update.
|
||||
float m_pogoImpulse;
|
||||
float m_pogoVelocity;
|
||||
float m_pogoLength;
|
||||
|
||||
// Pogo ray cast results for drawing.
|
||||
DynamicMoverProbe m_probes[m_probeCapacity];
|
||||
int m_probeCount;
|
||||
int m_selectedProbe;
|
||||
|
||||
private:
|
||||
void CreateMoverJoint( b3BodyId baseId, b3Vec3 maxForce );
|
||||
};
|
||||
351
samples/geometric_mover.cpp
Normal file
351
samples/geometric_mover.cpp
Normal file
|
|
@ -0,0 +1,351 @@
|
|||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "geometric_mover.h"
|
||||
|
||||
#include "gfx/draw.h"
|
||||
|
||||
#include "box3d/box3d.h"
|
||||
#include "box3d/math_functions.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <float.h>
|
||||
|
||||
static bool MoverFilterCallback( b3ShapeId shapeId, void* context )
|
||||
{
|
||||
GeometricMover* self = (GeometricMover*)context;
|
||||
for ( int i = 0; i < self->m_ignoreCount; ++i )
|
||||
{
|
||||
if ( B3_ID_EQUALS( shapeId, self->m_ignoreShapeIds[i] ) )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* planeResults, int planeCount,
|
||||
void* context )
|
||||
{
|
||||
if ( MoverFilterCallback( shapeId, context ) == false )
|
||||
{
|
||||
// ignore these planes but continue looking for more
|
||||
return true;
|
||||
}
|
||||
|
||||
GeometricMover* self = static_cast<GeometricMover*>( context );
|
||||
float maxPush = FLT_MAX;
|
||||
bool clipVelocity = true;
|
||||
MoverShapeUserData* userData =
|
||||
static_cast<MoverShapeUserData*>( b3Shape_GetUserData( shapeId ) );
|
||||
if ( userData != nullptr )
|
||||
{
|
||||
maxPush = userData->maxPush;
|
||||
clipVelocity = userData->clipVelocity;
|
||||
}
|
||||
|
||||
for ( int i = 0; i < planeCount && self->m_planeCount < GeometricMover::m_planeCapacity; ++i )
|
||||
{
|
||||
assert( b3IsValidPlane( planeResults[i].plane ) );
|
||||
self->m_planes[self->m_planeCount] = {
|
||||
.plane = planeResults[i].plane,
|
||||
.pushLimit = maxPush,
|
||||
.push = 0.0f,
|
||||
.clipVelocity = clipVelocity,
|
||||
};
|
||||
|
||||
// The plane point is relative to the origin passed to the collide call
|
||||
self->m_planeExtras[self->m_planeCount] = {
|
||||
.point = b3OffsetPos( self->m_position, planeResults[i].point ),
|
||||
.shapeId = shapeId,
|
||||
};
|
||||
self->m_planeCount += 1;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
GeometricMover::GeometricMover()
|
||||
{
|
||||
GeometricMoverDef def;
|
||||
m_jumpSpeed = def.jumpSpeed;
|
||||
m_maxSpeed = def.maxSpeed;
|
||||
m_minSpeed = def.minSpeed;
|
||||
m_stopSpeed = def.stopSpeed;
|
||||
m_accelerate = def.accelerate;
|
||||
m_friction = def.friction;
|
||||
m_gravity = def.gravity;
|
||||
m_pogoRestLength = def.pogoRestLength;
|
||||
m_pogoHertz = def.pogoHertz;
|
||||
m_pogoDampingRatio = def.pogoDampingRatio;
|
||||
m_clipVelocity = true;
|
||||
m_ignoreShapeIds = nullptr;
|
||||
m_ignoreCount = 0;
|
||||
|
||||
m_capsule = def.capsule;
|
||||
m_worldId = b3_nullWorldId;
|
||||
m_position = def.position;
|
||||
m_velocity = b3Vec3_zero;
|
||||
m_pogoVelocity = 0.0f;
|
||||
m_onGround = false;
|
||||
|
||||
m_planeCount = 0;
|
||||
m_totalIterations = 0;
|
||||
|
||||
m_pogoOrigin = b3Pos_zero;
|
||||
m_pogoTranslation = b3Vec3_zero;
|
||||
m_pogoPoint = b3Pos_zero;
|
||||
m_pogoHit = false;
|
||||
}
|
||||
|
||||
void GeometricMover::Create( b3WorldId worldId, const GeometricMoverDef* def )
|
||||
{
|
||||
m_capsule = def->capsule;
|
||||
m_jumpSpeed = def->jumpSpeed;
|
||||
m_maxSpeed = def->maxSpeed;
|
||||
m_minSpeed = def->minSpeed;
|
||||
m_stopSpeed = def->stopSpeed;
|
||||
m_accelerate = def->accelerate;
|
||||
m_friction = def->friction;
|
||||
m_gravity = def->gravity;
|
||||
m_pogoRestLength = def->pogoRestLength;
|
||||
m_pogoHertz = def->pogoHertz;
|
||||
m_pogoDampingRatio = def->pogoDampingRatio;
|
||||
|
||||
m_worldId = worldId;
|
||||
m_position = def->position;
|
||||
m_velocity = b3Vec3_zero;
|
||||
m_pogoVelocity = 0.0f;
|
||||
m_onGround = false;
|
||||
m_planeCount = 0;
|
||||
m_totalIterations = 0;
|
||||
m_pogoHit = false;
|
||||
}
|
||||
|
||||
bool GeometricMover::Jump()
|
||||
{
|
||||
if ( m_onGround == false )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
m_velocity.y = m_jumpSpeed;
|
||||
m_onGround = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Solve a normal constraint between the mover and each touched rigid body. This is the
|
||||
// same velocity constraint the solver uses for a contact, except the mover is treated as
|
||||
// having infinite mass, so all of the impulse lands on the body.
|
||||
void GeometricMover::PushBodies()
|
||||
{
|
||||
for ( int i = 0; i < m_planeCount; ++i )
|
||||
{
|
||||
b3BodyId bodyId = b3Shape_GetBody( m_planeExtras[i].shapeId );
|
||||
if ( b3Body_GetType( bodyId ) != b3_dynamicBody )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
MoverShapeUserData* userData =
|
||||
static_cast<MoverShapeUserData*>( b3Shape_GetUserData( m_planeExtras[i].shapeId ) );
|
||||
if ( userData != nullptr && userData->canMoverPush == false )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Plane normals point at the mover
|
||||
b3Pos point = m_planeExtras[i].point;
|
||||
b3Vec3 normal = b3Neg( m_planes[i].plane.normal );
|
||||
|
||||
float invMassA = 0.0f;
|
||||
float invMassB = b3Body_GetInverseMass( bodyId );
|
||||
b3Matrix3 invIB = b3Body_GetWorldInverseRotationalInertia( bodyId );
|
||||
|
||||
b3Pos centerB = b3Body_GetWorldCenter( bodyId );
|
||||
b3Vec3 rB = b3SubPos( point, centerB );
|
||||
|
||||
b3Vec3 rnB = b3Cross( rB, normal );
|
||||
float kNormal = invMassA + invMassB + b3Dot( rnB, b3MulMV( invIB, rnB ) );
|
||||
float normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
|
||||
|
||||
b3Vec3 vB = b3Body_GetLinearVelocity( bodyId );
|
||||
b3Vec3 omegaB = b3Body_GetAngularVelocity( bodyId );
|
||||
b3Vec3 vrB = b3Add( vB, b3Cross( omegaB, rB ) );
|
||||
float vn = b3Dot( b3Sub( vrB, m_velocity ), normal );
|
||||
|
||||
// Push only, a separating body is left alone
|
||||
float impulse = b3MaxFloat( -normalMass * vn, 0.0f );
|
||||
|
||||
b3Vec3 P = b3MulSV( impulse, normal );
|
||||
m_velocity = b3MulSub( m_velocity, invMassA, P );
|
||||
|
||||
b3Body_ApplyLinearImpulse( bodyId, P, point, true );
|
||||
}
|
||||
}
|
||||
|
||||
// Movement follows the Quake ground and air model:
|
||||
// https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390
|
||||
void GeometricMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle,
|
||||
float speedScale )
|
||||
{
|
||||
// Friction
|
||||
float speed = b3Length( m_velocity );
|
||||
if ( speed < m_minSpeed )
|
||||
{
|
||||
m_velocity.x = 0.0f;
|
||||
m_velocity.z = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Linear damping above stopSpeed and fixed reduction below stopSpeed
|
||||
float control = speed < m_stopSpeed ? m_stopSpeed : speed;
|
||||
|
||||
// friction has units of 1/time
|
||||
float drop = control * m_friction * timeStep;
|
||||
float newSpeed = b3MaxFloat( 0.0f, speed - drop );
|
||||
float ratio = newSpeed / speed;
|
||||
m_velocity.x *= ratio;
|
||||
m_velocity.z *= ratio;
|
||||
}
|
||||
|
||||
float maxSpeed = speedScale * m_maxSpeed;
|
||||
|
||||
b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right;
|
||||
float desiredSpeed;
|
||||
b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity );
|
||||
|
||||
if ( desiredSpeed > maxSpeed )
|
||||
{
|
||||
desiredSpeed = maxSpeed;
|
||||
}
|
||||
|
||||
if ( m_onGround )
|
||||
{
|
||||
m_velocity.y = 0.0f;
|
||||
}
|
||||
|
||||
// Accelerate
|
||||
float currentSpeed = b3Dot( m_velocity, desiredDirection );
|
||||
float addSpeed = desiredSpeed - currentSpeed;
|
||||
if ( addSpeed > 0.0f )
|
||||
{
|
||||
float accelSpeed = m_accelerate * maxSpeed * timeStep;
|
||||
if ( accelSpeed > addSpeed )
|
||||
{
|
||||
accelSpeed = addSpeed;
|
||||
}
|
||||
|
||||
m_velocity += accelSpeed * desiredDirection;
|
||||
}
|
||||
|
||||
m_velocity.y -= m_gravity * timeStep;
|
||||
|
||||
// The probe reaches past the rest length so the spring can be stretched
|
||||
float rayLength = m_pogoRestLength + m_capsule.radius;
|
||||
b3Pos rayOrigin = m_position + m_capsule.center1;
|
||||
b3Vec3 rayTranslation = -rayLength * b3Vec3_axisY;
|
||||
b3QueryFilter pogoFilter = { 1, ~2u, 0, "pogo" };
|
||||
b3RayResult rayResult =
|
||||
b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, pogoFilter );
|
||||
|
||||
m_pogoOrigin = rayOrigin;
|
||||
m_pogoTranslation = rayTranslation;
|
||||
m_pogoPoint = rayResult.point;
|
||||
|
||||
// After gravity was applied, disable pogo when still moving up.
|
||||
// Avoids getting pulled back to the ground when jumping.
|
||||
bool suppressPogo = m_velocity.y > 0.0f;
|
||||
|
||||
if ( rayResult.hit == false || suppressPogo )
|
||||
{
|
||||
m_onGround = false;
|
||||
m_pogoVelocity = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_onGround = true;
|
||||
float pogoCurrentLength = rayResult.fraction * rayLength;
|
||||
float offset = pogoCurrentLength - m_pogoRestLength;
|
||||
m_pogoVelocity =
|
||||
b3SpringDamper( m_pogoHertz, m_pogoDampingRatio, offset, m_pogoVelocity, timeStep );
|
||||
}
|
||||
|
||||
m_pogoHit = m_onGround;
|
||||
|
||||
b3Pos startPosition = m_position;
|
||||
b3Pos target = m_position + timeStep * m_velocity + timeStep * m_pogoVelocity * b3Vec3_axisY;
|
||||
|
||||
// Want the mover to collide with allies
|
||||
b3QueryFilter moverFilter = {
|
||||
.categoryBits = 1, .maskBits = ~0u, .id = 1, .name = "mover_collide" };
|
||||
|
||||
// The cast should ignore allies
|
||||
b3QueryFilter castFilter = {
|
||||
.categoryBits = 1, .maskBits = ~2u, .id = 1, .name = "mover_cast" };
|
||||
|
||||
m_totalIterations = 0;
|
||||
float tolerance = 0.01f;
|
||||
|
||||
for ( int iteration = 0; iteration < 5; ++iteration )
|
||||
{
|
||||
m_planeCount = 0;
|
||||
|
||||
b3World_CollideMover( m_worldId, m_position, &m_capsule, moverFilter, PlaneResultFcn,
|
||||
this );
|
||||
|
||||
b3Vec3 targetDelta = target - m_position;
|
||||
b3PlaneSolverResult result = b3SolvePlanes( targetDelta, m_planes, m_planeCount );
|
||||
|
||||
m_totalIterations += result.iterationCount;
|
||||
|
||||
float fraction = b3World_CastMover( m_worldId, m_position, &m_capsule, result.delta,
|
||||
castFilter, MoverFilterCallback, this );
|
||||
|
||||
b3Vec3 delta = fraction * result.delta;
|
||||
m_position = m_position + delta;
|
||||
|
||||
if ( b3LengthSquared( delta ) < tolerance * tolerance )
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
PushBodies();
|
||||
|
||||
if ( m_clipVelocity )
|
||||
{
|
||||
// Stop pushing into surfaces so speed can't accumulate against a wall
|
||||
m_velocity = b3ClipVector( m_velocity, m_planes, m_planeCount );
|
||||
}
|
||||
else if ( timeStep > 0.0f )
|
||||
{
|
||||
m_velocity = ( 1.0f / timeStep ) * ( m_position - startPosition );
|
||||
}
|
||||
}
|
||||
|
||||
void GeometricMover::Draw() const
|
||||
{
|
||||
if ( m_pogoHit )
|
||||
{
|
||||
DrawLine( m_pogoOrigin, m_pogoPoint, MakeColor( b3_colorGreen ) );
|
||||
}
|
||||
else
|
||||
{
|
||||
DrawLine( m_pogoOrigin, b3OffsetPos( m_pogoOrigin, m_pogoTranslation ),
|
||||
MakeColor( b3_colorGray ) );
|
||||
}
|
||||
|
||||
for ( int i = 0; i < m_planeCount; ++i )
|
||||
{
|
||||
b3Plane plane = m_planes[i].plane;
|
||||
b3Pos p1 = m_position + ( plane.offset - m_capsule.radius ) * plane.normal;
|
||||
b3Pos p2 = p1 + 0.1f * plane.normal;
|
||||
DrawPoint( p1, 5.0f, MakeColor( b3_colorYellow ) );
|
||||
DrawLine( p1, p2, MakeColor( b3_colorYellow ) );
|
||||
}
|
||||
|
||||
DrawSolidCapsule( { m_position, b3Quat_identity }, m_capsule, MakeColor( b3_colorBlue ) );
|
||||
DrawLine( m_position, m_position + m_velocity, MakeColor( b3_colorPurple ) );
|
||||
}
|
||||
114
samples/geometric_mover.h
Normal file
114
samples/geometric_mover.h
Normal file
|
|
@ -0,0 +1,114 @@
|
|||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "box3d/collision.h"
|
||||
#include "box3d/types.h"
|
||||
|
||||
struct GeometricMoverDef
|
||||
{
|
||||
b3Pos position = {};
|
||||
b3Capsule capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f };
|
||||
float jumpSpeed = 5.0f;
|
||||
float maxSpeed = 6.0f;
|
||||
float minSpeed = 0.01f;
|
||||
float stopSpeed = 1.0f;
|
||||
float accelerate = 30.0f;
|
||||
float friction = 4.0f;
|
||||
float gravity = 15.0f;
|
||||
float pogoRestLength = 0.9f;
|
||||
float pogoHertz = 4.0f;
|
||||
float pogoDampingRatio = 0.7f;
|
||||
};
|
||||
|
||||
// Optional shape user data read by GeometricMover when it gathers collision planes.
|
||||
// Shapes without user data are treated as rigid, fully clipping, and pushable.
|
||||
struct MoverShapeUserData
|
||||
{
|
||||
// Distance limit on the push this surface may apply to the mover. FLT_MAX is rigid.
|
||||
float maxPush;
|
||||
|
||||
// False keeps the velocity along this surface, which is what soft collision needs.
|
||||
bool clipVelocity;
|
||||
|
||||
// False stops the mover from shoving this body.
|
||||
bool canMoverPush;
|
||||
};
|
||||
|
||||
// Extra data kept alongside each collision plane so the mover can push rigid bodies.
|
||||
struct PlaneExtra
|
||||
{
|
||||
b3Pos point;
|
||||
b3ShapeId shapeId;
|
||||
};
|
||||
|
||||
// Geometric character mover. The capsule is moved by casting and by solving collision
|
||||
// planes, so the game keeps full control of the motion.
|
||||
//
|
||||
// A pogo spring holds the capsule above the ground so it can move up steps.
|
||||
class GeometricMover
|
||||
{
|
||||
public:
|
||||
GeometricMover();
|
||||
|
||||
void Create( b3WorldId worldId, const GeometricMoverDef* def );
|
||||
|
||||
// Advance the mover. Throttle is the input along the forward and right directions on [-1, 1].
|
||||
void Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, float speedScale );
|
||||
|
||||
// Jump if grounded. Returns false when airborne.
|
||||
bool Jump();
|
||||
|
||||
void Draw() const;
|
||||
|
||||
static constexpr int m_planeCapacity = 8;
|
||||
|
||||
float m_jumpSpeed;
|
||||
float m_maxSpeed;
|
||||
float m_minSpeed;
|
||||
float m_stopSpeed;
|
||||
float m_accelerate;
|
||||
|
||||
// Friction in units of 1/time.
|
||||
float m_friction;
|
||||
|
||||
// The mover is not simulated so it needs its own gravity.
|
||||
float m_gravity;
|
||||
|
||||
float m_pogoRestLength;
|
||||
float m_pogoHertz;
|
||||
float m_pogoDampingRatio;
|
||||
|
||||
// The clipper avoids picking up velocity from soft collision depenetration. Otherwise the
|
||||
// velocity comes from the position delta, which is more intuitive in some cases.
|
||||
bool m_clipVelocity;
|
||||
|
||||
// Shapes the mover passes through. Set before calling Update.
|
||||
const b3ShapeId* m_ignoreShapeIds;
|
||||
int m_ignoreCount;
|
||||
|
||||
// The capsule is relative to the mover position.
|
||||
b3Capsule m_capsule;
|
||||
|
||||
b3WorldId m_worldId;
|
||||
|
||||
b3Pos m_position;
|
||||
b3Vec3 m_velocity;
|
||||
float m_pogoVelocity;
|
||||
bool m_onGround;
|
||||
|
||||
// Planes from the last solve, kept for debug drawing.
|
||||
b3CollisionPlane m_planes[m_planeCapacity] = {};
|
||||
PlaneExtra m_planeExtras[m_planeCapacity] = {};
|
||||
int m_planeCount;
|
||||
int m_totalIterations;
|
||||
|
||||
b3Pos m_pogoOrigin;
|
||||
b3Vec3 m_pogoTranslation;
|
||||
b3Pos m_pogoPoint;
|
||||
bool m_pogoHit;
|
||||
|
||||
private:
|
||||
void PushBodies();
|
||||
};
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1,353 +0,0 @@
|
|||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "mover.h"
|
||||
|
||||
#include "gfx/draw.h"
|
||||
#include "sample.h"
|
||||
|
||||
#include "box3d/box3d.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <gfx/keycodes.h>
|
||||
|
||||
static bool MoverFilterCallback( b3ShapeId shapeId, void* context )
|
||||
{
|
||||
CharacterMover* self = (CharacterMover*)context;
|
||||
for ( int i = 0; i < self->m_ignoreCount; ++i )
|
||||
{
|
||||
if ( B3_ID_EQUALS( shapeId, self->m_ignoreShapeIds[i] ) )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void CharacterMover::Initialize( Sample* sample, b3Pos position )
|
||||
{
|
||||
m_sample = sample;
|
||||
m_transform.p = position;
|
||||
m_transform.q = b3Quat_identity;
|
||||
m_velocity = { 0.0f, 0.0f, 0.0f };
|
||||
m_capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f };
|
||||
|
||||
m_planeCount = 0;
|
||||
m_totalIterations = 0;
|
||||
m_pogoVelocity = 0.0f;
|
||||
m_onGround = false;
|
||||
m_sprint = false;
|
||||
|
||||
m_ignoreShapeIds = nullptr;
|
||||
m_ignoreCount = 0;
|
||||
}
|
||||
|
||||
static bool PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* planeResults, int planeCount, void* context )
|
||||
{
|
||||
if ( MoverFilterCallback( shapeId, context ) == false )
|
||||
{
|
||||
// ignore these planes but continue looking for more
|
||||
return true;
|
||||
}
|
||||
|
||||
CharacterMover* self = static_cast<CharacterMover*>( context );
|
||||
float maxPush = FLT_MAX;
|
||||
bool clipVelocity = true;
|
||||
MoverShapeUserData* userData = static_cast<MoverShapeUserData*>( (void*)b3Shape_GetUserData( shapeId ) );
|
||||
if ( userData != nullptr )
|
||||
{
|
||||
maxPush = userData->maxPush;
|
||||
clipVelocity = userData->clipVelocity;
|
||||
}
|
||||
|
||||
for ( int i = 0; i < planeCount && self->m_planeCount < CharacterMover::m_planeCapacity; ++i )
|
||||
{
|
||||
assert( b3IsValidPlane( planeResults[i].plane ) );
|
||||
self->m_planes[self->m_planeCount] = {
|
||||
.plane = planeResults[i].plane,
|
||||
.pushLimit = maxPush,
|
||||
.push = 0.0f,
|
||||
.clipVelocity = clipVelocity,
|
||||
};
|
||||
self->m_planeExtras[self->m_planeCount] = {
|
||||
.point = b3OffsetPos( self->m_transform.p, planeResults[i].point ),
|
||||
.shapeId = shapeId,
|
||||
};
|
||||
self->m_planeCount += 1;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, bool clipVelocity )
|
||||
{
|
||||
// Friction
|
||||
float speed = b3Length( m_velocity );
|
||||
if ( speed < m_minSpeed )
|
||||
{
|
||||
m_velocity.x = 0.0f;
|
||||
m_velocity.z = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Linear damping above stopSpeed and fixed reduction below stopSpeed
|
||||
float control = speed < m_stopSpeed ? m_stopSpeed : speed;
|
||||
|
||||
// friction has units of 1/time
|
||||
float drop = control * m_friction * timeStep;
|
||||
float newSpeed = b3MaxFloat( 0.0f, speed - drop );
|
||||
float ratio = newSpeed / speed;
|
||||
m_velocity.x *= ratio;
|
||||
m_velocity.z *= ratio;
|
||||
}
|
||||
|
||||
float maxSpeed = m_sprint ? 1.5f * m_maxSpeed : m_maxSpeed;
|
||||
|
||||
b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right;
|
||||
float desiredSpeed;
|
||||
b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity );
|
||||
|
||||
if ( desiredSpeed > maxSpeed )
|
||||
{
|
||||
desiredVelocity *= maxSpeed / desiredSpeed;
|
||||
desiredSpeed = maxSpeed;
|
||||
}
|
||||
|
||||
if ( m_onGround )
|
||||
{
|
||||
m_velocity.y = 0.0f;
|
||||
}
|
||||
|
||||
// Accelerate
|
||||
float currentSpeed = b3Dot( m_velocity, desiredDirection );
|
||||
float addSpeed = desiredSpeed - currentSpeed;
|
||||
if ( addSpeed > 0.0f )
|
||||
{
|
||||
float accelSpeed = m_accelerate * maxSpeed * timeStep;
|
||||
if ( accelSpeed > addSpeed )
|
||||
{
|
||||
accelSpeed = addSpeed;
|
||||
}
|
||||
|
||||
m_velocity += accelSpeed * desiredDirection;
|
||||
}
|
||||
|
||||
m_velocity.y -= m_gravity * timeStep;
|
||||
|
||||
b3WorldId worldId = m_sample->m_worldId;
|
||||
|
||||
float pogoRestLength = 3.0f * m_capsule.radius;
|
||||
float rayLength = pogoRestLength + m_capsule.radius;
|
||||
b3Pos rayOrigin = b3TransformWorldPoint( m_transform, m_capsule.center1 );
|
||||
b3Vec3 rayTranslation = -rayLength * b3Vec3_axisY;
|
||||
b3QueryFilter skipTeamFilter = { 1, ~2u };
|
||||
skipTeamFilter.name = "pogo";
|
||||
b3RayResult rayResult = b3World_CastRayClosest( worldId, rayOrigin, rayTranslation, skipTeamFilter );
|
||||
|
||||
// After gravity was applied, disable pogo when still moving up.
|
||||
// Avoids getting pulled back to the ground when jumping.
|
||||
bool suppressPogo = m_velocity.y > 0.0f;
|
||||
|
||||
if ( rayResult.hit == false || suppressPogo )
|
||||
{
|
||||
m_onGround = false;
|
||||
m_pogoVelocity = 0.0f;
|
||||
|
||||
DrawLine( rayOrigin, b3OffsetPos( rayOrigin, rayTranslation ), MakeColor( b3_colorGray ) );
|
||||
}
|
||||
else
|
||||
{
|
||||
m_onGround = true;
|
||||
float pogoCurrentLength = rayResult.fraction * rayLength;
|
||||
|
||||
float zeta = 0.7f;
|
||||
float hertz = 4.0f;
|
||||
float omega = 2.0f * B3_PI * hertz;
|
||||
float omegaH = omega * timeStep;
|
||||
|
||||
m_pogoVelocity = ( m_pogoVelocity - omega * omegaH * ( pogoCurrentLength - pogoRestLength ) ) /
|
||||
( 1.0f + 2.0f * zeta * omegaH + omegaH * omegaH );
|
||||
DrawLine( rayOrigin, rayResult.point, MakeColor( b3_colorGreen ) );
|
||||
}
|
||||
|
||||
b3Pos startPosition = m_transform.p;
|
||||
b3Pos target = m_transform.p + timeStep * m_velocity + timeStep * m_pogoVelocity * b3Vec3_axisY;
|
||||
|
||||
// Want the mover to collide with allies
|
||||
b3QueryFilter moverFilter = { .categoryBits = 1, .maskBits = ~0u, .id = 1, .name = "mover_collide" };
|
||||
|
||||
// The cast should ignore allies
|
||||
b3QueryFilter castFilter = { .categoryBits = 1, .maskBits = ~2u, .id = 1, .name = "mover_cast" };
|
||||
|
||||
m_totalIterations = 0;
|
||||
float tolerance = 0.01f;
|
||||
|
||||
for ( int iteration = 0; iteration < 5; ++iteration )
|
||||
{
|
||||
m_planeCount = 0;
|
||||
|
||||
b3Capsule mover;
|
||||
mover.center1 = m_capsule.center1;
|
||||
mover.center2 = m_capsule.center2;
|
||||
mover.radius = m_capsule.radius;
|
||||
|
||||
b3World_CollideMover( worldId, m_transform.p, &mover, moverFilter, PlaneResultFcn, this );
|
||||
|
||||
b3Vec3 targetDelta = target - m_transform.p;
|
||||
b3PlaneSolverResult result = b3SolvePlanes( targetDelta, m_planes, m_planeCount );
|
||||
|
||||
m_totalIterations += result.iterationCount;
|
||||
|
||||
b3Vec3 delta = result.delta;
|
||||
|
||||
float fraction = b3World_CastMover( worldId, m_transform.p, &mover, delta, castFilter, MoverFilterCallback, this );
|
||||
|
||||
delta *= fraction;
|
||||
m_transform.p = m_transform.p + delta;
|
||||
|
||||
if ( b3LengthSquared( delta ) < tolerance * tolerance )
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
for ( int i = 0; i < m_planeCount; ++i )
|
||||
{
|
||||
b3BodyId bodyId = b3Shape_GetBody( m_planeExtras[i].shapeId );
|
||||
b3BodyType bodyType = b3Body_GetType( bodyId );
|
||||
if ( bodyType != b3_dynamicBody )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
MoverShapeUserData* userData = static_cast<MoverShapeUserData*>( b3Shape_GetUserData( m_planeExtras[i].shapeId ) );
|
||||
if ( userData != nullptr && userData->canMoverPush == false )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3Pos point = m_planeExtras[i].point;
|
||||
b3Vec3 normal = b3Neg( m_planes[i].plane.normal );
|
||||
|
||||
float invMassA = 0.0f;
|
||||
float invMassB = b3Body_GetInverseMass( bodyId );
|
||||
b3Matrix3 invIB = b3Body_GetWorldInverseRotationalInertia( bodyId );
|
||||
|
||||
b3Pos pB = b3Body_GetWorldCenter( bodyId );
|
||||
b3Vec3 rB = b3SubPos( point, pB );
|
||||
|
||||
b3Vec3 rnB = b3Cross( rB, normal );
|
||||
float kNormal = invMassA + invMassB + b3Dot( rnB, b3MulMV( invIB, rnB ) );
|
||||
float normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
|
||||
|
||||
b3Vec3 vB = b3Body_GetLinearVelocity( bodyId );
|
||||
b3Vec3 omegaB = b3Body_GetAngularVelocity( bodyId );
|
||||
b3Vec3 vrB = b3Add( vB, b3Cross( omegaB, rB ) );
|
||||
float vn = b3Dot( b3Sub( vrB, m_velocity ), normal );
|
||||
float impulse = b3MaxFloat( -normalMass * vn, 0.0f );
|
||||
|
||||
b3Vec3 P = b3MulSV( impulse, normal );
|
||||
m_velocity = b3MulSub( m_velocity, invMassA, P );
|
||||
|
||||
b3Body_ApplyLinearImpulse( bodyId, P, point, true );
|
||||
}
|
||||
|
||||
if ( clipVelocity )
|
||||
{
|
||||
// Using the velocity clipper can avoid picking up velocity from depenetration.
|
||||
// This allows the mover to avoid velocity from soft collision depenetration.
|
||||
m_velocity = b3ClipVector( m_velocity, m_planes, m_planeCount );
|
||||
}
|
||||
else if ( timeStep > 0.0f )
|
||||
{
|
||||
// Using the position delta is more holistic and intuitive in some cases.
|
||||
m_velocity = ( 1.0f / timeStep ) * ( m_transform.p - startPosition );
|
||||
}
|
||||
}
|
||||
|
||||
void CharacterMover::Step( b3ShapeId* ignoreShapes, int ignoreCount, bool clipVelocity )
|
||||
{
|
||||
m_ignoreShapeIds = ignoreShapes;
|
||||
m_ignoreCount = ignoreCount;
|
||||
|
||||
b3Vec2 throttle = { 0.0f, 0.0f };
|
||||
b3Vec3 forward = -m_sample->m_camera->GetForward();
|
||||
b3Vec3 right = m_sample->m_camera->GetRight();
|
||||
|
||||
forward.y = 0.0f;
|
||||
forward = b3Normalize( forward );
|
||||
|
||||
if ( m_sample->m_camera->m_thirdPerson )
|
||||
{
|
||||
if ( IsKeyDown( KEY_W ) )
|
||||
{
|
||||
throttle.x += 1.0f;
|
||||
}
|
||||
|
||||
if ( IsKeyDown( KEY_S ) )
|
||||
{
|
||||
throttle.x -= 1.0f;
|
||||
}
|
||||
|
||||
if ( IsKeyDown( KEY_A ) )
|
||||
{
|
||||
throttle.y -= 1.0f;
|
||||
}
|
||||
|
||||
if ( IsKeyDown( KEY_D ) )
|
||||
{
|
||||
throttle.y += 1.0f;
|
||||
}
|
||||
|
||||
if ( IsKeyDown( KEY_SPACE ) && m_onGround == true )
|
||||
{
|
||||
m_velocity.y = m_jumpSpeed;
|
||||
m_onGround = false;
|
||||
}
|
||||
|
||||
if ( m_onGround == true )
|
||||
{
|
||||
m_sprint = IsKeyDown( KEY_LEFT_SHIFT );
|
||||
}
|
||||
else
|
||||
{
|
||||
m_sprint = false;
|
||||
}
|
||||
}
|
||||
|
||||
float hertz = m_sample->m_context->hertz;
|
||||
float timeStep = hertz > 0.0f ? 1.0f / hertz : 0.0f;
|
||||
|
||||
// throttle = { 0.0f, 0.0f, -1.0f };
|
||||
|
||||
SolveMove( timeStep, forward, right, throttle, clipVelocity );
|
||||
|
||||
b3Pos position = m_transform.p;
|
||||
|
||||
// Follow the mover and latch the draw origin before drawing, so the overlays below demote
|
||||
// against the same eye the view renders from.
|
||||
if ( m_sample->m_camera->m_thirdPerson )
|
||||
{
|
||||
m_sample->m_camera->m_pivot = position;
|
||||
m_sample->m_camera->UpdateTransform();
|
||||
}
|
||||
|
||||
SetDrawOrigin( m_sample->m_camera->DrawOrigin() );
|
||||
|
||||
int count = m_planeCount;
|
||||
for ( int i = 0; i < count; ++i )
|
||||
{
|
||||
b3Plane plane = m_planes[i].plane;
|
||||
b3Pos p1 = position + ( plane.offset - m_capsule.radius ) * plane.normal;
|
||||
b3Pos p2 = p1 + 0.1f * plane.normal;
|
||||
DrawPoint( p1, 5.0f, MakeColor( b3_colorYellow ) );
|
||||
DrawLine( p1, p2, MakeColor( b3_colorYellow ) );
|
||||
}
|
||||
|
||||
DrawSolidCapsule( m_transform, m_capsule, MakeColor( b3_colorBlue ) );
|
||||
DrawLine( position, position + m_velocity, MakeColor( b3_colorPurple ) );
|
||||
|
||||
m_ignoreShapeIds = nullptr;
|
||||
m_ignoreCount = 0;
|
||||
}
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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 "?";
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
143
src/joint.c
143
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 );
|
||||
|
|
|
|||
41
src/joint.h
41
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 );
|
||||
|
|
|
|||
173
src/mover_joint.c
Normal file
173
src/mover_joint.c
Normal file
|
|
@ -0,0 +1,173 @@
|
|||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "body.h"
|
||||
#include "core.h"
|
||||
#include "joint.h"
|
||||
#include "math_internal.h"
|
||||
#include "physics_world.h"
|
||||
#include "recording.h"
|
||||
#include "solver.h"
|
||||
#include "solver_set.h"
|
||||
|
||||
// needed for dll export
|
||||
#include "box3d/box3d.h"
|
||||
|
||||
void b3MoverJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity )
|
||||
{
|
||||
B3_ASSERT( b3IsValidVec3( velocity ) );
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
B3_REC( world, MoverJointSetLinearVelocity, jointId, velocity );
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint );
|
||||
joint->moverJoint.linearVelocity = velocity;
|
||||
}
|
||||
|
||||
b3Vec3 b3MoverJoint_GetLinearVelocity( b3JointId jointId )
|
||||
{
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint );
|
||||
return joint->moverJoint.linearVelocity;
|
||||
}
|
||||
|
||||
void b3MoverJoint_SetMaxVelocityForce( b3JointId jointId, b3Vec3 maxForce )
|
||||
{
|
||||
B3_ASSERT( b3IsValidVec3( maxForce ) );
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
B3_REC( world, MoverJointSetMaxVelocityForce, jointId, maxForce );
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint );
|
||||
joint->moverJoint.maxVelocityForce = maxForce;
|
||||
}
|
||||
|
||||
b3Vec3 b3MoverJoint_GetMaxVelocityForce( b3JointId jointId )
|
||||
{
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint );
|
||||
return joint->moverJoint.maxVelocityForce;
|
||||
}
|
||||
|
||||
b3Vec3 b3GetMoverJointForce( b3World* world, b3JointSim* base )
|
||||
{
|
||||
return b3MulSV( world->inv_h, base->moverJoint.linearVelocityImpulse );
|
||||
}
|
||||
|
||||
void b3PrepareMoverJoint( b3JointSim* base, b3StepContext* context )
|
||||
{
|
||||
B3_ASSERT( base->type == b3_moverJoint );
|
||||
|
||||
int idA = base->bodyIdA;
|
||||
int idB = base->bodyIdB;
|
||||
|
||||
b3World* world = context->world;
|
||||
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, idA );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, idB );
|
||||
|
||||
B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
|
||||
|
||||
b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex );
|
||||
b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex );
|
||||
|
||||
int localIndexA = bodyA->localIndex;
|
||||
int localIndexB = bodyB->localIndex;
|
||||
|
||||
b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA );
|
||||
b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB );
|
||||
|
||||
float mA = bodySimA->invMass;
|
||||
float mB = bodySimB->invMass;
|
||||
|
||||
base->invMassA = mA;
|
||||
base->invMassB = mB;
|
||||
base->invIA = b3Mat3_zero;
|
||||
base->invIB = b3Mat3_zero;
|
||||
|
||||
b3MoverJoint* joint = &base->moverJoint;
|
||||
joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
|
||||
joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
|
||||
|
||||
float k = mA + mB;
|
||||
joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f;
|
||||
|
||||
if ( context->enableWarmStarting == false )
|
||||
{
|
||||
joint->linearVelocityImpulse = b3Vec3_zero;
|
||||
}
|
||||
}
|
||||
|
||||
void b3WarmStartMoverJoint( b3JointSim* base, b3StepContext* context )
|
||||
{
|
||||
B3_ASSERT( base->type == b3_moverJoint );
|
||||
|
||||
float mA = base->invMassA;
|
||||
float mB = base->invMassB;
|
||||
|
||||
b3MoverJoint* joint = &base->moverJoint;
|
||||
|
||||
// dummy state for static bodies
|
||||
b3BodyState dummyState = b3_identityBodyState;
|
||||
|
||||
b3BodyState* stateA =
|
||||
joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
|
||||
b3BodyState* stateB =
|
||||
joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
|
||||
|
||||
if ( stateA->flags & b3_dynamicFlag )
|
||||
{
|
||||
stateA->linearVelocity =
|
||||
b3MulSub( stateA->linearVelocity, mA, joint->linearVelocityImpulse );
|
||||
}
|
||||
|
||||
if ( stateB->flags & b3_dynamicFlag )
|
||||
{
|
||||
stateB->linearVelocity =
|
||||
b3MulAdd( stateB->linearVelocity, mB, joint->linearVelocityImpulse );
|
||||
}
|
||||
}
|
||||
|
||||
void b3SolveMoverJoint( b3JointSim* base, b3StepContext* context )
|
||||
{
|
||||
B3_ASSERT( base->type == b3_moverJoint );
|
||||
|
||||
float mA = base->invMassA;
|
||||
float mB = base->invMassB;
|
||||
|
||||
// dummy state for static bodies
|
||||
b3BodyState dummyState = b3_identityBodyState;
|
||||
|
||||
b3MoverJoint* joint = &base->moverJoint;
|
||||
b3BodyState* stateA =
|
||||
joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
|
||||
b3BodyState* stateB =
|
||||
joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
|
||||
|
||||
b3Vec3 vA = stateA->linearVelocity;
|
||||
b3Vec3 vB = stateB->linearVelocity;
|
||||
|
||||
b3Vec3 maxForce = joint->maxVelocityForce;
|
||||
if ( maxForce.x > 0.0f || maxForce.y > 0.0f || maxForce.z > 0.0f )
|
||||
{
|
||||
b3Vec3 cdot = b3Sub( b3Sub( vB, vA ), joint->linearVelocity );
|
||||
b3Vec3 impulse = b3MulSV( -joint->linearMass, cdot );
|
||||
|
||||
b3Vec3 oldImpulse = joint->linearVelocityImpulse;
|
||||
b3Vec3 maxImpulse = b3MulSV( context->h, maxForce );
|
||||
joint->linearVelocityImpulse =
|
||||
b3Clamp( b3Add( oldImpulse, impulse ), b3Neg( maxImpulse ), maxImpulse );
|
||||
impulse = b3Sub( joint->linearVelocityImpulse, oldImpulse );
|
||||
|
||||
vA = b3MulSub( vA, mA, impulse );
|
||||
vB = b3MulAdd( vB, mB, impulse );
|
||||
}
|
||||
else
|
||||
{
|
||||
joint->linearVelocityImpulse = b3Vec3_zero;
|
||||
}
|
||||
|
||||
if ( stateA->flags & b3_dynamicFlag )
|
||||
{
|
||||
stateA->linearVelocity = vA;
|
||||
}
|
||||
|
||||
if ( stateB->flags & b3_dynamicFlag )
|
||||
{
|
||||
stateB->linearVelocity = vB;
|
||||
}
|
||||
}
|
||||
281
src/pogo_joint.c
Normal file
281
src/pogo_joint.c
Normal file
|
|
@ -0,0 +1,281 @@
|
|||
// SPDX-FileCopyrightText: 2026 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "body.h"
|
||||
#include "core.h"
|
||||
#include "joint.h"
|
||||
#include "math_internal.h"
|
||||
#include "physics_world.h"
|
||||
#include "recording.h"
|
||||
#include "solver.h"
|
||||
#include "solver_set.h"
|
||||
|
||||
// needed for dll export
|
||||
#include "box3d/box3d.h"
|
||||
|
||||
void b3PogoJoint_SetRestLength( b3JointId jointId, float length )
|
||||
{
|
||||
B3_ASSERT( b3IsValidFloat( length ) && length >= 0.0f );
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
B3_REC( world, PogoJointSetRestLength, jointId, length );
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
joint->pogoJoint.restLength = length;
|
||||
}
|
||||
|
||||
float b3PogoJoint_GetRestLength( b3JointId jointId )
|
||||
{
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
return joint->pogoJoint.restLength;
|
||||
}
|
||||
|
||||
void b3PogoJoint_SetSpringHertz( b3JointId jointId, float hertz )
|
||||
{
|
||||
B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
B3_REC( world, PogoJointSetSpringHertz, jointId, hertz );
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
joint->pogoJoint.hertz = hertz;
|
||||
}
|
||||
|
||||
float b3PogoJoint_GetSpringHertz( b3JointId jointId )
|
||||
{
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
return joint->pogoJoint.hertz;
|
||||
}
|
||||
|
||||
void b3PogoJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio )
|
||||
{
|
||||
B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
B3_REC( world, PogoJointSetSpringDampingRatio, jointId, dampingRatio );
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
joint->pogoJoint.dampingRatio = dampingRatio;
|
||||
}
|
||||
|
||||
float b3PogoJoint_GetSpringDampingRatio( b3JointId jointId )
|
||||
{
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
return joint->pogoJoint.dampingRatio;
|
||||
}
|
||||
|
||||
float b3PogoJoint_GetCurrentLength( b3JointId jointId )
|
||||
{
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
b3JointSim* base = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
|
||||
b3WorldTransform worldTransformA = b3GetBodyTransform( world, base->bodyIdA );
|
||||
b3Transform transformA = b3ToRelativeTransform( worldTransformA, worldTransformA.p );
|
||||
b3Transform transformB =
|
||||
b3ToRelativeTransform( b3GetBodyTransform( world, base->bodyIdB ), worldTransformA.p );
|
||||
|
||||
b3Vec3 axis = b3RotateVector( b3MulQuat( transformB.q, base->localFrameB.q ), b3Vec3_axisY );
|
||||
b3Vec3 pA = b3TransformPoint( transformA, base->localFrameA.p );
|
||||
b3Vec3 pB = b3TransformPoint( transformB, base->localFrameB.p );
|
||||
return b3Dot( b3Sub( pB, pA ), axis );
|
||||
}
|
||||
|
||||
float b3PogoJoint_GetImpulse( b3JointId jointId )
|
||||
{
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
return joint->pogoJoint.impulse;
|
||||
}
|
||||
|
||||
float b3PogoJoint_GetCurrentSpeed( b3JointId jointId )
|
||||
{
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
return joint->pogoJoint.velocity;
|
||||
}
|
||||
|
||||
b3Vec3 b3GetPogoJointForce( b3World* world, b3JointSim* base )
|
||||
{
|
||||
return b3MulSV( world->inv_h * base->pogoJoint.impulse, base->pogoJoint.normal );
|
||||
}
|
||||
|
||||
void b3PreparePogoJoint( b3JointSim* base, b3StepContext* context )
|
||||
{
|
||||
B3_ASSERT( base->type == b3_pogoJoint );
|
||||
|
||||
b3World* world = context->world;
|
||||
|
||||
// chase body id to the solver set where the body lives
|
||||
int idA = base->bodyIdA;
|
||||
int idB = base->bodyIdB;
|
||||
|
||||
b3Body* bodyA = b3Array_Get( world->bodies, idA );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, idB );
|
||||
|
||||
B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
|
||||
|
||||
b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex );
|
||||
b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex );
|
||||
|
||||
int localIndexA = bodyA->localIndex;
|
||||
int localIndexB = bodyB->localIndex;
|
||||
|
||||
b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA );
|
||||
b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB );
|
||||
|
||||
float mA = bodySimA->invMass;
|
||||
float mB = bodySimB->invMass;
|
||||
b3Matrix3 iA = bodySimA->invInertiaWorld;
|
||||
b3Matrix3 iB = bodySimB->invInertiaWorld;
|
||||
|
||||
base->invMassA = mA;
|
||||
base->invMassB = mB;
|
||||
base->invIA = iA;
|
||||
base->invIB = iB;
|
||||
|
||||
b3PogoJoint* joint = &base->pogoJoint;
|
||||
joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
|
||||
joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
|
||||
|
||||
// Compute joint anchor frames with world space rotation, relative to center of mass
|
||||
joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q );
|
||||
joint->frameA.p = b3RotateVector( bodySimA->transform.q,
|
||||
b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
|
||||
joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
|
||||
joint->frameB.p = b3RotateVector( bodySimB->transform.q,
|
||||
b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
|
||||
|
||||
// Compute the initial center delta. Incremental position updates are relative to this.
|
||||
joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
|
||||
|
||||
b3Vec3 rnA = b3Cross( joint->frameA.p, joint->normal );
|
||||
b3Vec3 rnB = b3Cross( joint->frameB.p, joint->normal );
|
||||
float k = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) );
|
||||
joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f;
|
||||
|
||||
if ( context->enableWarmStarting == false )
|
||||
{
|
||||
joint->impulse = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void b3WarmStartPogoJoint( b3JointSim* base, b3StepContext* context )
|
||||
{
|
||||
B3_ASSERT( base->type == b3_pogoJoint );
|
||||
|
||||
float mA = base->invMassA;
|
||||
float mB = base->invMassB;
|
||||
b3Matrix3 iA = base->invIA;
|
||||
b3Matrix3 iB = base->invIB;
|
||||
|
||||
b3PogoJoint* joint = &base->pogoJoint;
|
||||
|
||||
if ( joint->hertz == 0.0f )
|
||||
{
|
||||
joint->impulse = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// dummy state for static bodies
|
||||
b3BodyState dummyState = b3_identityBodyState;
|
||||
|
||||
b3BodyState* stateA =
|
||||
joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
|
||||
b3BodyState* stateB =
|
||||
joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
|
||||
|
||||
b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
|
||||
b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
|
||||
|
||||
b3Vec3 linearImpulse = b3MulSV( joint->impulse, joint->normal );
|
||||
|
||||
if ( stateA->flags & b3_dynamicFlag )
|
||||
{
|
||||
stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse );
|
||||
stateA->angularVelocity =
|
||||
b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Cross( rA, linearImpulse ) ) );
|
||||
}
|
||||
|
||||
if ( stateB->flags & b3_dynamicFlag )
|
||||
{
|
||||
stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, linearImpulse );
|
||||
stateB->angularVelocity =
|
||||
b3Add( stateB->angularVelocity, b3MulMV( iB, b3Cross( rB, linearImpulse ) ) );
|
||||
}
|
||||
}
|
||||
|
||||
void b3SolvePogoJoint( b3JointSim* base, b3StepContext* context, bool useBias )
|
||||
{
|
||||
B3_ASSERT( base->type == b3_pogoJoint );
|
||||
|
||||
float mA = base->invMassA;
|
||||
float mB = base->invMassB;
|
||||
b3Matrix3 iA = base->invIA;
|
||||
b3Matrix3 iB = base->invIB;
|
||||
|
||||
// dummy state for static bodies
|
||||
b3BodyState dummyState = b3_identityBodyState;
|
||||
|
||||
b3PogoJoint* joint = &base->pogoJoint;
|
||||
if ( joint->hertz == 0.0f )
|
||||
{
|
||||
joint->impulse = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
b3BodyState* stateA =
|
||||
joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
|
||||
b3BodyState* stateB =
|
||||
joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
|
||||
|
||||
b3Vec3 vA = stateA->linearVelocity;
|
||||
b3Vec3 wA = stateA->angularVelocity;
|
||||
b3Vec3 vB = stateB->linearVelocity;
|
||||
b3Vec3 wB = stateB->angularVelocity;
|
||||
|
||||
b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
|
||||
b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
|
||||
|
||||
float bias = 0.0f;
|
||||
if ( useBias )
|
||||
{
|
||||
// This is a custom pogo position correction designed to avoid feeding velocity into
|
||||
// the mover body. Otherwise the mover can make huge jumps going up stairs. This still
|
||||
// works with continuous collision.
|
||||
|
||||
b3Vec3 dcA = stateA->deltaPosition;
|
||||
b3Vec3 dcB = stateB->deltaPosition;
|
||||
b3Vec3 d = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter );
|
||||
|
||||
// The pogo axis is usually fixed as the world y-axis. Different than the ground normal.
|
||||
b3Vec3 pogoAxis = b3RotateVector( joint->frameB.q, b3Vec3_axisY );
|
||||
float c = b3Dot( pogoAxis, d ) - joint->restLength;
|
||||
|
||||
// This could be divided by dot(joint->normal, pogoAxis) to account for the constraint
|
||||
// direction, but I'd rather diminish the pogo recovery rate than risk making it huge.
|
||||
joint->velocity =
|
||||
b3SpringDamper( joint->hertz, joint->dampingRatio, c, joint->velocity, context->h );
|
||||
bias = -joint->velocity;
|
||||
}
|
||||
|
||||
b3Vec3 vr = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) );
|
||||
float cdot = b3Dot( joint->normal, vr );
|
||||
|
||||
float maxTensionImpulse = context->h * joint->maxTensionForce;
|
||||
float maxCompressionImpulse = context->h * joint->maxCompressionForce;
|
||||
float oldImpulse = joint->impulse;
|
||||
float impulse = -joint->linearMass * ( cdot + bias );
|
||||
joint->impulse =
|
||||
b3ClampFloat( joint->impulse + impulse, -maxTensionImpulse, maxCompressionImpulse );
|
||||
impulse = joint->impulse - oldImpulse;
|
||||
|
||||
b3Vec3 P = b3MulSV( impulse, joint->normal );
|
||||
vA = b3MulSub( vA, mA, P );
|
||||
wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) );
|
||||
vB = b3MulAdd( vB, mB, P );
|
||||
wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) );
|
||||
|
||||
if ( stateA->flags & b3_dynamicFlag )
|
||||
{
|
||||
stateA->linearVelocity = vA;
|
||||
stateA->angularVelocity = wA;
|
||||
}
|
||||
|
||||
if ( stateB->flags & b3_dynamicFlag )
|
||||
{
|
||||
stateB->linearVelocity = vB;
|
||||
stateB->angularVelocity = wB;
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 ) )
|
||||
|
|
|
|||
72
src/replay.c
72
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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in a new issue