added shear transform and solved a number of numerical issues

src/org/poly2tri/Point.scala

@@ -45,7 +45,6 @@ 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) = !(p.x == x && p.y == y)
   
   def <(p: Point) = {
     if(p.x == x) 

src/org/poly2tri/Poly2Tri.scala

@@ -104,10 +104,6 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
         g.setColor(red)
         g.draw(triangle)
       }
-      //val triangle = new Polygon
-      //tesselator.triangles(hiLighter).foreach(p => triangle.addPoint(p.x, p.y))
-      //g.setColor(blue)
-      //g.draw(triangle)
    } else if (debug && drawMap){
     for(mp <- tesselator.monoPolies) {
       val poly = new Polygon
@@ -171,22 +167,22 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
     val p16 = Point(300,312)
     
     segments = new ArrayBuffer[Segment]
-    segments += new Segment(p1, p2)
+    segments += new Segment(p16, p1) 
-    segments += new Segment(p2, p3)
+    segments += new Segment(p9, p10)
-    segments += new Segment(p3, p4)
+    segments += new Segment(p13, p14)
-    segments += new Segment(p4, p5)
     segments += new Segment(p5, p6)
-    segments += new Segment(p6, p7) 
+    segments += new Segment(p2, p3)
+    segments += new Segment(p1, p2)
+    segments += new Segment(p4, p5)
     segments += new Segment(p7, p8)
     segments += new Segment(p8, p9)
-    segments += new Segment(p9, p10)
     segments += new Segment(p10, p11)
     segments += new Segment(p11, p12)
     segments += new Segment(p12, p13)
-    segments += new Segment(p13, p14)
+    segments += new Segment(p3, p4)
-    segments += new Segment(p14, p15)
     segments += new Segment(p15, p16)
-    segments += new Segment(p16, p1)     
+    segments += new Segment(p14, p15)
+    segments += new Segment(p6, p7)
     
     tesselator = new Triangulator(segments)
     tesselator process

src/org/poly2tri/QueryGraph.scala

@@ -44,6 +44,7 @@ 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
@@ -52,6 +53,14 @@ 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/Trapezoid.scala

@@ -61,7 +61,7 @@ class Trapezoid(val leftPoint: Point, var rightPoint: Point, val top: Segment, v
   
   // Determines if this point lies inside the trapezoid
   def contains(point: Point) = {
-     (point.x > leftPoint.x && point.x < rightPoint.x && top > point && bottom < point)
+    (point.x > leftPoint.x && point.x < rightPoint.x && top > point && bottom < point)
   }
   
   def vertices: Array[Point] = {
@@ -80,9 +80,15 @@ 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 += leftPoint.clone
-    if(rightPoint ! bottom.q) bottom.mPoints += rightPoint.clone
+    if(rightPoint != bottom.q) bottom.mPoints += rightPoint.clone
-    if(leftPoint ! top.p) top.mPoints += leftPoint.clone
+    if(leftPoint != top.p) top.mPoints += leftPoint.clone
-    if(rightPoint ! top.q) top.mPoints += rightPoint.clone
+    if(rightPoint != top.q) top.mPoints += rightPoint.clone
+  }
+  
+  def debugData {
+    println("LeftPoint = " + leftPoint + " | RightPoint = " + rightPoint)
+    println("Top Segment: p = " + top.p + ", q = " + top.q)
+    println("Bottom Segment: p = " + bottom.p + ", q = " + bottom.q)
   }
 }

src/org/poly2tri/TrapezoidalMap.scala

@@ -78,9 +78,6 @@ class TrapezoidalMap {
     trapezoids(2).update(null, trapezoids(0), null, trapezoids(3))
     trapezoids(3).update(trapezoids(1), trapezoids(2), t.upperRight, t.lowerRight)
     
-    s.above = trapezoids(1)
-    s.below = trapezoids(2)
-    
     trapezoids
   }
 
@@ -149,7 +146,6 @@ class TrapezoidalMap {
   def case4(t: Trapezoid, s: Segment) = {
       
     val lp = if(s.p.x == t.leftPoint.x) s.p else t.leftPoint
-    val rp = if(s.q.x == t.rightPoint.x) s.q else t.rightPoint
     
     val topCross = (tCross == t.top)
     val bottomCross = (bCross == t.bottom)
@@ -201,7 +197,7 @@ class TrapezoidalMap {
     val top = new Segment(Point(min.x, max.y), Point(max.x, max.y))
     val bottom = new Segment(Point(min.x, min.y), Point(max.x, min.y))
     val left = bottom.p
-    val right = bottom.q
+    val right = top.q
     
     return new Trapezoid(left, right, top, bottom)
   }

src/org/poly2tri/Triangulator.scala

@@ -111,7 +111,7 @@ class Triangulator(segments: ArrayBuffer[Segment]) {
   
   // Initialize trapezoidal map and query structure
   private val trapezoidalMap = new TrapezoidalMap
-  private val boundingBox = trapezoidalMap.boundingBox(segments)
+  private val boundingBox = trapezoidalMap.boundingBox(segmentList)
   private val queryGraph = new QueryGraph(Sink.init(boundingBox))
   private val xMonoPoly = new ArrayBuffer[MonotoneMountain]
   
@@ -121,7 +121,7 @@ class Triangulator(segments: ArrayBuffer[Segment]) {
       if(s.mPoints.size > 0) {
          val mountain = new MonotoneMountain
          val k = Util.msort((p1: Point, p2: Point) => p1 < p2)(s.mPoints.toList)
-         val points = s.p :: k ::: List(s.q)
+         val points = s.p.clone :: k ::: List(s.q.clone)
          points.foreach(p => mountain += p)
          mountain.triangulate
          xMonoPoly += mountain
@@ -139,16 +139,25 @@ class Triangulator(segments: ArrayBuffer[Segment]) {
   private def orderSegments = {
     // Ignore vertical segments!
     val segs = new ArrayBuffer[Segment]
-    for(s <- segments) 
+    for(s <- segments) {
+      val p = shearTransform(s.p)
+      val q = shearTransform(s.q)
       // Point p must be to the left of point q
-      if(s.p.x > s.q.x) {
+      if(p.x > q.x) {
-        segs += new Segment(s.q.clone, s.p.clone)
+        segs += new Segment(q, p)
-      } else if(s.p.x < s.q.x)
+      } else if(p.x < q.x) {
-        segs += new Segment(s.p.clone, s.q.clone)
+        segs += new Segment(p, q)
+      }
+    }
     // Randomized triangulation improves performance
     // See Seidel's paper, or O'Rourke's book, p. 57 
-    // Turn this off for while bug hunting math robustness issues
+    Random.shuffle(segs)
-    //Random.shuffle(segs)
     segs
   }
+  
+  // Prevents any two distinct endpoints from lying on a common vertical line, and avoiding
+  // the degenerate case. See Mark de Berg et al, Chapter 6.3
+  //val SHEER = 0.001f
+  def shearTransform(point: Point) = Point(point.x + 0.0001f * point.y, point.y)
+ 
 }

src/org/poly2tri/XNode.scala

@@ -33,10 +33,7 @@ package org.poly2tri
 class XNode(point: Point, lChild: Node, rChild: Node) extends Node(lChild, rChild) {
 
   override def locate(s: Segment): Sink = {
-    if(s.p.x > point.x) {
+    if(s.p.x >= point.x) {
-      // Move to the right in the graph
-      return right.locate(s)
-    } else if(s.p.x == point.x) {
       // Move to the right in the graph
       return right.locate(s)
     } else {

src/org/poly2tri/YNode.scala

@@ -33,16 +33,20 @@ package org.poly2tri
 class YNode(segment: Segment, lChild: Node, rChild: Node) extends Node(lChild, rChild) {
 
   override def locate(s: Segment): Sink = {
+    //println(s.p.y)
+    //println(Math.round(segment.slope * s.p.x + segment.b))
     if (segment > s.p) {
       // Move down the graph
       return right.locate(s)
-    } else if (segment < s.p){
+    } else if (segment < s.p) {
+      //println("*****")
+      //println(s.p.y)
+      //println(Math.round(segment.slope * s.p.x + segment.b))
       // Move up the graph
       return left.locate(s)
-      
     } else {
       // s and segment share the same endpoint, p
-      if (s.slope < segment.slope) {
+      if (Math.round(s.slope) < Math.round(segment.slope)) {
         // Move down the graph
         return right.locate(s)
       } else {