From 778041a43f6ba564558ef7e60e07062277bcca25 Mon Sep 17 00:00:00 2001
From: vng <viktor.govako@gmail.com>
Date: Fri, 21 Jan 2011 10:35:45 +0200
Subject: [PATCH] - IsPolygonCCW more stable now - IsSegmentInCone processes
 '3-points in line' correcly now

---
 geometry/geometry_tests/polygon_test.cpp | 15 +++++++++-
 geometry/polygon.hpp                     | 38 +++++++++++++++++++-----
 2 files changed, 45 insertions(+), 8 deletions(-)

diff --git a/geometry/geometry_tests/polygon_test.cpp b/geometry/geometry_tests/polygon_test.cpp
index 1a74ed17da..9f475d9b7a 100644
--- a/geometry/geometry_tests/polygon_test.cpp
+++ b/geometry/geometry_tests/polygon_test.cpp
@@ -123,7 +123,7 @@ namespace
   }
 }
 
-UNIT_TEST(IsPolygonCCW)
+UNIT_TEST(IsPolygonCCW_Smoke)
 {
   P arr1[] = { P(1, 1), P(2, 0), P(3, 2) };
   TestPolygonCCW(arr1, arr1 + ARRAY_SIZE(arr1));
@@ -134,3 +134,16 @@ UNIT_TEST(IsPolygonCCW)
   P arr3[] = { P(0, 1), P(1, 1), P(1, 0), P(2, 0), P(2, 1), P(1, 1), P(1, 2), P(0, 2) };
   TestPolygonCCW(arr3, arr3 + ARRAY_SIZE(arr3));
 }
+
+UNIT_TEST(IsPolygonCCW_DataSet)
+{
+  P arr[] = { P(27.3018836975098, 61.7740631103516), P(27.2981071472168, 61.7816162109375), P(27.2962188720703, 61.7831611633301),
+              P(27.293815612793, 61.7814445495605), P(27.2926139831543, 61.783332824707), P(27.2919273376465, 61.787109375),
+              P(27.2948455810547, 61.7865943908691), P(27.2958755493164, 61.7883110046387), P(27.3001670837402, 61.779899597168),
+              P(27.3036003112793, 61.7771530151367), P(27.3015403747559, 61.7747497558594) };
+
+  size_t const n = ARRAY_SIZE(arr);
+  if (!IsPolygonCCW(arr, arr + n))
+    reverse(arr, arr + n);
+  TestPolygonCCW(arr, arr + n);
+}
diff --git a/geometry/polygon.hpp b/geometry/polygon.hpp
index d2734f8b15..504320417a 100644
--- a/geometry/polygon.hpp
+++ b/geometry/polygon.hpp
@@ -57,6 +57,7 @@ template <typename IterT> bool IsPolygonCCW(IterT beg, IterT end)
 {
   ASSERT ( TestPolygonPreconditions(beg, end), () );
 
+  // find the most down (left) point
   double minY = numeric_limits<double>::max();
   IterT iRes;
   for (IterT i = beg; i != end; ++i)
@@ -68,8 +69,26 @@ template <typename IterT> bool IsPolygonCCW(IterT beg, IterT end)
     }
   }
 
-  return CrossProduct(*iRes - *PrevIterInCycle(iRes, beg, end),
-                      *NextIterInCycle(iRes, beg, end) - *iRes) > 0;
+  double cp = CrossProduct(*iRes - *PrevIterInCycle(iRes, beg, end),
+                           *NextIterInCycle(iRes, beg, end) - *iRes);
+  if (cp != 0.0)
+    return (cp > 0.0);
+
+  // fid the most up (left) point
+  double maxY = numeric_limits<double>::min();
+  for (IterT i = beg; i != end; ++i)
+  {
+    if ((*i).y > maxY || ((*i).y == maxY && (*i).x < (*iRes).x))
+    {
+      iRes = i;
+      maxY = (*i).y;
+    }
+  }
+
+  cp =  CrossProduct(*iRes - *PrevIterInCycle(iRes, beg, end),
+                     *NextIterInCycle(iRes, beg, end) - *iRes);
+  ASSERT_NOT_EQUAL ( cp, 0.0, () );
+  return (cp > 0.0);
 }
 
 /// Is segment (v, v1) in cone (vPrev, v, vNext)?
@@ -80,18 +99,23 @@ template <typename PointT> bool IsSegmentInCone(PointT v, PointT v1, PointT vPre
   PointT const edgeL = vPrev - v;
   PointT const edgeR = vNext - v;
   double const cpLR = CrossProduct(edgeR, edgeL);
-  //ASSERT(!my::AlmostEqual(cpLR, 0.0),
-  ASSERT_NOT_EQUAL(cpLR, 0.0,
-         ("vPrev, v, vNext shouldn't be collinear!", edgeL, edgeR, v, v1, vPrev, vNext));
+
+  if (my::AlmostEqual(cpLR,  0.0))
+  {
+    // Points vPrev, v, vNext placed on one line;
+    // use property that polygon has CCW orientation.
+    return CrossProduct(vNext - vPrev, v1 - vPrev) > 0.0;
+  }
+
   if (cpLR > 0)
   {
     // vertex is convex
-    return CrossProduct(diff, edgeR) < 0 && CrossProduct(diff, edgeL) > 0;
+    return CrossProduct(diff, edgeR) < 0 && CrossProduct(diff, edgeL) > 0.0;
   }
   else
   {
     // vertex is reflex
-    return CrossProduct(diff, edgeR) < 0 || CrossProduct(diff, edgeL) > 0;
+    return CrossProduct(diff, edgeR) < 0 || CrossProduct(diff, edgeL) > 0.0;
   }
 }