From 79fe4a0acb97ed502a4606f3969c190ccb06dc84 Mon Sep 17 00:00:00 2001
From: Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
Date: Tue, 1 Mar 2016 08:44:02 -0500
Subject: [PATCH] More accurate pushout force calculation for circle-segment
 collisions. (fixes lp:1551579)

---
 common/geometry/shape_collisions.cpp | 22 +++++++++++++++++-----
 include/geometry/shape_circle.h      |  2 +-
 include/geometry/shape_segment.h     |  2 +-
 3 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/common/geometry/shape_collisions.cpp b/common/geometry/shape_collisions.cpp
index bb4f330a1d..773dcc9a2d 100644
--- 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
index 700f61d860..a915ef97ce 100644
--- a/include/geometry/shape_circle.h
+++ b/include/geometry/shape_circle.h
@@ -63,7 +63,7 @@ public:
     {
         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
index 217368e72f..71ed5b17c4 100644
--- a/include/geometry/shape_segment.h
+++ b/include/geometry/shape_segment.h
@@ -59,7 +59,7 @@ public:
 
     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 )