added earclip algorithm for benchmarking

src/org/poly2tri/EarClip.scala

@@ -0,0 +1,359 @@
+package org.poly2tri
+
+/**
+ * Ported from jBox2D. Original author: ewjordan 
+ * Triangulates a polygon using simple ear-clipping algorithm. Returns
+ * size of Triangle array unless the polygon can't be triangulated.
+ * This should only happen if the polygon self-intersects,
+ * though it will not _always_ return null for a bad polygon - it is the
+ * caller's responsibility to check for self-intersection, and if it
+ * doesn't, it should at least check that the return value is non-null
+ * before using. You're warned!
+ *
+ * Triangles may be degenerate, especially if you have identical points
+ * in the input to the algorithm.  Check this before you use them.
+ *
+ * This is totally unoptimized, so for large polygons it should not be part
+ * of the simulation loop.
+ *
+ * Returns:
+ * -1 if algorithm fails (self-intersection most likely)
+ * 0 if there are not enough vertices to triangulate anything.
+ * Number of triangles if triangulation was successful.
+ *
+ * results will be filled with results - ear clipping always creates vNum - 2
+ * or fewer (due to pinch point polygon snipping), so allocate an array of
+ * this size.
+ */
+class EarClip {
+
+	val tol = .001f
+	var hasPinchPoint = false
+	var pinchIndexA = -1
+	var pinchIndexB = -1
+	var pin: Poly = null
+ 
+	def triangulatePolygon(xv: Array[Float], yv: Array[Float], vn: Int, results: Array[Triangle]): Int = {
+			
+	        if (vn < 3) return 0
+	        var vNum = vn
+			//Recurse and split on pinch points
+			val pA = new Poly
+			val pB = new Poly
+			pin = new Poly(xv, yv, vNum)
+			if (resolvePinchPoint(pin,pA,pB)){
+				val mergeA = new Array[Triangle](pA.nVertices)
+				val mergeB = new Array[Triangle](pB.nVertices)
+				for (i <- 0 until pA.nVertices) {
+					mergeA(i) = new Triangle();
+				}
+				for (i <- 0 until pB.nVertices) {
+					mergeB(i) = new Triangle();
+				}
+				val nA = triangulatePolygon(pA.x,pA.y,pA.nVertices,mergeA)
+				val nB = triangulatePolygon(pB.x,pB.y,pB.nVertices,mergeB)
+				if (nA == -1 || nB == -1){
+					return -1;
+				}
+				for (i <- 0 until nA){
+					results(i).set(mergeA(i));
+				}
+				for (i <- 0 until nB){
+					results(nA+i).set(mergeB(i));
+				}
+				return (nA+nB);
+			}
+
+	        val buffer = new Array[Triangle](vNum-2);
+	        for (i <- 0 until buffer.size) {
+	        	buffer(i) = new Triangle();
+	        }
+	        var bufferSize = 0;
+	        var xrem = new Array[Float](vNum)
+	        var yrem = new Array[Float](vNum)
+	        for (i <- 0 until vNum) {
+	            xrem(i) = xv(i);
+	            yrem(i) = yv(i);
+	        }
+			
+			val xremLength = vNum;
+			
+	        while (vNum > 3) {
+	        	//System.out.println("vNum: "+vNum);
+	            // Find an ear
+	            var earIndex = -1;
+				var earMaxMinCross = -1000.0f;
+	            for (i <- 0 until vNum) {
+	                if (isEar(i, xrem, yrem, vNum)) {
+						val lower = remainder(i-1,vNum);
+						val upper = remainder(i+1,vNum);
+						var d1 = Point(xrem(upper)-xrem(i),yrem(upper)-yrem(i));
+						var d2 = Point(xrem(i)-xrem(lower),yrem(i)-yrem(lower));
+						var d3 = Point(xrem(lower)-xrem(upper),yrem(lower)-yrem(upper));
+
+						d1 = d1.normalize
+						d2 = d2.normalize
+						d3 = d3.normalize
+						val cross12 = Math.abs( d1 cross d2 )
+						val cross23 = Math.abs( d2 cross d3 )
+						val cross31 = Math.abs( d3 cross d1 )
+						//Find the maximum minimum angle
+						val minCross = Math.min(cross12, Math.min(cross23,cross31))
+						if (minCross > earMaxMinCross){
+							earIndex = i
+							earMaxMinCross = minCross
+						}
+	                }
+	            }
+				
+	            // If we still haven't found an ear, we're screwed.
+	            // Note: sometimes this is happening because the
+				// remaining points are collinear.  Really these
+				// should just be thrown out without halting triangulation.
+				if (earIndex == -1){
+						
+					System.out.println("Couldn't find an ear, dumping remaining poly:\n");
+					System.out.println("Please submit this dump to ewjordan at Box2d forums\n");
+					for (i <- 0 until bufferSize) {
+						results(i).set(buffer(i));
+					}
+			
+					if (bufferSize > 0) return bufferSize;
+	                else return -1;
+				}
+				
+	            // Clip off the ear:
+	            // - remove the ear tip from the list
+
+	            vNum -= 1;
+	            val newx = new Array[Float](vNum)
+	            val newy = new Array[Float](vNum)
+	            var currDest = 0;
+	            for (i <- 0 until vNum) {
+	                if (currDest == earIndex) currDest += 1
+	                newx(i) = xrem(currDest);
+	                newy(i) = yrem(currDest);
+	                currDest += 1;
+	            }
+				
+	            // - add the clipped triangle to the triangle list
+	            val under = if(earIndex == 0) (vNum) else (earIndex - 1)
+	            val over = if(earIndex == vNum) 0 else (earIndex + 1)
+	            val toAdd = new Triangle(xrem(earIndex), yrem(earIndex), xrem(over), yrem(over), xrem(under), yrem(under));
+	            buffer(bufferSize) = toAdd
+	            bufferSize += 1;
+				
+	            // - replace the old list with the new one
+	            xrem = newx;
+	            yrem = newy;
+	        }
+			
+	        val toAdd = new Triangle(xrem(1), yrem(1), xrem(2), yrem(2), xrem(0), yrem(0))
+	        buffer(bufferSize) = toAdd;
+	        bufferSize += 1;
+			
+	        assert(bufferSize == xremLength-2)
+			
+	        for (i <- 0 until bufferSize) {
+	            results(i).set(buffer(i))
+	        }
+			
+	        return bufferSize;
+	}
+ 
+	/**
+	 * Finds and fixes "pinch points," points where two polygon
+	 * vertices are at the same point.
+	 *
+	 * If a pinch point is found, pin is broken up into poutA and poutB
+	 * and true is returned; otherwise, returns false.
+	 *
+	 * Mostly for internal use.
+	 * 
+	 * O(N^2) time, which sucks...
+	 */
+	private def resolvePinchPoint(pin: Poly, poutA: Poly, poutB: Poly): Boolean = {
+	  
+		if (pin.nVertices < 3) return false
+		hasPinchPoint = false
+		pinchIndexA = -1
+		pinchIndexB = -1
+		pinchIndex
+		if (hasPinchPoint){
+			val sizeA = pinchIndexB - pinchIndexA;
+			if (sizeA == pin.nVertices) return false;
+			val xA = new Array[Float](sizeA);
+			val yA = new Array[Float](sizeA);
+			for (i <- 0 until sizeA){
+				val ind = remainder(pinchIndexA+i,pin.nVertices);
+				xA(i) = pin.x(ind);
+				yA(i) = pin.y(ind);
+			}
+			val tempA = new Poly(xA,yA,sizeA);
+			poutA.set(tempA);
+			
+			val sizeB = pin.nVertices - sizeA;
+			val xB = new Array[Float](sizeB);
+			val yB = new Array[Float](sizeB);
+			for (i <- 0 until sizeB){
+				val ind = remainder(pinchIndexB+i,pin.nVertices);
+				xB(i) = pin.x(ind);
+				yB(i) = pin.y(ind);
+			}
+			val tempB = new Poly(xB,yB,sizeB);
+			poutB.set(tempB);
+		}
+		return hasPinchPoint;
+	}
+ 
+	 //Fix for obnoxious behavior for the % operator for negative numbers...
+	private def remainder(x: Int, modulus: Int): Int = {
+		var rem = x % modulus
+		while (rem < 0){
+			rem += modulus
+		}
+		return rem
+	}
+ 
+	 def pinchIndex: Boolean = {
+	   for (i <- 0 until pin.nVertices) {
+	     if(!hasPinchPoint) {
+			for (j <- i+1 until pin.nVertices){
+				//Don't worry about pinch points where the points
+				//are actually just dupe neighbors
+				if (Math.abs(pin.x(i)-pin.x(j))<tol&&Math.abs(pin.y(i)-pin.y(j))<tol&&j!=i+1){
+					pinchIndexA = i
+					pinchIndexB = j
+					hasPinchPoint = true
+					return false
+				}
+			}
+		  }
+	   } 
+       return true
+	 }
+  
+	 /**
+		 * Checks if vertex i is the tip of an ear in polygon defined by xv[] and
+	     * yv[].
+		 *
+		 * Assumes clockwise orientation of polygon...ick
+	     */
+	private def isEar(i: Int , xv: Array[Float], yv: Array[Float], xvLength: Int): Boolean = {
+	  
+	        var dx0, dy0, dx1, dy1 = 0f
+	        if (i >= xvLength || i < 0 || xvLength < 3) {
+	            return false;
+	        }
+	        var upper = i + 1
+	        var lower = i - 1
+	        if (i == 0) {
+	            dx0 = xv(0) - xv(xvLength - 1)
+	            dy0 = yv(0) - yv(xvLength - 1)
+	            dx1 = xv(1) - xv(0)
+	            dy1 = yv(1) - yv(0)
+	            lower = xvLength - 1
+	        }
+	        else if (i == xvLength - 1) {
+	            dx0 = xv(i) - xv(i - 1);
+	            dy0 = yv(i) - yv(i - 1);
+	            dx1 = xv(0) - xv(i);
+	            dy1 = yv(0) - yv(i);
+	            upper = 0;
+	        }
+	        else {
+	            dx0 = xv(i) - xv(i - 1);
+	            dy0 = yv(i) - yv(i - 1);
+	            dx1 = xv(i + 1) - xv(i);
+	            dy1 = yv(i + 1) - yv(i);
+	        }
+	        val cross = dx0 * dy1 - dx1 * dy0;
+	        if (cross > 0)
+	            return false;
+	        val myTri = new Triangle(xv(i), yv(i), xv(upper), yv(upper), xv(lower), yv(lower))
+	        for (j <- 0 until xvLength) {
+	            if (!(j == i || j == lower || j == upper)) {
+	            if (myTri.containsPoint(xv(j), yv(j)))
+	                return false;
+	            }
+	        }
+	        return true;
+	}
+ 
+}
+
+class Poly(var x: Array[Float], var y: Array[Float], var nVertices: Int) {
+  
+	var areaIsSet = false
+	var area = 0f
+ 
+	def this(_x: Array[Float], _y: Array[Float]) = this(_x,_y,_x.size)
+	def this() = this(null, null, 0)
+	
+	def set(p: Poly ) {
+	    if (nVertices != p.nVertices){
+			nVertices = p.nVertices;
+			x = new Array[Float](nVertices)
+			y = new Array[Float](nVertices)
+	    }
+			
+	    for (i <- 0 until nVertices) {
+	        x(i) = p.x(i)
+	        y(i) = p.y(i)
+	    }
+		areaIsSet = false
+	}
+}
+
+class Triangle(var x1: Float, var y1: Float, var x2: Float, var y2: Float, var x3: Float, var y3: Float) {
+    
+	def this() = this(0,0,0,0,0,0)
+ 
+    val x = new Array[Float](3)
+    val y = new Array[Float](3)
+    
+    // Automatically fixes orientation to ccw
+
+	  val dx1 = x2-x1
+	  val dx2 = x3-x1
+	  val dy1 = y2-y1
+	  val dy2 = y3-y1
+	  val cross = dx1*dy2-dx2*dy1
+	  val ccw = (cross>0)
+	  if (ccw){
+	    x(0) = x1; x(1) = x2; x(2) = x3;
+	    y(0) = y1; y(1) = y2; y(2) = y3;
+	  } else{
+	    x(0) = x1; x(1) = x3; x(2) = x2;
+	    y(0) = y1; y(1) = y3; y(2) = y2;
+	  }
+
+    def set(t: Triangle) {
+    	x(0) = t.x(0)
+    	x(1) = t.x(1)
+    	x(2) = t.x(2)
+    	y(0) = t.y(0)
+    	y(1) = t.y(1)
+    	y(2) = t.y(2)
+    }
+
+    
+    def containsPoint(_x: Float, _y: Float): Boolean = {
+      
+      val vx2 = _x-x(0); val vy2 = _y-y(0);
+      val vx1 = x(1)-x(0); val vy1 = y(1)-y(0);
+      val vx0 = x(2)-x(0); val vy0 = y(2)-y(0);
+      
+      val dot00 = vx0*vx0+vy0*vy0;
+      val dot01 = vx0*vx1+vy0*vy1;
+      val dot02 = vx0*vx2+vy0*vy2;
+      val dot11 = vx1*vx1+vy1*vy1;
+      val dot12 = vx1*vx2+vy1*vy2;
+      val invDenom = 1.0f / (dot00*dot11 - dot01*dot01);
+      val u = (dot11*dot02 - dot01*dot12)*invDenom;
+      val v = (dot00*dot12 - dot01*dot02)*invDenom;
+      
+      return ((u>=0)&&(v>=0)&&(u+v<=1));    
+    }
+    
+ }

