diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index 5cf7796..370315c 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -1790,6 +1790,99 @@ public: static int sampleShapeCastDebug = RegisterSample( "Collision", "Shape Cast Debug", ShapeCastDebug::Create ); +// Box resting a few millimeters above a ground box, inside the cast target distance. +// With encroach enabled the box can be cast away from the surface. Without it the +// cast reports an initial overlap and the box is stuck. +class EncroachCast : public Sample +{ +public: + explicit EncroachCast( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 120.0f, 25.0f, 8.0f, { 0.0f, 0.5f, 0.0f } ); + } + + m_ground = b3MakeTransformedBoxHull( 3.0f, 0.25f, 3.0f, { { 0.0f, -0.25f, 0.0f }, b3Quat_identity } ); + m_box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + m_gap = 4.0f; + m_angle = 0.5f; + m_distance = 2.0f; + m_canEncroach = true; + } + + bool HasSolverControls() const override + { + return false; + } + + bool DrawControls() override + { + ImGui::Checkbox( "Can Encroach", &m_canEncroach ); + ImGui::PushItemWidth( 12.0f * ImGui::GetFontSize() ); + ImGui::SliderFloat( "Gap (mm)", &m_gap, 0.0f, 20.0f, "%.1f" ); + ImGui::SliderFloat( "Angle", &m_angle, -5.0f, 5.0f, "%.1f deg" ); + ImGui::SliderFloat( "Distance", &m_distance, 0.0f, 4.0f, "%.1f" ); + ImGui::PopItemWidth(); + return true; + } + + void Render() override + { + Sample::Render(); + + DrawGroundGrid( 20 ); + + b3Transform transform = { { 0.0f, 0.5f + 0.001f * m_gap, 0.0f }, b3Quat_identity }; + float radians = m_angle * B3_DEG_TO_RAD; + b3Vec3 translation = m_distance * b3Vec3{ cosf( radians ), sinf( radians ), 0.0f }; + + b3ShapeCastPairInput input = {}; + input.proxyA = { m_ground.boxPoints, m_ground.base.vertexCount, 0.0f }; + input.proxyB = { m_box.boxPoints, m_box.base.vertexCount, 0.0f }; + input.transform = transform; + input.translationB = translation; + input.maxFraction = 1.0f; + input.canEncroach = m_canEncroach; + + b3CastOutput output = b3ShapeCast( &input ); + + DrawHull( b3WorldTransform_identity, &m_ground.base, MakeColor( b3_colorSteelBlue ) ); + DrawHull( b3MakeWorldTransform( transform ), &m_box.base, MakeColor( b3_colorGreen ) ); + + float fraction = output.hit ? output.fraction : 1.0f; + b3Transform end = { transform.p + fraction * translation, transform.q }; + DrawHull( b3MakeWorldTransform( end ), &m_box.base, MakeColor( output.hit ? b3_colorRed : b3_colorGray ) ); + DrawLine( b3ToPos( transform.p ), b3ToPos( transform.p + translation ), MakeColor( b3_colorWhite ) ); + + if ( output.hit ) + { + b3Pos p = b3ToPos( output.point ); + DrawPoint( p, 8.0f, MakeColor( b3_colorRed ) ); + DrawLine( p, b3OffsetPos( p, 0.5f * output.normal ), MakeColor( b3_colorYellow ) ); + } + + DrawTextLine( "hit = %s, fraction = %.3f, iterations = %d", output.hit ? "true" : "false", output.fraction, + output.iterations ); + DrawTextLine( "Encroach applies to gaps up to %.2f mm", 1250.0f * B3_LINEAR_SLOP ); + } + + static Sample* Create( SampleContext* context ) + { + return new EncroachCast( context ); + } + + b3BoxHull m_ground; + b3BoxHull m_box; + float m_gap; + float m_angle; + float m_distance; + bool m_canEncroach; +}; + +static int sampleEncroachCast = RegisterSample( "Collision", "Encroach Cast", EncroachCast::Create ); + class DistanceDebug : public Sample { public: diff --git a/src/distance.c b/src/distance.c index ffe0c0e..57f0814 100644 --- a/src/distance.c +++ b/src/distance.c @@ -1087,6 +1087,36 @@ b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input ) { target = distanceOutput.distance - linearSlop; } + else if ( input->canEncroach && distanceOutput.distance > 0.0f ) + { + // This is for box movers and can't normally be reached by capsule + // movers or regular shape casts. + + // Near-target start without overlap: advance by first order while + // retaining a rest separation. Sweeps that cannot consume the free + // gap (receding, tangential, grazing) miss. + float retained = 0.5f * linearSlop; + float needed = distanceOutput.distance - retained; + float approach = -b3Dot( delta2, distanceOutput.normal ); + if ( approach <= b3MaxFloat( needed, 0.0f ) ) + { + // Moving away, no hit. + return output; + } + + float fraction = needed > 0.0f ? needed / approach : 0.0f; + if ( fraction >= input->maxFraction ) + { + // Still no hit because the max fraction is small. + return output; + } + + output.fraction = fraction; + output.point = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + output.normal = distanceOutput.normal; + output.hit = true; + return output; + } else { // Initial overlap diff --git a/test/test_distance.c b/test/test_distance.c index 97d3881..59254f5 100644 --- a/test/test_distance.c +++ b/test/test_distance.c @@ -79,6 +79,136 @@ static int ShapeCastTest( void ) return 0; } +// unit box with a segment resting 2mm from its +x face: inside the cast +// target distance, not overlapped +static b3ShapeCastPairInput EncroachCastInput( b3Vec3 translationB ) +{ + static const b3Vec3 vas[] = { { -1.0f, -1.0f, 0.0f }, { 1.0f, -1.0f, 0.0f }, { 1.0f, 1.0f, 0.0f }, { -1.0f, 1.0f, 0.0f } }; + static const b3Vec3 vbs[] = { + { 1.002f, -1.0f, 0.0f }, + { 1.002f, 1.0f, 0.0f }, + }; + + b3ShapeCastPairInput input; + input.proxyA = (b3ShapeProxy){ vas, ARRAY_COUNT( vas ), 0.0f }; + input.proxyB = (b3ShapeProxy){ vbs, ARRAY_COUNT( vbs ), 0.0f }; + input.transform = b3Transform_identity; + input.translationB = translationB; + input.maxFraction = 1.0f; + input.canEncroach = true; + return input; +} + +static int ShapeCastEncroachRecedeTest( void ) +{ + b3ShapeCastPairInput input = EncroachCastInput( (b3Vec3){ 1.0f, 0.0f, 0.0f } ); + b3CastOutput output = b3ShapeCast( &input ); + ENSURE( output.hit == false ); + + return 0; +} + +static int ShapeCastEncroachSlideTest( void ) +{ + b3ShapeCastPairInput input = EncroachCastInput( (b3Vec3){ 0.0f, 2.0f, 0.0f } ); + b3CastOutput output = b3ShapeCast( &input ); + ENSURE( output.hit == false ); + + return 0; +} + +static int ShapeCastEncroachClosingTest( void ) +{ + b3ShapeCastPairInput input = EncroachCastInput( (b3Vec3){ -1.0f, 0.0f, 0.0f } ); + b3CastOutput output = b3ShapeCast( &input ); + ENSURE( output.hit ); + ENSURE_SMALL( output.fraction, FLT_EPSILON ); + ENSURE_SMALL( output.normal.x - 1.0f, 0.001f ); + + return 0; +} + +static int ShapeCastEncroachGradedTest( void ) +{ + // 5mm gap: closing consumes the gap down to the retained rest separation + static const b3Vec3 vbs[] = { + { 1.005f, -1.0f, 0.0f }, + { 1.005f, 1.0f, 0.0f }, + }; + + b3ShapeCastPairInput input = EncroachCastInput( (b3Vec3){ -1.0f, 0.0f, 0.0f } ); + input.proxyB = (b3ShapeProxy){ vbs, ARRAY_COUNT( vbs ), 0.0f }; + b3CastOutput output = b3ShapeCast( &input ); + ENSURE( output.hit ); + ENSURE_SMALL( output.fraction - 0.0025f, 0.0005f ); + ENSURE_SMALL( output.normal.x - 1.0f, 0.001f ); + + return 0; +} + +static int ShapeCastEncroachMaxFractionTest( void ) +{ + // 5mm gap closes at fraction 0.0025 + static const b3Vec3 vbs[] = { + { 1.005f, -1.0f, 0.0f }, + { 1.005f, 1.0f, 0.0f }, + }; + + b3ShapeCastPairInput input = EncroachCastInput( (b3Vec3){ -1.0f, 0.0f, 0.0f } ); + input.proxyB = (b3ShapeProxy){ vbs, ARRAY_COUNT( vbs ), 0.0f }; + input.maxFraction = 0.002f; + b3CastOutput output = b3ShapeCast( &input ); + ENSURE( output.hit == false ); + + input.maxFraction = 0.003f; + output = b3ShapeCast( &input ); + ENSURE( output.hit ); + ENSURE_SMALL( output.fraction - 0.0025f, 0.0005f ); + + return 0; +} + +static int ShapeCastEncroachGrazeTest( void ) +{ + // mostly tangential slide with a slight closing component: free + static const b3Vec3 vbs[] = { + { 1.005f, -1.0f, 0.0f }, + { 1.005f, 1.0f, 0.0f }, + }; + + b3ShapeCastPairInput input = EncroachCastInput( (b3Vec3){ -0.001f, 2.0f, 0.0f } ); + input.proxyB = (b3ShapeProxy){ vbs, ARRAY_COUNT( vbs ), 0.0f }; + b3CastOutput output = b3ShapeCast( &input ); + ENSURE( output.hit == false ); + + return 0; +} + +static int ShapeCastStartTouchingDefaultTest( void ) +{ + b3ShapeCastPairInput input = EncroachCastInput( (b3Vec3){ 1.0f, 0.0f, 0.0f } ); + input.canEncroach = false; + b3CastOutput output = b3ShapeCast( &input ); + ENSURE( output.hit ); + + return 0; +} + +static int ShapeCastEncroachOverlapTest( void ) +{ + static const b3Vec3 vcs[] = { + { 0.5f, -1.0f, 0.0f }, + { 0.5f, 1.0f, 0.0f }, + }; + + b3ShapeCastPairInput input = EncroachCastInput( (b3Vec3){ 1.0f, 0.0f, 0.0f } ); + input.proxyB = (b3ShapeProxy){ vcs, ARRAY_COUNT( vcs ), 0.0f }; + b3CastOutput output = b3ShapeCast( &input ); + ENSURE( output.hit ); + + return 0; +} + static int TimeOfImpactTest( void ) { b3Vec3 vas[] = { { -1.0f, -1.0f }, { 1.0f, -1.0f }, { 1.0f, 1.0f }, { -1.0f, 1.0f } }; @@ -108,6 +238,14 @@ int DistanceTest( void ) RUN_SUBTEST( SegmentDistanceTest ); RUN_SUBTEST( ShapeDistanceTest ); RUN_SUBTEST( ShapeCastTest ); + RUN_SUBTEST( ShapeCastEncroachRecedeTest ); + RUN_SUBTEST( ShapeCastEncroachSlideTest ); + RUN_SUBTEST( ShapeCastEncroachClosingTest ); + RUN_SUBTEST( ShapeCastEncroachGradedTest ); + RUN_SUBTEST( ShapeCastEncroachMaxFractionTest ); + RUN_SUBTEST( ShapeCastEncroachGrazeTest ); + RUN_SUBTEST( ShapeCastStartTouchingDefaultTest ); + RUN_SUBTEST( ShapeCastEncroachOverlapTest ); RUN_SUBTEST( TimeOfImpactTest ); return 0;