diff --git a/src/org/poly2tri/Poly2Tri.scala b/src/org/poly2tri/Poly2Tri.scala
index d38baee..51a3593 100644
--- a/src/org/poly2tri/Poly2Tri.scala
+++ b/src/org/poly2tri/Poly2Tri.scala
@@ -87,7 +87,7 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
   val i18 = "data/i.18"
   val tank = "data/tank.dat"
   
-  var currentModel = nazcaMonkey
+  var currentModel = strange
   var doCDT = true
   
   var mouseButton = 0
diff --git a/src/org/poly2tri/cdt/AFront.scala b/src/org/poly2tri/cdt/AFront.scala
index d2e7f6d..6709096 100644
--- a/src/org/poly2tri/cdt/AFront.scala
+++ b/src/org/poly2tri/cdt/AFront.scala
@@ -98,8 +98,6 @@ class AFront(iTriangle: Triangle) {
     val point1 = edge.q
     val point2 = edge.p
     
-    var edgeTri1: Triangle = null
-    var edgeTri2: Triangle = null
     var marked = false
     
     // Scan the advancing front and update Node triangle pointers
@@ -108,29 +106,19 @@ class AFront(iTriangle: Triangle) {
       T2.foreach(t => {
         if(t.contains(node.point, node.next.point))
           node.triangle = t
-        if(!marked && t.contains(point1, point2)) {
-          edgeTri2 = t
-          edgeTri2 markEdge(point1, point2)
           marked = true
-        }
       })
       
-      marked = false
-      
-      T1.foreach(t => {
-        if(t.contains(node.point, node.next.point))
-          node.triangle = t
-        if(!marked && t.contains(point1, point2)) {
-          edgeTri1 = t
-          edgeTri1 markEdge(point1, point2)
-          marked = true
-        }
-      })
+      if(!marked)
+        T1.foreach(t => {
+          if(t.contains(node.point, node.next.point))
+            node.triangle = t
+        })
 
       node = node.next
     }
    
-    edgeTri1.markNeighbor(edgeTri2)
+    T1.last.markNeighbor(T2.last)
     
   }
   
diff --git a/src/org/poly2tri/cdt/CDT.scala b/src/org/poly2tri/cdt/CDT.scala
index a26c58f..0f44a41 100644
--- a/src/org/poly2tri/cdt/CDT.scala
+++ b/src/org/poly2tri/cdt/CDT.scala
@@ -34,6 +34,7 @@ import scala.collection.mutable.{ArrayBuffer, Set}
 
 import shapes.{Segment, Point, Triangle}
 import utils.Util
