From c87b136522aa071a7e118214fbedd69a2ad08a75 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tomasz=20W=C5=82ostowski?= <tomasz.wlostowski@cern.ch>
Date: Wed, 18 Feb 2015 00:40:11 +0100
Subject: [PATCH] geometry: new methods and fixes in SEG class

---
 common/geometry/seg.cpp |  2 ++
 include/geometry/seg.h  | 65 ++++++++++++++++++++++++++++++++++++-----
 2 files changed, 59 insertions(+), 8 deletions(-)

diff --git a/common/geometry/seg.cpp b/common/geometry/seg.cpp
index daff746f98..8ec1565fb5 100644
--- a/common/geometry/seg.cpp
+++ b/common/geometry/seg.cpp
@@ -35,6 +35,7 @@ bool SEG::PointCloserThan( const VECTOR2I& aP, int aDist ) const
 {
     VECTOR2I d = B - A;
     ecoord dist_sq = (ecoord) aDist * aDist;
+    ecoord dist_sq_thr = (ecoord) (aDist + 1) * (aDist + 1);
 
     SEG::ecoord l_squared = d.Dot( d );
     SEG::ecoord t = d.Dot( aP - A );
@@ -60,6 +61,7 @@ bool SEG::PointCloserThan( const VECTOR2I& aP, int aDist ) const
 
         if( num > ( dist_sq + 100 ) )
             return false;
+
         else if( num < ( dist_sq - 100 ) )
             return true;
     }
diff --git a/include/geometry/seg.h b/include/geometry/seg.h
index 43ceec3b41..855271a512 100644
--- a/include/geometry/seg.h
+++ b/include/geometry/seg.h
@@ -206,6 +206,13 @@ public:
         return sqrt( SquaredDistance( aP ) );
     }
 
+    void CanonicalCoefs ( ecoord& qA, ecoord& qB, ecoord& qC) const
+    {
+        qA = A.y - B.y;
+        qB = B.x - A.x;
+        qC = -qA * A.x - qB * A.y;
+    }
+
     /**
      * Function Collinear()
      *
@@ -215,16 +222,38 @@ public:
      */
     bool Collinear( const SEG& aSeg ) const
     {
-        ecoord qa = A.y - B.y;
-        ecoord qb = B.x - A.x;
-        ecoord qc = -qa * A.x - qb * A.y;
-
+        ecoord qa, qb, qc;
+        CanonicalCoefs ( qa, qb, qc );
+        
         ecoord d1 = std::abs( aSeg.A.x * qa + aSeg.A.y * qb + qc );
         ecoord d2 = std::abs( aSeg.B.x * qa + aSeg.B.y * qb + qc );
 
         return ( d1 <= 1 && d2 <= 1 );
     }
 
+    bool ApproxCollinear( const SEG& aSeg ) const
+    {
+        ecoord p, q, r;
+        CanonicalCoefs ( p, q, r );
+    
+        ecoord dist1 = ( p * aSeg.A.x + q * aSeg.A.y + r ) / sqrt( p * p + q * q );     
+        ecoord dist2 = ( p * aSeg.B.x + q * aSeg.B.y + r ) / sqrt( p * p + q * q );     
+
+        return std::abs(dist1) <= 1 && std::abs(dist2) <= 1;
+    }
+
+    bool ApproxParallel ( const SEG& aSeg ) const
+    {
+        ecoord p, q, r;
+        CanonicalCoefs ( p, q, r );
+        
+        ecoord dist1 = ( p * aSeg.A.x + q * aSeg.A.y + r ) / sqrt( p * p + q * q );     
+        ecoord dist2 = ( p * aSeg.B.x + q * aSeg.B.y + r ) / sqrt( p * p + q * q );     
+
+        return std::abs(dist1 - dist2) <= 1;
+    }
+
+
     bool Overlaps( const SEG& aSeg ) const
     {
         if( aSeg.A == aSeg.B ) // single point corner case
@@ -262,6 +291,8 @@ public:
         return ( A - B ).SquaredEuclideanNorm();
     }
 
+    ecoord TCoef ( const VECTOR2I& aP ) const;
+
     /**
      * Function Index()
      *
@@ -277,7 +308,11 @@ public:
 
     bool PointCloserThan( const VECTOR2I& aP, int aDist ) const;
 
-//    friend std::ostream& operator<<( std::ostream& stream, const SEG& aSeg );
+    void Reverse()
+    {
+        std::swap ( A, B );
+    }
+
 private:
     bool ccw( const VECTOR2I& aA, const VECTOR2I& aB, const VECTOR2I &aC ) const;
 
@@ -288,9 +323,18 @@ private:
 
 inline VECTOR2I SEG::LineProject( const VECTOR2I& aP ) const
 {
-    // fixme: numerical errors for large integers
-    assert( false );
-    return VECTOR2I( 0, 0 );
+    VECTOR2I d = B - A;
+    ecoord l_squared = d.Dot( d );
+
+    if( l_squared == 0 )
+        return A;
+
+    ecoord t = d.Dot( aP - A );
+
+    int xp = rescale( t, (ecoord)d.x, l_squared );
+    int yp = rescale( t, (ecoord)d.y, l_squared );
+
+    return A + VECTOR2I( xp, yp );
 }
 
 
@@ -305,6 +349,11 @@ inline int SEG::LineDistance( const VECTOR2I& aP, bool aDetermineSide ) const
     return aDetermineSide ? dist : abs( dist );
 }
 
+inline SEG::ecoord SEG::TCoef ( const VECTOR2I& aP ) const
+{
+    VECTOR2I d = B - A;
+    return d.Dot ( aP - A);
+}
 
 inline const VECTOR2I SEG::NearestPoint( const VECTOR2I& aP ) const
 {