src/org/poly2tri/MonoToneMountain.scala

@@ -38,7 +38,7 @@ class MonotoneMountain {
 	var tail, head: Point = null
 	var size = 0
 
-	val convexPoints = new Queue[Point]
+	val convexPoints = new ArrayBuffer[Point]
     // Monotone mountain points
 	val monoPoly = new ArrayBuffer[Point]
     // Triangles that constitute the mountain
@@ -53,17 +53,25 @@ class MonotoneMountain {
 	  size match {
 	    case 0 => 
 	      head = point
+	      size += 1
       case 1 =>
+        // Keep repeat points out of the list
+        if(point ! head) {
 	        tail = point
 	        tail.prev = head
 	        head.next = tail
+	        size += 1
+        }
       case _ =>
+        // Keep repeat points out of the list
+        if(point ! tail) {
 	        tail.next = point
 	        point.prev = tail
 	        tail = point
-	  }
 	        size += 1
         }
+	  }
+	}
 
 	// Remove a point from the list
 	def remove(point: Point) {
@@ -92,13 +100,13 @@ class MonotoneMountain {
         if(a >= PI_SLOP || a <= -PI_SLOP)
         remove(p)
         else
-        if(convex(p)) convexPoints.enqueue(p)
+        if(convex(p)) convexPoints += p
         p = p.next
       }
     
       while(!convexPoints.isEmpty) {
 	     
-        val ear = convexPoints.dequeue
+        val ear = convexPoints.remove(0)
         val a = ear.prev
         val b = ear
         val c = ear.next
@@ -108,8 +116,8 @@ class MonotoneMountain {
 	     
         // Remove ear, update angles and convex list
         remove(ear)
-        if(valid(a)) convexPoints.enqueue(a)
+        if(valid(a)) convexPoints += a
-        if(valid(c)) convexPoints.enqueue(c)
+        if(valid(c)) convexPoints += c
       }
       assert(size <= 3, "Triangulation bug, please report")
    

src/org/poly2tri/Point.scala

@@ -45,14 +45,7 @@ case class Point(val x: Float, val y: Float) {
   def dot(p: Point) = x * p.x + y * p.y
   def length = Math.sqrt(x * x + y * y).toFloat
   def normalize = this / length  
-  
+  def <(p: Point) = (x < p.x)    
-  def <(p: Point) = {
+  def !(p: Point) = !(p.x == x && p.y == y)
-    if(p.x == x) 
-      if(y <= p.y) true
-      else false
-    else
-      (x < p.x)
-  }
-    
   override def clone = Point(x, y)
 }

src/org/poly2tri/Poly2Tri.scala

@@ -63,8 +63,12 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
   var drawSegs = true
   var hiLighter = 0
   
+  var earClipResults = new Array[Triangle](14)
+  val earClip = new EarClip
+  
   def init(container: GameContainer) {
     poly
+    earClipPoly
   }
   
   def update(gc: GameContainer, delta: Int) {
@@ -119,6 +123,16 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
          g.drawLine(s.p.x,s.p.y,s.q.x,s.q.y)
    }
    
+    /*
+   earClipResults.foreach(t => {
+      val triangle = new Polygon
+      triangle.addPoint(t.x(0), t.y(0))
+      triangle.addPoint(t.x(1), t.y(1))
+      triangle.addPoint(t.x(2), t.y(2))
+      g.setColor(red)
+      g.draw(triangle)
+    })*/
+   
   }
   
   override def keyPressed(key:Int, c:Char) {
@@ -139,7 +153,7 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
         hiLighter = tesselator.triangles.size-1
     }
     if(c == 'm') drawMap = !drawMap
-    if(c == '1') {poly; hiLighter = 0}
+    if(c == '1') {poly; earClipPoly; hiLighter = 0}
     if(c == '2') {snake; hiLighter = 0}
     if(c == '3') {star; hiLighter = 0}
     if(c == 's') drawSegs = !drawSegs
@@ -185,7 +199,14 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
     segments += new Segment(p6, p7)
     
     tesselator = new Triangulator(segments)
+    val t1 = System.nanoTime
     tesselator process
+    val t2 = System.nanoTime
+    println
+    println("**Poly1**")
+    println("Poly2Tri core (ms) = " + tesselator.coreTime*1e-6)
+    println("Poly2Tri total (ms) = " + (t2-t1)*1e-6)
+
    }
   
   def star {
@@ -213,7 +234,14 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
     segments += new Segment(p9, p10)
     segments += new Segment(p10, p1)
     tesselator = new Triangulator(segments)
+    
+    val t1 = System.nanoTime
     tesselator process
+    val t2 = System.nanoTime
+    println
+    println("**Star**")
+    println("Poly2Tri core (ms) = " + tesselator.coreTime*1e-6)
+    println("Poly2Tri total (ms) = " + (t2-t1)*1e-6)
   }
   
   // Test #2