+import seidel.MonotoneMountain
 
 /**
  * Sweep-line, Constrained Delauney Triangulation (CDT)
@@ -242,6 +243,10 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
       
       aFront.constrainedEdge(sNode, eNode, T1, T2, edge)
       
+      // Mark constrained edge
+      T1.last markEdge(point1, point2)
+      T2.last markEdge(point1, point2)
+      
     } else if(firstTriangle == null) {
       
       // No triangles are intersected by the edge; edge must lie outside the mesh
@@ -251,7 +256,7 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
       val point1 = if(ahead) edge.q else edge.p
       val point2 = if(ahead) edge.p else edge.q
       
-      var pNode = aFront.locatePoint(point1)
+      var pNode = if(ahead) node else aFront.locatePoint(point1)
       val first = pNode
       
       val points = new ArrayBuffer[Point]
@@ -266,30 +271,19 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
         nTriangles += pNode.triangle
 		pNode = pNode.next
 	  }
-                                                   
+
       // Triangulate empty areas.
       val T = new ArrayBuffer[Triangle]
       triangulate(points.toArray, List(point1, point2), T)
-      
-      //T.foreach(t => mesh.debug += t)
-      
-      // Select edge triangle
-      var edgeTri: Triangle = null
-      var i = 0
-      while(edgeTri == null)  {
-        if(T(i).contains(point1, point2)) 
-          edgeTri = T(i)
-        i += 1
-      }
-      
-      // Update advancing front 
-      aFront link (first, pNode, edgeTri)
-      
+     
       // Update neighbors
       edgeNeighbors(nTriangles, T)
-            
+      
+      // Update advancing front 
+      aFront link (first, pNode, T.last)
+      
       // Mark constrained edge
-      edgeTri markEdge(point1, point2)
+      T.last markEdge(point1, point2)
       
     } else if(firstTriangle.contains(edge.q, edge.p)) { 
       // Mark constrained edge
@@ -297,7 +291,6 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
       firstTriangle.finalized = true
     } else {
       throw new Exception("Triangulation error")
-      //null
     }
     
   }
@@ -316,18 +309,18 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
     
   }
   
-  
   // Marc Vigo Anglada's triangulate pseudo-polygon algo 
+  // See "An improved incremental algorithm for constructing restricted Delaunay triangulations"
   private def triangulate(P: Array[Point], ab: List[Point], T: ArrayBuffer[Triangle]) {
     
-    val a = ab.first
-    val b = ab.last
+    val a = ab(0)
+    val b = ab(1)
     var i = 0
     
     if(P.size > 1) {
-      var c = P.first
+      var c = P(0)
       for(j <- 1 until P.size) {
-        if(illegal(a, c, b, P(j))) {
+        if(illegal(a, b, c, P(j))) {
           c = P(j)
           i = j
         } 
@@ -389,31 +382,18 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
       angle
   }
   
-  // Do edges need to be swapped? Robust CircumCircle test
-  // See section 3.7 from "Triangulations and Applications" by O. Hjelle & M. Deahlen
-  private def illegal(p1: Point, p2: Point, p3: Point, p4:Point): Boolean = {
+  // Circumcircle test. 
+  // Determines if point d lies inside triangle abc's circumcircle 
+  def illegal(a: Point, b: Point, c: Point, d: Point): Boolean = {
+    
+    val ccw = Util.orient2d(a, b, c) > 0
+    
+    // Make sure abc is oriented counter-clockwise
+    if(ccw) 
+      Util.incircle(a, b, c, d) 
+    else 
+      Util.incircle(a, c, b, d)
     
-	  val v1 = p3 - p2
-      val v2 = p1 - p2
-      val v3 = p1 - p4
-      val v4 = p3 - p4
-      
-      val cosA = v1 dot v2
-      val cosB = v3 dot v4
-      
-      if(cosA < 0 && cosB < 0) 
-        return true 
-      
-      if(cosA > 0 && cosB > 0)
-        return false
-      
-      val sinA = v1 cross v2
-      val sinB = v3 cross v4
-
-      if((cosA*sinB + sinA*cosB) < 0f) 
-        true
-      else
-        false
   }
   
   // Ensure adjacent triangles are legal
@@ -424,10 +404,8 @@ class CDT(val points: List[Point], val segments: List[Segment], iTriangle: Trian
     
     val point = t1.points(0)
     val oPoint = t2 oppositePoint t1
-    
-    val collinear = t1.collinear(oPoint) || t2.collinear(oPoint, point)
         
-    if(illegal(t1.points(1), oPoint, t1.points(2), t1.points(0)) && !t2.finalized) {
+    if(illegal(t1.points(1), t1.points(0), t1.points(2), oPoint) && !t2.finalized) {
 
         // Flip edge and rotate everything clockwise
 	    t1.legalize(oPoint)
diff --git a/src/org/poly2tri/utils/Util.scala b/src/org/poly2tri/utils/Util.scala
index 4957c29..f4e775c 100644
--- a/src/org/poly2tri/utils/Util.scala
+++ b/src/org/poly2tri/utils/Util.scala
@@ -12,7 +12,8 @@ object Util {
   
   val epsilon = exactinit
   val ccwerrboundA = (3.0 + 16.0 * epsilon) * epsilon
-
+  val iccerrboundA = (10.0 + 96.0 * epsilon) * epsilon
+  
   // From "Scala By Example," by Martin Odersky
   def msort[A](less: (A, A) => Boolean)(xs: List[A]): List[A] = {
     def merge(xs1: List[A], xs2: List[A]): List[A] =
@@ -120,7 +121,7 @@ object Util {
 	     return det
 	   }
 	
-	   val errbound = Util.ccwerrboundA * detsum
+	   val errbound = ccwerrboundA * detsum
 	   if ((det >= errbound) || (-det >= errbound)) {
 	     return det
 	   } else {    
@@ -129,7 +130,136 @@ object Util {
          return orient2d(pa, pb, c)
 	   }
 
- }
+   }
+   
+   // Returns triangle circumcircle point and radius
+   def circumCircle(a: Point, b: Point, c: Point): Tuple2[Point, Float] = {
+     
+     val A = det(a, b, c)
+     val C = detC(a, b, c)
+     
+     val bx1 = Point(a.x*a.x + a.y*a.y, a.y)
+     val bx2 = Point(b.x*b.x + b.y*b.y, b.y)
+     val bx3 = Point(c.x*c.x + c.y*c.y, c.y)
+     val bx = det(bx1, bx2, bx3)
+     
+     val by1 = Point(a.x*a.x + a.y*a.y, a.x)
+     val by2 = Point(b.x*b.x + b.y*b.y, b.x)
+     val by3 = Point(c.x*c.x + c.y*c.y, c.x)
+     val by = det(by1, by2, by3)
+     
+     val x = bx / (2*A)
+     val y = by / (2*A)
+     
+     val center = Point(x, y)
+     val radius = Math.sqrt(bx*bx + by*by - 4*A*C).toFloat / (2*Math.abs(A))
+     
+     (center, radius)
+   }
+   
+   def det(p1: Point, p2: Point, p3: Point): Float = {
+     
+     val a11 = p1.x
+     val a12 = p1.y
+     val a13,a23,a33 = 1f
+     val a21 = p2.x
+     val a22 = p2.y
+     val a31 = p3.x
+     val a32 = p3.y
+     
+     a11*(a22*a33-a23*a32) - a12*(a21*a33 - a23*a31) + a13*(a21*a32-a22*a31)
+     
+   }
+   
+   def detC(p1: Point, p2: Point, p3: Point): Float = {
+     
+     val a11 = p1.x*p1.x + p1.y*p1.y
+     val a12 = p1.x
+     val a13 = p1.y
+     val a21 = p2.x*p2.x + p2.y*p2.y
+     val a22 = p2.x
+     val a23 = p2.y
+     val a31 = p3.x*p3.x + p3.y*p3.y
+     val a32 = p3.x
+     val a33 = p3.y
+     
+     a11*(a22*a33-a23*a32) - a12*(a21*a33 - a23*a31) + a13*(a21*a32-a22*a31)
+     
+   }
+   
+   /* Approximate 2D incircle test.  Nonrobust.  By Jonathan Shewchuk
+    * Return a positive value if the point pd lies inside the     
+    * circle passing through pa, pb, and pc; a negative value if  
+    * it lies outside; and zero if the four points are cocircular.
+    * The points pa, pb, and pc must be in counterclockwise       
+    * order, or the sign of the result will be reversed.   
+   */
+   def incirclefast(pa: Point, pb: Point, pc: Point, pd: Point): Boolean = {
+     
+    val adx = pa.x - pd.x
+    val ady = pa.y - pd.y
+    val bdx = pb.x - pd.x
+    val bdy = pb.y - pd.y
+    val cdx = pc.x - pd.x
+    val cdy = pc.y - pd.y
+
+    val abdet = adx * bdy - bdx * ady
+    val bcdet = bdx * cdy - cdx * bdy
+    val cadet = cdx * ady - adx * cdy
+    val alift = adx * adx + ady * ady
+    val blift = bdx * bdx + bdy * bdy
+    val clift = cdx * cdx + cdy * cdy
+
+    alift * bcdet + blift * cadet + clift * abdet >= 0
+    
+  }
+
+   /* Robust 2D incircle test, modified. Original By Jonathan Shewchuk
+    * Return a positive value if the point pd lies inside the     
+    * circle passing through pa, pb, and pc; a negative value if  
+    * it lies outside; and zero if the four points are cocircular.
+    * The points pa, pb, and pc must be in counterclockwise       
+    * order, or the sign of the result will be reversed.   
+   */
+   def incircle(pa: Point, pb: Point, pc: Point, pd: Point): Boolean = {
+  
+     val adx = pa.x - pd.x
+     val bdx = pb.x - pd.x
+     val cdx = pc.x - pd.x
+     val ady = pa.y - pd.y
+     val bdy = pb.y - pd.y
+     val cdy = pc.y - pd.y
+
+     val bdxcdy = bdx * cdy
+     val cdxbdy = cdx * bdy
+     val alift = adx * adx + ady * ady
+
+     val cdxady = cdx * ady
+     val adxcdy = adx * cdy
+     val blift = bdx * bdx + bdy * bdy
+
+     val adxbdy = adx * bdy
+     val bdxady = bdx * ady
+     val clift = cdx * cdx + cdy * cdy
+
+     val det = alift * (bdxcdy - cdxbdy) + 
+               blift * (cdxady - adxcdy) + 
+               clift * (adxbdy - bdxady)
+
+     val permanent = (Math.abs(bdxcdy) + Math.abs(cdxbdy)) * alift + 
+                     (Math.abs(cdxady) + Math.abs(adxcdy)) * blift + 
+                     (Math.abs(adxbdy) + Math.abs(bdxady)) * clift
+     
+     val errbound = iccerrboundA * permanent
+     
+     if ((det > errbound) || (-det > errbound)) {
+       return det >= 0
+     } else {
+       throw new Exception("Points nearly collinear")
+     }
+
+}
+
  
 }