From 9cf885c3831600dd0034721b6e270af1e5d47e20 Mon Sep 17 00:00:00 2001
From: Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
Date: Fri, 12 Jun 2015 17:11:50 +0200
Subject: [PATCH] common/geometry: rouding error fixes

---
 common/geometry/shape_collisions.cpp | 31 ++++++++++++++++++++++++++++
 include/geometry/shape_rect.h        | 29 +-------------------------
 include/geometry/shape_segment.h     |  6 +++---
 3 files changed, 35 insertions(+), 31 deletions(-)

diff --git a/common/geometry/shape_collisions.cpp b/common/geometry/shape_collisions.cpp
index f3bac90524..a5dbbfa258 100644
--- a/common/geometry/shape_collisions.cpp
+++ b/common/geometry/shape_collisions.cpp
@@ -350,3 +350,34 @@ bool SHAPE::Collide( const SHAPE* aShape, int aClerance ) const
 
     return CollideShapes( this, aShape, aClerance, false, dummy );
 }
+
+bool SHAPE_RECT::Collide( const SEG& aSeg, int aClearance ) const
+    {
+        //VECTOR2I pmin = VECTOR2I( std::min( aSeg.a.x, aSeg.b.x ), std::min( aSeg.a.y, aSeg.b.y ) );
+        //VECTOR2I pmax = VECTOR2I( std::max( aSeg.a.x, aSeg.b.x ), std::max( aSeg.a.y, aSeg.b.y ));
+        //BOX2I r( pmin, VECTOR2I( pmax.x - pmin.x, pmax.y - pmin.y ) );
+
+        //if( BBox( 0 ).SquaredDistance( r ) > aClearance * aClearance )
+        //    return false;
+
+        if( BBox( 0 ).Contains( aSeg.A ) || BBox( 0 ).Contains( aSeg.B ) )
+            return true;
+
+        VECTOR2I vts[] = { VECTOR2I( m_p0.x, m_p0.y ),
+                           VECTOR2I( m_p0.x, m_p0.y + m_h ),
+                           VECTOR2I( m_p0.x + m_w, m_p0.y + m_h ),
+                           VECTOR2I( m_p0.x + m_w, m_p0.y ),
+                           VECTOR2I( m_p0.x, m_p0.y ) };
+
+        for( int i = 0; i < 4; i++ )
+        {
+            SEG s( vts[i], vts[i + 1], i );
+
+            int64_t dist = s.Distance( aSeg );
+
+            if( s.Distance( aSeg ) < aClearance )
+                return true;
+        }
+
+        return false;
+    }
\ No newline at end of file
diff --git a/include/geometry/shape_rect.h b/include/geometry/shape_rect.h
index 43bc3c5e01..682abcfeff 100644
--- a/include/geometry/shape_rect.h
+++ b/include/geometry/shape_rect.h
@@ -90,34 +90,7 @@ public:
     }
 
     /// @copydoc SHAPE::Collide()
-    bool Collide( const SEG& aSeg, int aClearance = 0 ) const
-    {
-        //VECTOR2I pmin = VECTOR2I( std::min( aSeg.a.x, aSeg.b.x ), std::min( aSeg.a.y, aSeg.b.y ) );
-        //VECTOR2I pmax = VECTOR2I( std::max( aSeg.a.x, aSeg.b.x ), std::max( aSeg.a.y, aSeg.b.y ));
-        //BOX2I r( pmin, VECTOR2I( pmax.x - pmin.x, pmax.y - pmin.y ) );
-
-        //if( BBox( 0 ).SquaredDistance( r ) > aClearance * aClearance )
-        //    return false;
-
-        if( BBox( 0 ).Contains( aSeg.A ) || BBox( 0 ).Contains( aSeg.B ) )
-            return true;
-
-        VECTOR2I vts[] = { VECTOR2I( m_p0.x, m_p0.y ),
-                           VECTOR2I( m_p0.x, m_p0.y + m_h ),
-                           VECTOR2I( m_p0.x + m_w, m_p0.y + m_h ),
-                           VECTOR2I( m_p0.x + m_w, m_p0.y ),
-                           VECTOR2I( m_p0.x, m_p0.y ) };
-
-        for( int i = 0; i < 4; i++ )
-        {
-            SEG s( vts[i], vts[i + 1], i );
-
-            if( s.Distance( aSeg ) <= aClearance )
-                return true;
-        }
-
-        return false;
-    }
+    bool Collide( const SEG& aSeg, int aClearance = 0 ) const;
 
     /**
      * Function GetPosition()
diff --git a/include/geometry/shape_segment.h b/include/geometry/shape_segment.h
index 6dc3adea2f..94e9c5a93a 100644
--- a/include/geometry/shape_segment.h
+++ b/include/geometry/shape_segment.h
@@ -49,17 +49,17 @@ public:
 
     const BOX2I BBox( int aClearance = 0 ) const
     {
-        return BOX2I( m_seg.A, m_seg.B - m_seg.A ).Inflate( aClearance + m_width / 2 );
+        return BOX2I( m_seg.A, m_seg.B - m_seg.A ).Inflate( aClearance + (m_width + 1) / 2 );
     }
 
     bool Collide( const SEG& aSeg, int aClearance = 0 ) const
     {
-        return m_seg.Distance( aSeg ) <= m_width / 2 + aClearance;
+        return m_seg.Distance( aSeg ) < (m_width + 1) / 2 + aClearance;
     }
 
     bool Collide( const VECTOR2I& aP, int aClearance = 0 ) const
     {
-        return m_seg.Distance( aP ) <= m_width / 2 + aClearance;
+        return m_seg.Distance( aP ) <= (m_width + 1) / 2 + aClearance;
     }
 
     void SetSeg( const SEG& aSeg )