@@ -248,8 +276,62 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
     segments += new Segment(p11, p12)
     segments += new Segment(p12, p1)
     tesselator = new Triangulator(segments)
+    
+    val t1 = System.nanoTime
     tesselator process
-  }
+    val t2 = System.nanoTime
-  
+    println
+    println("**Snake**")
+    println("Poly2Tri core (ms) = " + tesselator.coreTime*1e-6)
+    println("Poly2Tri total (ms) = " + (t2-t1)*1e-6)
+
+  }
+  
+  def earClipPoly {
+    
+    val polyX = Array(400f, 500f, 520f, 460f, 580f, 480f, 360f, 360f, 300f, 200f, 120f, 200f, 340f, 208f, 180f, 300f)
+    val polyY = Array(472f, 392f, 272f, 232f, 212f, 152f, 172f, 52f, 112f, 32f, 92f, 72f, 272f, 212f, 352f, 312f)
+    
+    for(i <- 0 until earClipResults.size) earClipResults(i) = new Triangle
+    val t1 = System.nanoTime
+    earClip.triangulatePolygon(polyX, polyY, polyX.size, earClipResults)
+    val t2 = System.nanoTime
+    println("Earclip total (ms) =  " + (t2-t1)*1e-6)
+  }
+  
+  def earClipSnake {
+    
+    val polyX = Array(200f, 300f, 400f, 500f, 600f, 600f, 500f, 400f, 300f, 200f, 110f, 110f)
+    val polyY = Array(110f, 200f, 110f, 200f, 110f, 200f, 300f, 200f, 300f, 200f, 300f, 200f)
+    
+    for(i <- 0 until earClipResults.size) earClipResults(i) = new Triangle
+    val t1 = System.nanoTime
+    earClip.triangulatePolygon(polyX, polyY, polyX.size, earClipResults)
+    val t2 = System.nanoTime
+    println("Earclip total (ms) =  " + (t2-t1)*1e-6)
+  }
+  
+  def earClipStar {
+    
+    val p1 = Point(350,75)
+    val p2 = Point(379,161)
+    val p3 = Point(469,161)
+    val p4 = Point(397,215)
+    val p5 = Point(423,301)
+    val p6 = Point(350,250)
+    val p7 = Point(277,301)
+    val p8 = Point(303,215)
+    val p9 = Point(231,161)
+    val p10 = Point(321,161)
+    
+    val polyX = Array(350f, 379f, 469f, 397f, 423f, 350f, 277f, 303f, 231f, 321f)
+    val polyY = Array(75f, 161f, 161f, 215f, 301f, 250f, 301f,215f, 161f, 161f)
+    
+    for(i <- 0 until earClipResults.size) earClipResults(i) = new Triangle
+    val t1 = System.nanoTime
+    earClip.triangulatePolygon(polyX, polyY, polyX.size, earClipResults)
+    val t2 = System.nanoTime
+    println("Earclip total (ms) =  " + (t2-t1)*1e-6)
+  }
   
 }

