diff --git a/util/Polygon.cc b/util/Polygon.cc
index a187cb5..eab5079 100644
--- a/util/Polygon.cc
+++ b/util/Polygon.cc
@@ -11,7 +11,7 @@
  * The angle is from vector 1 to vector 2, positive anticlockwise
  * The result is between -pi -> pi
 */
-double angle2D(double x1, double y1, double x2, double y2)
+static double angle2D(double x1, double y1, double x2, double y2)
 {
     double dtheta, theta1, theta2;
 
@@ -26,8 +26,41 @@ double angle2D(double x1, double y1, double x2, double y2)
     return dtheta;
 }
 
-bool Polygon::containsPoint2D(const Vector & v) const
+static bool similarPoint(const Vector & v, const Vector & w)
 {
+    return FP_EQUAL(v[0], w[0]) && FP_EQUAL(v[1], w[1]) && FP_EQUAL(v[2], w[2]);
+}
+
+Polygon & Polygon::add(const Vector & v)
+{
+    int sz = size();
+    if (sz < 1 || !similarPoint(v, *(*this)[sz-1]))
+    {
+        push_back(new Vector(v));
+    }
+    return *this;
+}
+
+Polygon & Polygon::add(refptr<Vector> v)
+{
+    int sz = size();
+    if (sz < 1 || !similarPoint(*v, *(*this)[sz-1]))
+    {
+        push_back(v);
+    }
+    return *this;
+}
+
+bool Polygon::containsPoint2D(const Vector & v)
+{
+    if (size() < 3)
+    {
+        return false;
+    }
+    if (similarPoint(*(*this)[0], *(*this)[size()-1]))
+    {
+        pop_back();
+    }
     double angle_sum = 0.0;
     for (int i = 0, sz = size(); i < sz; i++)
     {
@@ -39,8 +72,16 @@ bool Polygon::containsPoint2D(const Vector & v) const
     return FP_EQUAL(angle_sum, 2.0 * M_PI);
 }
 
-bool Polygon::containsPointConvex(const Vector & v) const
+bool Polygon::containsPointConvex(const Vector & v)
 {
+    if (size() < 3)
+    {
+        return false;
+    }
+    if (similarPoint(*(*this)[0], *(*this)[size()-1]))
+    {
+        pop_back();
+    }
     double angle_sum = 0.0;
     for (int i = 0, sz = size(); i < sz; i++)
     {
diff --git a/util/Polygon.h b/util/Polygon.h
index 88e73ae..678e8c5 100644
--- a/util/Polygon.h
+++ b/util/Polygon.h
@@ -10,18 +10,10 @@
 class Polygon : public std::vector< refptr<Vector> >
 {
     public:
-        Polygon & add(const Vector & v)
-        {
-            push_back(new Vector(v));
-            return *this;
-        }
-        Polygon & add(refptr<Vector> v)
-        {
-            push_back(v);
-            return *this;
-        }
-        bool containsPoint2D(const Vector & v) const;
-        bool containsPointConvex(const Vector & v) const;
+        Polygon & add(const Vector & v);
+        Polygon & add(refptr<Vector> v);
+        bool containsPoint2D(const Vector & v);
+        bool containsPointConvex(const Vector & v);
 };
 
 #endif