diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index d9e69d8..252e52e 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -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 ); diff --git a/samples/dynamic_mover.cpp b/samples/dynamic_mover.cpp index f1e4fa6..dd1e9a1 100644 --- a/samples/dynamic_mover.cpp +++ b/samples/dynamic_mover.cpp @@ -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; } diff --git a/samples/dynamic_mover.h b/samples/dynamic_mover.h index 26d558b..fcb1ed1 100644 --- a/samples/dynamic_mover.h +++ b/samples/dynamic_mover.h @@ -138,6 +138,7 @@ public: b3WorldId m_worldId; b3BodyId m_groundId; b3BodyId m_moverId; + b3BodyId m_surfaceBodyId; b3JointId m_moverJointId; b3JointId m_pogoJointId; diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index 554b198..e4356ce 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -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 ) { diff --git a/src/mover_joint.c b/src/mover_joint.c index b6508ee..a8c9f7f 100644 --- a/src/mover_joint.c +++ b/src/mover_joint.c @@ -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 ); diff --git a/src/pogo_joint.c b/src/pogo_joint.c index dfa09ae..f59c3ca 100644 --- a/src/pogo_joint.c +++ b/src/pogo_joint.c @@ -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 ); diff --git a/src/recording_ops.inl b/src/recording_ops.inl index e66a593..d5c82f2 100644 --- a/src/recording_ops.inl +++ b/src/recording_ops.inl @@ -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. diff --git a/src/replay.c b/src/replay.c index 16b5124..0d77d47 100644 --- a/src/replay.c +++ b/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 );