diff --git a/samples/dynamic_mover.cpp b/samples/dynamic_mover.cpp index 8138c4f..f1e4fa6 100644 --- a/samples/dynamic_mover.cpp +++ b/samples/dynamic_mover.cpp @@ -76,6 +76,8 @@ DynamicMover::DynamicMover() m_castResult = {}; m_velocity = b3Vec3_zero; + m_timeSliding = 0.0f; + m_slideDuration = 0.2f; m_onGround = false; m_walkable = false; m_jumping = false; @@ -332,16 +334,29 @@ void DynamicMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 if ( m_castResult.hit ) { + // Is the ground too steep to walk? + m_walkable = m_castResult.normal.y >= m_minGroundNormalY; + // 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; + if ( m_onGround ) + { + if ( m_walkable ) + { + m_timeSliding = b3MaxFloat( m_timeSliding - timeStep, 0.0f ); + } + else + { + m_timeSliding = b3MinFloat( m_timeSliding + timeStep, m_slideDuration ); + } + } } else { m_onGround = false; m_walkable = false; + m_timeSliding = b3MaxFloat( m_timeSliding - timeStep, 0.0f ); } // Connect the mover joint to platform. Acts like friction. @@ -388,7 +403,15 @@ void DynamicMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 float maxForce; if ( m_onGround ) { - maxForce = m_walkable ? m_maxGroundForce : 0.0f; + if ( m_slideDuration > 0.0f ) + { + float scale = b3ClampFloat( 1.0f - m_timeSliding / m_slideDuration, 0.0f, 1.0f ); + maxForce = scale * m_maxGroundForce; + } + else + { + maxForce = m_walkable ? m_maxGroundForce : 0.0f; + } } else { diff --git a/samples/dynamic_mover.h b/samples/dynamic_mover.h index fd73910..26d558b 100644 --- a/samples/dynamic_mover.h +++ b/samples/dynamic_mover.h @@ -144,6 +144,8 @@ public: DynamicMoverCastResult m_castResult; b3Vec3 m_velocity; + float m_timeSliding; + float m_slideDuration; bool m_onGround; bool m_walkable; bool m_jumping; diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index e38dc5c..554b198 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -946,9 +946,20 @@ public: b3CreateRevoluteJoint( m_worldId, &jointDef ); } - // Ramps + // Ramp to bridge. 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 ); + + // Wall + AddRamp( { 20.0f, 3.0f, -40.0f }, { 20.0f, -0.5f, -40.0f }, 5.0f, b3_colorLightSteelBlue ); + + // Ramp + AddRamp( { 35.0f, 3.0f, -46.0f }, { 35.0f, -0.5f, -39.5f }, 5.0f, b3_colorLightSteelBlue ); + + // Steep + AddRamp( { 50.0f, 3.0f, -42.5f }, { 50.0f, -0.5f, -39.5f }, 5.0f, b3_colorLightSteelBlue ); + + // Curb + AddRamp( { 65.0f, 0.25f, -40.0f }, { 65.0f, -0.5f, -40.0f }, 5.0f, b3_colorLightSteelBlue ); // Ledges for ( int i = 0; i < 2; ++i ) @@ -1795,6 +1806,8 @@ public: 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 ); + + DrawTextLine( "time sliding %.2f", m_mover.m_timeSliding ); } static Sample* Create( SampleContext* context ) diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index 08e8847..d908f5c 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -1722,13 +1722,17 @@ public: b3Vec3 axis = b3Normalize( b3Cross( u, v ) ); b3Vec3 w = b3Cross( axis, u ); constexpr int segmentCount = 96; + b3CosSin cs = b3ComputeCosSin( 2.0f * B3_PI / segmentCount ); + float x1 = 1.0f, y1 = 0.0f; b3Pos previous = GaussPoint( u ); - for ( int i = 1; i <= segmentCount; ++i ) + for ( int i = 0; i < segmentCount; ++i ) { - float angle = 2.0f * B3_PI * i / segmentCount; - b3CosSin cs = b3ComputeCosSin( angle ); - b3Pos point = GaussPoint( b3Add( b3MulSV( cs.cosine, u ), b3MulSV( cs.sine, w ) ) ); + float x2 = cs.cosine * x1 - cs.sine * y1; + float y2 = cs.sine * x1 + cs.cosine * y1; + b3Pos point = GaussPoint( b3Add( b3MulSV( x2, u ), b3MulSV( y2, w ) ) ); DrawLineEx( previous, point, color, 1.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED ); + x1 = x2; + y1 = y2; previous = point; } } diff --git a/src/mover_joint.c b/src/mover_joint.c index 3f269e5..b6508ee 100644 --- a/src/mover_joint.c +++ b/src/mover_joint.c @@ -205,8 +205,32 @@ static b3HexColor b3LerpColor( b3HexColor a, b3HexColor b, float t ) return (b3HexColor)result; } -void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA, - b3WorldTransform transformB ) +static void b3DrawArrow( b3DebugDraw* draw, b3Pos a, b3Pos b, b3Pos planeNormal, float size, + b3HexColor color ) +{ + draw->DrawSegmentFcn( a, b, color, draw->context ); + + b3Vec3 shaft = b3Sub( b, a ); + float shaftLen = b3Length( shaft ); + if ( shaftLen < 1e-6f ) + { + return; + } + + b3Vec3 dir = { shaft.x / shaftLen, shaft.y / shaftLen, shaft.z / shaftLen }; + float headLen = size; + + 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 ) ); + draw->DrawSegmentFcn( b, tip1, color, draw->context ); + draw->DrawSegmentFcn( b, tip2, color, draw->context ); +} + +void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, + b3WorldTransform transformA, b3WorldTransform transformB ) { B3_ASSERT( base->type == b3_moverJoint ); B3_UNUSED( transformA ); @@ -227,30 +251,39 @@ void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3Wo b3Vec3 error = b3Sub( b3Sub( vB, vA ), joint->linearVelocity ); b3Vec2 planeError = { b3Dot( error, axisX ), b3Dot( error, axisZ ) }; - b3Vec2 planeTarget = { b3Dot( joint->linearVelocity, axisX ), b3Dot( joint->linearVelocity, axisZ ) }; + b3Vec2 planeTarget = { 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_colorLimeGreen, b3_colorRed, t ); + b3HexColor color = b3LerpColor( b3_colorWhite, b3_colorBlue, t ); + float radius = draw->jointScale; + b3Vec3 rx = b3MulSV( radius, axisX ); + b3Vec3 rz = b3MulSV( radius, axisZ ); const int segmentCount = 32; - b3Pos p1 = b3OffsetPos( frameB.p, axisX ); - for ( int i = 1; i <= segmentCount; ++i ) + b3CosSin cs = b3ComputeCosSin( 2.0f * B3_PI / segmentCount ); + float x1 = 1.0f, y1 = 0.0f; + b3Pos p1 = b3OffsetPos( frameB.p, rx ); + for ( int i = 0; i < segmentCount; ++i ) { - float angle = 2.0f * B3_PI * i / segmentCount; - b3CosSin cs = b3ComputeCosSin( angle ); - b3Pos p2 = b3OffsetPos( frameB.p, b3Blend2( cs.cosine, axisX, cs.sine, axisZ ) ); + float x2 = cs.cosine * x1 - cs.sine * y1; + float y2 = cs.sine * x1 + cs.cosine * y1; + b3Pos p2 = b3OffsetPos( frameB.p, b3Blend2( x2, rx, y2, rz ) ); draw->DrawSegmentFcn( p1, p2, color, draw->context ); + x1 = x2; + y1 = y2; p1 = p2; } if ( joint->maxForce > 0.0f ) { + b3Vec3 force = b3Blend2( joint->impulse.x, rx, joint->impulse.y, rz ); + b3Vec3 dir = b3Normalize( force ); + b3Pos origin = b3OffsetPos( frameB.p, b3MulSV( -radius, dir ) ); float scale = world->inv_h / joint->maxForce; - b3Vec3 force = b3Blend2( scale * joint->impulse.x, axisX, scale * joint->impulse.y, axisZ ); - b3Pos tip = b3OffsetPos( frameB.p, force ); - draw->DrawSegmentFcn( frameB.p, tip, b3_colorAzure, draw->context ); - draw->DrawPointFcn( tip, 6.0f, b3_colorAzure, draw->context ); + b3Pos tip = b3OffsetPos( origin, b3MulSV( scale, force ) ); + b3DrawArrow( draw, origin, tip, matrixB.cy, 0.1f * draw->jointScale, b3_colorWhite ); } } diff --git a/src/pogo_joint.c b/src/pogo_joint.c index bc69e92..dfa09ae 100644 --- a/src/pogo_joint.c +++ b/src/pogo_joint.c @@ -282,8 +282,8 @@ void b3SolvePogoJoint( b3JointSim* base, b3StepContext* context, bool useBias ) } } -void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA, - b3WorldTransform transformB, float scale ) +void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, + b3WorldTransform transformA, b3WorldTransform transformB, float scale ) { B3_ASSERT( base->type == b3_pogoJoint ); @@ -337,7 +337,7 @@ void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3Wor { float side = i == segmentCount ? 0.0f : ( i & 1 ) ? width : -width; float t = (float)i / (float)segmentCount; - b3Pos p2 = b3OffsetPos( pB, b3MulAdd( b3MulSV( -t * length, axis ), side, perp1 ) ); + b3Pos p2 = b3OffsetPos( pB, b3MulAdd( b3MulSV( -t * length, axis ), side, perp2 ) ); draw->DrawSegmentFcn( p1, p2, color, draw->context ); p1 = p2; } @@ -345,10 +345,12 @@ void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3Wor // Ground plane b3Pos rest = b3OffsetPos( pB, b3MulSV( -joint->restLength, axis ) ); float tick = 1.5f * width; - draw->DrawSegmentFcn( b3OffsetPos( rest, b3MulSV( -tick, perp1 ) ), b3OffsetPos( rest, b3MulSV( tick, perp1 ) ), - b3_colorWhite, draw->context ); - draw->DrawSegmentFcn( b3OffsetPos( rest, b3MulSV( -tick, perp2 ) ), b3OffsetPos( rest, b3MulSV( tick, perp2 ) ), - b3_colorWhite, draw->context ); + draw->DrawSegmentFcn( b3OffsetPos( rest, b3MulSV( -tick, perp1 ) ), + b3OffsetPos( rest, b3MulSV( tick, perp1 ) ), b3_colorWhite, + draw->context ); + draw->DrawSegmentFcn( b3OffsetPos( rest, b3MulSV( -tick, perp2 ) ), + b3OffsetPos( rest, b3MulSV( tick, perp2 ) ), b3_colorWhite, + draw->context ); if ( b3Length( b3SubPos( pA, foot ) ) > B3_LINEAR_SLOP ) { @@ -356,13 +358,14 @@ void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3Wor } draw->DrawPointFcn( pA, 6.0f, color, draw->context ); - draw->DrawSegmentFcn( pA, b3OffsetPos( pA, b3MulSV( 0.25f * scale, joint->normal ) ), b3_colorOrange, draw->context ); + draw->DrawSegmentFcn( pA, b3OffsetPos( pA, b3MulSV( 0.5f * scale, joint->normal ) ), + b3_colorOrange, draw->context ); if ( draw->drawJointExtras ) { char buffer[96]; - snprintf( buffer, 96, "length = %.3f, rest = %.3f, f = %.1f, v = %.3f", length, joint->restLength, force, - joint->velocity ); + snprintf( buffer, 96, "length = %.3f, rest = %.3f, f = %.1f, v = %.3f", length, + joint->restLength, force, joint->velocity ); draw->DrawStringFcn( pB, buffer, color, draw->context ); } } diff --git a/src/wheel_joint.c b/src/wheel_joint.c index 05665e9..fb229c3 100644 --- a/src/wheel_joint.c +++ b/src/wheel_joint.c @@ -1072,14 +1072,16 @@ void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform tra float upper = joint->upperSteeringLimit; b3CosSin cs = b3ComputeCosSin( lower ); + b3CosSin delta = b3ComputeCosSin( ( upper - lower ) / sliceCount ); b3Pos vertex1 = b3TransformWorldPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); for ( int index = 0; index < sliceCount; ++index ) { - float t2 = ( index + 1.0f ) / sliceCount; - float phi = b3LerpFloat( lower, upper, t2 ); + float cosine = delta.cosine * cs.cosine - delta.sine * cs.sine; + float sine = delta.sine * cs.cosine + delta.cosine * cs.sine; + cs.cosine = cosine; + cs.sine = sine; - cs = b3ComputeCosSin( phi ); b3Pos vertex2 = b3TransformWorldPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); if ( index == 0 )