more accurate pushout force calculation for circle-segment collisions

dolezaljan · Mar 1, 2016 · 0fa71d8 · 0fa71d8
1 parent bd80063
commit 0fa71d8
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 7 deletions.
diff --git a/common/geometry/shape_collisions.cpp b/common/geometry/shape_collisions.cpp
@@ -121,14 +121,26 @@ static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int aC
 
 static VECTOR2I pushoutForce( const SHAPE_CIRCLE& aA, const SEG& aB, int aClearance )
 {
-    VECTOR2I nearest = aB.NearestPoint( aA.GetCenter() );
-    VECTOR2I f (0, 0);
+    VECTOR2I f( 0, 0 );
 
-    int dist = ( nearest - aA.GetCenter() ).EuclideanNorm();
-    int min_dist = aClearance + aA.GetRadius();
+    const VECTOR2I c = aA.GetCenter();
+    const VECTOR2I nearest = aB.NearestPoint( c );
+
+    const int r = aA.GetRadius();
+
+    int dist = ( nearest - c ).EuclideanNorm();
+    int min_dist = aClearance + r;
 
     if( dist < min_dist )
-        f = ( aA.GetCenter() - nearest ).Resize ( min_dist - dist + 10 );
+    {
+        for( int corr = 0; corr < 5; corr++ )
+        {
+            f = ( aA.GetCenter() - nearest ).Resize( min_dist - dist + corr );
+
+            if( aB.Distance( c + f ) >= min_dist )
+                break;
+        }
+    }
 
     return f;
 }

diff --git a/include/geometry/shape_circle.h b/include/geometry/shape_circle.h
@@ -63,7 +63,7 @@ class SHAPE_CIRCLE : public SHAPE
     {
         int rc = aClearance + m_radius;
 
-        return aSeg.Distance( m_center ) <= rc;
+        return aSeg.Distance( m_center ) < rc;
     }
 
     void SetRadius( int aRadius )

diff --git a/include/geometry/shape_segment.h b/include/geometry/shape_segment.h
@@ -59,7 +59,7 @@ class SHAPE_SEGMENT : public SHAPE {
 
     bool Collide( const VECTOR2I& aP, int aClearance = 0 ) const
     {
-        return m_seg.Distance( aP ) <= ( m_width + 1 ) / 2 + aClearance;
+        return m_seg.Distance( aP ) < ( m_width + 1 ) / 2 + aClearance;
     }
 
     void SetSeg( const SEG& aSeg )