diff --git a/src/org/poly2tri/Poly2Tri.scala b/src/org/poly2tri/Poly2Tri.scala
index ff18b4d..f9243ae 100644
--- a/src/org/poly2tri/Poly2Tri.scala
+++ b/src/org/poly2tri/Poly2Tri.scala
@@ -283,11 +283,9 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
   def selectModel(model: String) {
     model match {
       case "data/nazca_monkey.dat" => 
-        //CDT.clearPoint = 50
-        doCDT = false; drawCDT = false; drawcdtMesh = false
+        CDT.clearPoint = 50
         loadModel(nazcaMonkey, 4.5f, Point(400, 300), 1500)
       case "data/bird.dat" => 
-        doCDT = false; drawCDT = false; drawcdtMesh = false
         CDT.clearPoint = 80
         loadModel(bird, 25f, Point(400, 300), 350)
       case "data/i.snake" => 
@@ -307,8 +305,7 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
         CDT.clearPoint = 7
         loadModel(i18, 20f, Point(600f, 500f), 20)
       case "data/nazca_heron.dat" => 
-        doCDT = false; drawCDT = false; drawcdtMesh = false
-        //CDT.clearPoint = 7
+        CDT.clearPoint = 7
         loadModel(nazcaHeron, 4.5f, Point(400f, 300f), 1500) 
       case "data/tank.dat" => 
         //doCDT = false; drawCDT = false; drawcdtMesh = false
diff --git a/src/org/poly2tri/cdt/CDT.scala b/src/org/poly2tri/cdt/CDT.scala
index 6556216..c3ef4f2 100644
--- a/src/org/poly2tri/cdt/CDT.scala
+++ b/src/org/poly2tri/cdt/CDT.scala
@@ -269,17 +269,15 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
       // Apply constraint; traverse the AFront, and build triangles
       
       val ahead = (edge.p.x > edge.q.x)
-      
       val point1 = if(ahead) edge.q else edge.p
       val point2 = if(ahead) edge.p else edge.q
       
+      var node = aFront.locate(point1)
+      val first = node
+      
       val points = new ArrayBuffer[Point]
       // Neighbor triangles
       val nTriangles = new ArrayBuffer[Triangle]
-      
-      var node = aFront.locate(point1)
-      
-      val first = node
       nTriangles += node.triangle
       
       node = node.next
@@ -297,21 +295,31 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
       triangulate(points.toArray, endPoints, T)
       
       // Update advancing front 
-      aFront link (first, node, T.first)
+      
+      // Select edge triangle
+      var edgeTri: Triangle = null
+      T.foreach(t => 
+        if(t.contains(first.point, node.point))
+          edgeTri = t
+      )
+      assert(edgeTri != null)
+      
+      aFront link (first, node, edgeTri)
+      
+      // Mark constrained edge
+      edgeTri mark(edge.p, edge.q)
       
       // Update neighbors
       edgeNeighbors(nTriangles, T)
       
-       // Mark constrained edge
-      T.first mark(edge.p, edge.q)
-      
       // Return original triangle
       triangle
       
     } else { 
       
       // Mark constrained edge
-      firstTriangle mark(edge.p, edge.q)
+      if(firstTriangle != null)
+        firstTriangle mark(edge.p, edge.q)
       triangle
     }
     
@@ -439,7 +447,7 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
     val t2 = node.triangle
     
     val point = t1.points(0)
-    /*
+    
     val oPoint = t2 oppositePoint t1
     
     // Try to avoid creating degenerate triangles
@@ -468,13 +476,13 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
         aFront.insertLegalized(t1.points(1), t1, node)
         
     } else {
-      */
+      
       // Update neighbor
       t2.markNeighbor(t1) 
       // Update advancing front
       aFront.insert(point, t1, node)
 
-    //}
+    }
 
   }  
   
diff --git a/src/org/poly2tri/shapes/Triangle.scala b/src/org/poly2tri/shapes/Triangle.scala
index 5fb7be6..780eff4 100644
--- a/src/org/poly2tri/shapes/Triangle.scala
+++ b/src/org/poly2tri/shapes/Triangle.scala
@@ -286,10 +286,8 @@ class Triangle(val points: Array[Point]) {
   
   // Initial mark edges sweep
   def mark(p: Point, q: Point) {
-    if(contains(p) && contains(q)) {
-      markEdge(p, q)
-      markNeighborEdge(p, q)
-    }
+     markEdge(p, q)
+     markNeighborEdge(p, q)
   }
   
   // Finalize edge marking
@@ -315,7 +313,7 @@ class Triangle(val points: Array[Point]) {
       edges(1) = true
     } else if ((q == points(1) && p == points(2)) || (q == points(2) && p == points(1))){
       edges(0) = true
-    }
+    } 
   }
   
   def area = {
diff --git a/src/org/poly2tri/utils/Util.scala b/src/org/poly2tri/utils/Util.scala
index ec5e9b9..ddb065e 100644
--- a/src/org/poly2tri/utils/Util.scala
+++ b/src/org/poly2tri/utils/Util.scala
@@ -8,7 +8,7 @@ import shapes.Point
 object Util {
   
   // Almost zero
-  val COLLINEAR_SLOP = 0.25f
+  val COLLINEAR_SLOP = 0.01f
   
   val epsilon = exactinit
   val ccwerrboundA = (3.0 + 16.0 * epsilon) * epsilon
@@ -124,11 +124,9 @@ object Util {
 	   if ((det >= errbound) || (-det >= errbound)) {
 	     return det
 	   } else {
-	     
 	     // Cheat a little bit.... we have a degenerate triangle
 	     val c = pc * 1.00001f
          return orient2d(pa, pb, c)
-
 	   }
 
  }