src/org/poly2tri/QueryGraph.scala

@@ -44,7 +44,6 @@ class QueryGraph(var head: Node) {
     val trapezoids = new ArrayBuffer[Trapezoid]
     trapezoids += locate(s)
     var j = 0
-    try {
     while(s.q.x > trapezoids(j).rightPoint.x) {
       if(s > trapezoids(j).rightPoint) {
         trapezoids += trapezoids(j).upperRight
@@ -53,14 +52,6 @@ class QueryGraph(var head: Node) {
       }
       j += 1
     }
-    } catch {
-      case e => {
-        println("Number of trapezoids = " + j)
-        trapezoids(j-1).debugData
-        e.printStackTrace()
-        System.exit(0)
-      }
-    }
     trapezoids
   }
   

src/org/poly2tri/Segment.scala

@@ -30,7 +30,7 @@
  */
 package org.poly2tri
 
-import scala.collection.mutable.HashSet
+import scala.collection.mutable.{ArrayBuffer}
 
 // Represents a simple polygon's edge
 class Segment(var p: Point, var q: Point) {
@@ -38,9 +38,12 @@ class Segment(var p: Point, var q: Point) {
   // Pointers used for building trapezoidal map
   var above, below: Trapezoid = null
   
+  // This can be adjusted accordingly
+  val MAX_MPOINTS = 25
   // Montone mountain points
-  // Use a HashSet to avoid repeats
+  val mPoints = new Array[Point](MAX_MPOINTS)
-  val mPoints = HashSet.empty[Point]
+  // mPoints index counter
+  var np = 0
   
   // Equation of a line: y = m*x + b
   // Slope of the line (m)

src/org/poly2tri/Trapezoid.scala

@@ -80,10 +80,10 @@ class Trapezoid(val leftPoint: Point, var rightPoint: Point, val top: Segment, v
   
   // Add points to monotone mountain
   def addPoints {
-    if(leftPoint != bottom.p) bottom.mPoints += leftPoint.clone
+    if(leftPoint != bottom.p) {bottom.mPoints(bottom.np) = leftPoint.clone; bottom.np += 1}
-    if(rightPoint != bottom.q) bottom.mPoints += rightPoint.clone
+    if(rightPoint != bottom.q) {bottom.mPoints(bottom.np) = rightPoint.clone; bottom.np += 1}
-    if(leftPoint != top.p) top.mPoints += leftPoint.clone
+    if(leftPoint != top.p) {top.mPoints(top.np) = leftPoint.clone; top.np += 1}
-    if(rightPoint != top.q) top.mPoints += rightPoint.clone
+    if(rightPoint != top.q) {top.mPoints(top.np) = rightPoint.clone; top.np += 1}
   }
   
   def debugData {

src/org/poly2tri/Triangulator.scala

@@ -36,6 +36,11 @@ import scala.collection.mutable.ArrayBuffer
 // algorithm for computing trapezoidal decompositions and for triangulating polygons"
 class Triangulator(segments: ArrayBuffer[Segment]) {
   
+  var sortTime = 0.0
+  var coreTime = 0.0
+  var trapezoidTime = 0.0
+  var markTime = 0.0
+  
   // Triangle decomposition list
   var triangles = new ArrayBuffer[Array[Point]]
   
@@ -44,12 +49,20 @@ class Triangulator(segments: ArrayBuffer[Segment]) {
   
   // Build the trapezoidal map and query graph
   def process {
-    
+    val t1 = System.nanoTime
-    for(s <- segmentList) {
+    var i = 0
+    while(i < segmentList.size) {
+      val s = segmentList(i)
       var traps = queryGraph.followSegment(s)
       // Remove trapezoids from trapezoidal Map
-      traps.foreach(trapezoidalMap.remove)
+      var j = 0
-      for(t <- traps) {
+      while(j < traps.size) {
+        trapezoidalMap.remove(traps(j))
+        j += 1
+      }
+      j = 0
+      while(j < traps.size) {
+        val t = traps(j)
         var tList: Array[Trapezoid] = null
         val containsP = t.contains(s.p)
         val containsQ = t.contains(s.q)
@@ -71,13 +84,21 @@ class Triangulator(segments: ArrayBuffer[Segment]) {
           queryGraph.case4(t.sink, s, tList)
         }
         // Add new trapezoids to map
-        tList.foreach(trapezoidalMap.add)
+        var k = 0
+        while(k < tList.size) {
+          trapezoidalMap.add(tList(k))
+          k += 1
         }
-      trapezoidalMap reset
+        j += 1
       }
+      trapezoidalMap.reset
+      i += 1
+    }
+    coreTime = System.nanoTime - t1
     
     // Mark outside trapezoids
-    trapezoidalMap.map.foreach(markOutside)
+    for(t <- trapezoidalMap.map) 
+      markOutside(t)
     
     // Collect interior trapezoids
     for(t <- trapezoidalMap.map) 
@@ -86,12 +107,17 @@ class Triangulator(segments: ArrayBuffer[Segment]) {
         t addPoints
       }
 
+    // Generate the triangles
     createMountains 
     
     // Extract all the triangles into a single list
-    for(i <- 0 until xMonoPoly.size)
+    for(i <- 0 until xMonoPoly.size) {
-      for(t <- xMonoPoly(i).triangles)
+      var j = 0
-    	  triangles += t
+      while(j < xMonoPoly(i).triangles.size) {
+    	triangles += xMonoPoly(i).triangles(j)
+        j += 1
+      }
+    }
     
     //println("# triangles = " + triangles.size)
   }
@@ -117,15 +143,32 @@ class Triangulator(segments: ArrayBuffer[Segment]) {
   
   // Build a list of x-monotone mountains
   private def createMountains {
-    for(s <- segmentList) {
+    var i = 0
-      if(s.mPoints.size > 0) {
+    while(i < segmentList.size) {
+      val s = segmentList(i)
+      if(s.np > 0) {
          val mountain = new MonotoneMountain
-         val k = Util.msort((p1: Point, p2: Point) => p1 < p2)(s.mPoints.toList)
+         var k: List[Point] = null
-         val points = s.p.clone :: k ::: List(s.q.clone)
+         val tmp = new Array[Point](s.np)
-         points.foreach(p => mountain += p)
+         for(i <- 0 until s.np) tmp(i) = s.mPoints(i)
+         if(s.np < 10) 
+           // Insertion sort is one of the fastest algorithms for sorting arrays containing 
+           // fewer than ten elements, or for lists that are already mostly sorted.
+           k = Util.insertionSort(tmp.toList, {(x1, x2) => x1 <= x2} )
+         else 
+           k = Util.msort((p1: Point, p2: Point) => p1 < p2)(tmp.toList)
+         val points = s.p :: k ::: List(s.q)
+         var j = 0
+         while(j < points.size) {
+           mountain += points(j)
+           j += 1
+         }
+         val t1 = System.nanoTime
          mountain.triangulate
+         coreTime += System.nanoTime - t1
          xMonoPoly += mountain
       }
+      i += 1
     }   
   }
   

src/org/poly2tri/Util.scala

@@ -16,6 +16,30 @@ object Util {
       if (n == 0) xs
       else merge(msort(less)(xs take n), msort(less)(xs drop n))
   }
+  
+  def insertionSort(values : List[Point], matcher : (Float, Float) => Boolean) : List[Point] = {
+       def iSort(values : List[Point]) : List[Point] = {
+           val result = values match {
+               case List() => List()
+               case value :: valuesTail => insert(value, iSort(valuesTail))
+           }
+           result
+       }
+       def insert(value : Point, values : List[Point]) : List[Point] = {
+           val result = values match {
+               // if list is empty return new list with single element in it
+               case List() => List(value)
+               // otherwise insert into list in order, recursively
+               case x :: xTail =>
+                   if (matcher(value.x, x.x)) 
+                       value :: values
+                   else 
+                       x :: insert(value, xTail)
+           }
+           result
+       }
+       iSort(values)
+   }
 }
 
 /** The object <code>Random</code> offers a default implementation