dynamic mover
This commit is contained in:
parent
f36d061ba2
commit
f0b1ed2fca
29 changed files with 3078 additions and 432 deletions
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 ) );
|
||||
}
|
||||
Loading…
Reference in a new issue