fixes
This commit is contained in:
parent
c5f9ed55c5
commit
9df6c22049
8 changed files with 175 additions and 70 deletions
|
|
@ -1418,6 +1418,18 @@ B3_API void b3PogoJoint_SetSpringDampingRatio( b3JointId jointId, float dampingR
|
|||
/// Get the spring damping ratio
|
||||
B3_API float b3PogoJoint_GetSpringDampingRatio( b3JointId jointId );
|
||||
|
||||
/// Set the max tension force, usually in Newtons.
|
||||
B3_API void b3PogoJoint_SetMaxTensionForce( b3JointId jointId, float force );
|
||||
|
||||
/// Get the max tension force, usually in Newtons.
|
||||
B3_API float b3PogoJoint_GetMaxTensionForce( b3JointId jointId );
|
||||
|
||||
/// Set the max compression force, usually in Newtons.
|
||||
B3_API void b3PogoJoint_SetMaxCompressionForce( b3JointId jointId, float force );
|
||||
|
||||
/// Get the max compression force, usually in Newtons.
|
||||
B3_API float b3PogoJoint_GetMaxCompressionForce( b3JointId jointId );
|
||||
|
||||
/// Get the current length of the pogo.
|
||||
B3_API float b3PogoJoint_GetCurrentLength( b3JointId jointId );
|
||||
|
||||
|
|
|
|||
|
|
@ -70,6 +70,7 @@ DynamicMover::DynamicMover()
|
|||
m_worldId = b3_nullWorldId;
|
||||
m_groundId = b3_nullBodyId;
|
||||
m_moverId = b3_nullBodyId;
|
||||
m_surfaceBodyId = b3_nullBodyId;
|
||||
m_moverJointId = b3_nullJointId;
|
||||
m_pogoJointId = b3_nullJointId;
|
||||
|
||||
|
|
@ -151,6 +152,7 @@ void DynamicMover::Create( b3WorldId worldId, const DynamicMoverDef* def )
|
|||
// them.
|
||||
shapeDef.baseMaterial.friction = 0.0f;
|
||||
shapeDef.baseMaterial.restitution = 0.0f;
|
||||
shapeDef.baseMaterial.customColor = b3_colorDodgerBlue;
|
||||
shapeDef.filter = def->filter;
|
||||
shapeDef.enablePreSolveEvents = def->enablePreSolveEvents;
|
||||
shapeDef.enableSensorEvents = def->enableSensorEvents;
|
||||
|
|
@ -462,41 +464,93 @@ void DynamicMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2
|
|||
if ( m_castResult.hit == true )
|
||||
{
|
||||
float moverMass = b3Body_GetMass( m_moverId );
|
||||
float moverWeight =
|
||||
m_gravityScale * b3Length( b3World_GetGravity( m_worldId ) ) * moverMass;
|
||||
float gravity = b3Length( b3World_GetGravity( m_worldId ) );
|
||||
float moverWeight = m_gravityScale * gravity * 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;
|
||||
bool pogoJointValid = b3Joint_IsValid( m_pogoJointId );
|
||||
|
||||
// Warm start from the joint that was just destroyed
|
||||
pogoDef.impulse = m_pogoImpulse;
|
||||
pogoDef.velocity = m_pogoVelocity;
|
||||
|
||||
if ( m_jumping )
|
||||
// Create or recreate the pogo joint as needed.
|
||||
if ( B3_ID_EQUALS( m_castResult.bodyId, m_surfaceBodyId ) == false ||
|
||||
pogoJointValid == false )
|
||||
{
|
||||
// Don't allow the pogo to pull down
|
||||
pogoDef.maxTensionForce = 0.0f;
|
||||
if ( pogoJointValid )
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
m_surfaceBodyId = m_castResult.bodyId;
|
||||
|
||||
b3PogoJointDef pogoDef = b3DefaultPogoJointDef();
|
||||
pogoDef.base.localFrameA.p =
|
||||
b3Body_GetLocalPoint( m_surfaceBodyId, m_castResult.point );
|
||||
pogoDef.base.localFrameB.p = m_capsule.center1 + pogoOffset;
|
||||
pogoDef.normal = m_castResult.normal;
|
||||
pogoDef.base.bodyIdA = m_surfaceBodyId;
|
||||
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
|
||||
{
|
||||
// The pogo can pull down at a multiple of the gravity force.
|
||||
pogoDef.maxTensionForce = m_pogoTensionScale * moverWeight;
|
||||
}
|
||||
// Preserve the joint, improving performance.
|
||||
|
||||
m_pogoJointId = b3CreatePogoJoint( m_worldId, &pogoDef );
|
||||
b3Transform localFrameA;
|
||||
localFrameA.p = b3Body_GetLocalPoint( m_surfaceBodyId, m_castResult.point );
|
||||
localFrameA.q = b3Quat_identity;
|
||||
b3Joint_SetLocalFrameA( m_pogoJointId, localFrameA );
|
||||
|
||||
if ( m_jumping )
|
||||
{
|
||||
// Don't allow the pogo to pull down
|
||||
b3PogoJoint_SetMaxTensionForce( m_pogoJointId, 0.0f );
|
||||
}
|
||||
else
|
||||
{
|
||||
// The pogo can pull down at a multiple of the gravity force.
|
||||
b3PogoJoint_SetMaxTensionForce( m_pogoJointId, m_pogoTensionScale * moverWeight );
|
||||
}
|
||||
|
||||
b3PogoJoint_SetMaxCompressionForce( m_pogoJointId,
|
||||
m_pogoCompressionScale * moverWeight );
|
||||
|
||||
b3PogoJoint_SetSpringHertz( m_pogoJointId, m_pogoHertz );
|
||||
b3PogoJoint_SetSpringDampingRatio( m_pogoJointId, m_pogoHertz );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( b3Joint_IsValid( m_pogoJointId ) )
|
||||
{
|
||||
b3DestroyJoint( m_pogoJointId, false );
|
||||
m_pogoJointId = b3_nullJointId;
|
||||
}
|
||||
|
||||
m_surfaceBodyId = b3_nullBodyId;
|
||||
m_pogoImpulse = 0.0f;
|
||||
m_pogoVelocity = 0.0f;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -138,6 +138,7 @@ public:
|
|||
b3WorldId m_worldId;
|
||||
b3BodyId m_groundId;
|
||||
b3BodyId m_moverId;
|
||||
b3BodyId m_surfaceBodyId;
|
||||
b3JointId m_moverJointId;
|
||||
b3JointId m_pogoJointId;
|
||||
|
||||
|
|
|
|||
|
|
@ -843,9 +843,6 @@ public:
|
|||
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:
|
||||
|
|
@ -875,7 +872,7 @@ public:
|
|||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
b3BodyId stairsId = b3CreateBody( m_worldId, &bodyDef );
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.customColor = b3_colorCornflowerBlue;
|
||||
shapeDef.baseMaterial.customColor = b3_colorOliveDrab;
|
||||
|
||||
for ( int i = 0; i < 10; ++i )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -149,38 +149,31 @@ void b3SolveMoverJoint( b3JointSim* base, b3StepContext* context )
|
|||
b3Vec3 vB = stateB->linearVelocity;
|
||||
|
||||
float maxForce = joint->maxForce;
|
||||
if ( maxForce > 0.0f )
|
||||
b3Vec3 vr = b3Sub( b3Sub( vB, vA ), joint->linearVelocity );
|
||||
b3Vec2 cdot = { b3Dot( vr, joint->axisX ), b3Dot( vr, joint->axisZ ) };
|
||||
b3Vec2 deltaImpulse = b3MulSV2( -joint->linearMass, cdot );
|
||||
b3Vec2 newImpulse = {
|
||||
joint->impulse.x + deltaImpulse.x,
|
||||
joint->impulse.y + deltaImpulse.y,
|
||||
};
|
||||
|
||||
float maxImpulse = context->h * maxForce;
|
||||
|
||||
float lengthSquared = b3Dot2( newImpulse, newImpulse );
|
||||
if ( lengthSquared > maxImpulse * maxImpulse )
|
||||
{
|
||||
b3Vec3 vr = b3Sub( b3Sub( vB, vA ), joint->linearVelocity );
|
||||
b3Vec2 cdot = { b3Dot( vr, joint->axisX ), b3Dot( vr, joint->axisZ ) };
|
||||
b3Vec2 deltaImpulse = b3MulSV2( -joint->linearMass, cdot );
|
||||
b3Vec2 newImpulse = {
|
||||
joint->impulse.x + deltaImpulse.x,
|
||||
joint->impulse.y + deltaImpulse.y,
|
||||
};
|
||||
|
||||
float maxImpulse = context->h * maxForce;
|
||||
|
||||
float lengthSquared = b3Dot2( newImpulse, newImpulse );
|
||||
if ( lengthSquared > maxImpulse * maxImpulse )
|
||||
{
|
||||
float scale = maxImpulse / sqrtf( lengthSquared );
|
||||
newImpulse.x *= scale;
|
||||
newImpulse.y *= scale;
|
||||
}
|
||||
|
||||
deltaImpulse = b3Sub2( newImpulse, joint->impulse );
|
||||
joint->impulse = newImpulse;
|
||||
|
||||
b3Vec3 P = b3Blend2( deltaImpulse.x, joint->axisX, deltaImpulse.y, joint->axisZ );
|
||||
vA = b3MulSub( vA, mA, P );
|
||||
vB = b3MulAdd( vB, mB, P );
|
||||
}
|
||||
else
|
||||
{
|
||||
joint->impulse = (b3Vec2){ 0.0f, 0.0f };
|
||||
float scale = maxImpulse / sqrtf( lengthSquared );
|
||||
newImpulse.x *= scale;
|
||||
newImpulse.y *= scale;
|
||||
}
|
||||
|
||||
deltaImpulse = b3Sub2( newImpulse, joint->impulse );
|
||||
joint->impulse = newImpulse;
|
||||
|
||||
b3Vec3 P = b3Blend2( deltaImpulse.x, joint->axisX, deltaImpulse.y, joint->axisZ );
|
||||
vA = b3MulSub( vA, mA, P );
|
||||
vB = b3MulAdd( vB, mB, P );
|
||||
|
||||
if ( stateA->flags & b3_dynamicFlag )
|
||||
{
|
||||
stateA->linearVelocity = vA;
|
||||
|
|
@ -205,12 +198,12 @@ static b3HexColor b3LerpColor( b3HexColor a, b3HexColor b, float t )
|
|||
return (b3HexColor)result;
|
||||
}
|
||||
|
||||
static void b3DrawArrow( b3DebugDraw* draw, b3Pos a, b3Pos b, b3Pos planeNormal, float size,
|
||||
static void b3DrawArrow( b3DebugDraw* draw, b3Pos a, b3Pos b, b3Vec3 planeNormal, float size,
|
||||
b3HexColor color )
|
||||
{
|
||||
draw->DrawSegmentFcn( a, b, color, draw->context );
|
||||
|
||||
b3Vec3 shaft = b3Sub( b, a );
|
||||
b3Vec3 shaft = b3SubPos( b, a );
|
||||
float shaftLen = b3Length( shaft );
|
||||
if ( shaftLen < 1e-6f )
|
||||
{
|
||||
|
|
@ -223,8 +216,8 @@ static void b3DrawArrow( b3DebugDraw* draw, b3Pos a, b3Pos b, b3Pos planeNormal,
|
|||
b3Vec3 perp = b3Normalize( b3Cross( planeNormal, dir ) );
|
||||
b3Vec3 backFromTip = b3MulSV( -headLen, dir );
|
||||
b3Vec3 sideStep = b3MulSV( headLen * 0.5f, perp );
|
||||
b3Vec3 tip1 = b3Add( b, b3Add( backFromTip, sideStep ) );
|
||||
b3Vec3 tip2 = b3Add( b, b3Sub( backFromTip, sideStep ) );
|
||||
b3Pos tip1 = b3OffsetPos( b, b3Add( backFromTip, sideStep ) );
|
||||
b3Pos tip2 = b3OffsetPos( b, b3Sub( backFromTip, sideStep ) );
|
||||
draw->DrawSegmentFcn( b, tip1, color, draw->context );
|
||||
draw->DrawSegmentFcn( b, tip2, color, draw->context );
|
||||
}
|
||||
|
|
@ -249,15 +242,19 @@ void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base,
|
|||
b3Vec3 vA = stateA != NULL ? stateA->linearVelocity : b3Vec3_zero;
|
||||
b3Vec3 vB = stateB != NULL ? stateB->linearVelocity : b3Vec3_zero;
|
||||
|
||||
b3Vec3 error = b3Sub( b3Sub( vB, vA ), joint->linearVelocity );
|
||||
b3Vec2 planeError = { b3Dot( error, axisX ), b3Dot( error, axisZ ) };
|
||||
b3Vec2 planeTarget = { b3Dot( joint->linearVelocity, axisX ),
|
||||
b3Vec3 relativeVelocity = b3Sub( b3Sub( vB, vA ), joint->linearVelocity );
|
||||
b3Vec2 planarVelocity = { b3Dot( relativeVelocity, axisX ), b3Dot( relativeVelocity, axisZ ) };
|
||||
b3Vec2 targetVelocity = { b3Dot( joint->linearVelocity, axisX ),
|
||||
b3Dot( joint->linearVelocity, axisZ ) };
|
||||
|
||||
// Colorize ring by ratio of actual speed to target speed.
|
||||
float referenceSpeed = b3MaxFloat( b3Length2( planeTarget ), 1.0f );
|
||||
float t = b3ClampFloat( b3Length2( planeError ) / referenceSpeed, 0.0f, 1.0f );
|
||||
b3HexColor color = b3LerpColor( b3_colorWhite, b3_colorBlue, t );
|
||||
b3HexColor color = b3_colorWhite;
|
||||
float targetSpeed = b3Length2( targetVelocity );
|
||||
if (targetSpeed > 0.0f)
|
||||
{
|
||||
float t = b3ClampFloat( b3Length2( planarVelocity ) / targetSpeed, 0.0f, 1.0f );
|
||||
color = b3LerpColor( b3_colorWhite, b3_colorBlue, t );
|
||||
}
|
||||
|
||||
float radius = draw->jointScale;
|
||||
b3Vec3 rx = b3MulSV( radius, axisX );
|
||||
|
|
|
|||
|
|
@ -60,6 +60,36 @@ float b3PogoJoint_GetSpringDampingRatio( b3JointId jointId )
|
|||
return joint->pogoJoint.dampingRatio;
|
||||
}
|
||||
|
||||
void b3PogoJoint_SetMaxTensionForce( b3JointId jointId, float force )
|
||||
{
|
||||
B3_ASSERT( b3IsValidFloat( force ) && force >= 0.0f );
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
B3_REC( world, PogoJointSetMaxTensionForce, jointId, force );
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
joint->pogoJoint.maxTensionForce = force;
|
||||
}
|
||||
|
||||
float b3PogoJoint_GetMaxTensionForce(b3JointId jointId)
|
||||
{
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
return joint->pogoJoint.maxTensionForce;
|
||||
}
|
||||
|
||||
void b3PogoJoint_SetMaxCompressionForce(b3JointId jointId, float force)
|
||||
{
|
||||
B3_ASSERT( b3IsValidFloat( force ) && force >= 0.0f );
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
B3_REC( world, PogoJointSetMaxCompressionForce, jointId, force );
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
joint->pogoJoint.maxCompressionForce = force;
|
||||
}
|
||||
|
||||
float b3PogoJoint_GetMaxCompressionForce(b3JointId jointId)
|
||||
{
|
||||
b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint );
|
||||
return joint->pogoJoint.maxCompressionForce;
|
||||
}
|
||||
|
||||
float b3PogoJoint_GetCurrentLength( b3JointId jointId )
|
||||
{
|
||||
b3World* world = b3GetWorld( jointId.world0 );
|
||||
|
|
|
|||
|
|
@ -222,6 +222,8 @@ B3_REC_OP( 0x84, CreatePogoJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( POGOJO
|
|||
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 ) )
|
||||
B3_REC_OP( 0x88, PogoJointSetMaxTensionForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) )
|
||||
B3_REC_OP( 0x89, PogoJointSetMaxCompressionForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) )
|
||||
|
||||
// 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.
|
||||
|
|
|
|||
12
src/replay.c
12
src/replay.c
|
|
@ -1740,6 +1740,18 @@ static void b3RecDispatch_PogoJointSetSpringDampingRatio(
|
|||
b3PogoJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio );
|
||||
}
|
||||
|
||||
static void b3RecDispatch_PogoJointSetMaxTensionForce(
|
||||
const b3RecArgs_PogoJointSetMaxTensionForce* a, b3RecReader* rdr )
|
||||
{
|
||||
b3PogoJoint_SetMaxTensionForce( b3RecMakeJointId( rdr, a->joint ), a->force );
|
||||
}
|
||||
|
||||
static void b3RecDispatch_PogoJointSetMaxCompressionForce(
|
||||
const b3RecArgs_PogoJointSetMaxCompressionForce* a, b3RecReader* rdr )
|
||||
{
|
||||
b3PogoJoint_SetMaxCompressionForce( b3RecMakeJointId( rdr, a->joint ), a->force );
|
||||
}
|
||||
|
||||
static void b3RecDispatch_StateHash( const b3RecArgs_StateHash* a, b3RecReader* rdr )
|
||||
{
|
||||
b3World* world = b3GetWorldFromId( rdr->replayWorldId );
|
||||
|
|
|
|||
Loading…
Reference in a new issue