VectorUtil: Generalize *seg2segIntersection* w/ epsilon and doCollinear

3 files changed, 94 insertions, 38 deletions

diff --git a/src/jogl/classes/com/jogamp/graph/curve/OutlineShape.java b/src/jogl/classes/com/jogamp/graph/curve/OutlineShape.java
index 18e7328ce..25791e17d 100644
--- a/src/jogl/classes/com/jogamp/graph/curve/OutlineShape.java
+++ b/src/jogl/classes/com/jogamp/graph/curve/OutlineShape.java
@@ -921,9 +921,9 @@ public final class OutlineShape implements Comparable<OutlineShape> {
                                                  tmpV1, tmpV2, tmpV3) ) {
                     return current;
                 }
-                if(VectorUtil.testTri2SegIntersection(a, b, c, prevV, current) ||
-                   VectorUtil.testTri2SegIntersection(a, b, c, current, nextV) ||
-                   VectorUtil.testTri2SegIntersection(a, b, c, prevV, nextV) ) {
+                if(VectorUtil.testTri2SegIntersection(a, b, c, prevV, current, FloatUtil.EPSILON, false) ||
+                   VectorUtil.testTri2SegIntersection(a, b, c, current, nextV, FloatUtil.EPSILON, false) ||
+                   VectorUtil.testTri2SegIntersection(a, b, c, prevV, nextV, FloatUtil.EPSILON, false) ) {
                     return current;
                 }
             }
diff --git a/src/jogl/classes/com/jogamp/math/VectorUtil.java b/src/jogl/classes/com/jogamp/math/VectorUtil.java
index 05cb90d33..6af93ae6b 100644
--- a/src/jogl/classes/com/jogamp/math/VectorUtil.java
+++ b/src/jogl/classes/com/jogamp/math/VectorUtil.java
@@ -709,7 +709,7 @@ public final class VectorUtil {
      * @param c vertex 1 of second segment
      * @param d vertex 2 of second segment
      * @return true if the segments intersect, otherwise returns false
-     * @deprecated use {@link #testSeg2SegIntersection(Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, Vert2fImmutable)}
+     * @deprecated use {@link #testSeg2SegIntersection(Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, float, boolean)}
      */
     @Deprecated
     public static boolean testSeg2SegIntersection0(final Vert2fImmutable a, final Vert2fImmutable b,
@@ -741,7 +741,7 @@ public final class VectorUtil {
      * @param d vertex 1 of first segment
      * @param e vertex 2 of first segment
      * @return true if the segment intersects at least one segment of the triangle, false otherwise
-     * @deprecated use {@link #testTri2SegIntersection(Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, Vert2fImmutable)}
+     * @deprecated use {@link #testTri2SegIntersection(Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, float, boolean)}
      */
     @Deprecated
     public static boolean testTri2SegIntersection0(final Vert2fImmutable a, final Vert2fImmutable b, final Vert2fImmutable c,
@@ -752,21 +752,33 @@ public final class VectorUtil {
     }
 
     /**
+     * Line segment intersection test and returning the intersecting point.
+     * <p>
      * See [p + t r = q + u s](https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/565282#565282)
      * and [its terse C# implementation](https://www.codeproject.com/tips/862988/find-the-intersection-point-of-two-line-segments)
+     * </p>
+     * <p>
+     * Implementation uses float precision.
+     * </p>
+     * @param p vertex 1 of first segment
+     * @param p2 vertex 2 of first segment
+     * @param q vertex 1 of second segment
+     * @param q2 vertex 2 of second segment
+     * @param epsilon optional epsilon. If {@code 0} just compare against {@code 0}, otherwise compare against this {@code epsilon}. See {@link FloatUtil#EPSILON}
+     * @param doCollinear consider collinear case, i.e. overlapping segments as an intersection returning {@code true}
+     * @return true if line segments are intersecting, otherwise false
      */
-    public static boolean seg2SegIntersection(final Vec3f result, final Vec2f p, final Vec2f p2, final Vec2f q, final Vec2f q2, final boolean doCollinear)
+    public static boolean seg2SegIntersection(final Vec3f result, final Vec2f p, final Vec2f p2, final Vec2f q, final Vec2f q2, final float epsilon, final boolean doCollinear)
     {
-        final float eps = FloatUtil.EPSILON;
         final Vec2f r = p2.minus(p);
         final Vec2f s = q2.minus(q);
         final float rxs = r.cross(s);
 
-        if ( FloatUtil.isZero(rxs) ) {
+        if ( FloatUtil.isZero(rxs, epsilon) ) {
             if( doCollinear ) {
                 final Vec2f q_p = q.minus(p);
                 final float qpxr = q_p.cross(r);
-                if ( FloatUtil.isZero(qpxr) ) // disabled collinear case
+                if ( FloatUtil.isZero(qpxr, epsilon) ) // disabled collinear case
                 {
                     // 1) r x s = 0 and (q - p) x r = 0, the two lines are collinear.
                     final Vec2f p_q = p.minus(q);
@@ -774,8 +786,8 @@ public final class VectorUtil {
                     final float pq_dot_s = p_q.dot(s);
                     // if ( ( 0 <= qp_dot_r && qp_dot_r <= r.dot(r) ) ||
                     //      ( 0 <= pq_dot_s && pq_dot_s <= s.dot(s) ) )
-                    if ( ( eps <= qp_dot_r && qp_dot_r - r.dot(r) <= eps ) ||
-                         ( eps <= pq_dot_s && pq_dot_s - s.dot(s) <= eps ) )
+                    if ( ( epsilon <= qp_dot_r && qp_dot_r - r.dot(r) <= epsilon ) ||
+                         ( epsilon <= pq_dot_s && pq_dot_s - s.dot(s) <= epsilon ) )
                     {
                         // 1.1) 0 <= (q - p) · r <= r · r or 0 <= (p - q) · s <= s · s, the two lines are overlapping
                         // FIXME: result set to q2 endpoint, OK?
@@ -807,7 +819,7 @@ public final class VectorUtil {
             final float u = qpxr / rxs;
 
             // if ( (0 <= t && t <= 1) && (0 <= u && u <= 1) )
-            if ( (eps <= t && t - 1 <= eps) && (eps <= u && u - 1 <= eps) )
+            if ( (epsilon <= t && t - 1 <= epsilon) && (epsilon <= u && u - 1 <= epsilon) )
             {
                 // 3) r × s ≠ 0 and 0 ≤ t ≤ 1 and 0 ≤ u ≤ 1, the two line segments meet at the point p + t * r = q + u * s.
                 result.set( p.plus( r.mul( t ) ), 0 ); // == q + (u * s)
@@ -826,19 +838,25 @@ public final class VectorUtil {
      * <p>
      * Implementation uses float precision.
      * </p>
+     * @param p vertex 1 of first segment
+     * @param p2 vertex 2 of first segment
+     * @param q vertex 1 of second segment
+     * @param q2 vertex 2 of second segment
+     * @param epsilon optional epsilon. If {@code 0} just compare against {@code 0}, otherwise compare against this {@code epsilon}. See {@link FloatUtil#EPSILON}
+     * @param doCollinear consider collinear case, i.e. overlapping segments as an intersection returning {@code true}
+     * @return true if line segments are intersecting, otherwise false
      */
-    public static boolean testSeg2SegIntersection(final Vec2f p, final Vec2f p2, final Vec2f q, final Vec2f q2, final boolean doCollinear)
+    public static boolean testSeg2SegIntersection(final Vec2f p, final Vec2f p2, final Vec2f q, final Vec2f q2, final float epsilon, final boolean doCollinear)
     {
-        final float eps = FloatUtil.EPSILON;
         final Vec2f r = p2.minus(p);
         final Vec2f s = q2.minus(q);
         final float rxs = r.cross(s);
 
-        if ( FloatUtil.isZero(rxs) ) {
+        if ( FloatUtil.isZero(rxs, epsilon) ) {
             if( doCollinear ) {
                 final Vec2f q_p = q.minus(p);
                 final float qpxr = q_p.cross(r);
-                if ( FloatUtil.isZero(qpxr) ) // disabled collinear case
+                if ( FloatUtil.isZero(qpxr, epsilon) ) // disabled collinear case
                 {
                     // 1) r x s = 0 and (q - p) x r = 0, the two lines are collinear.
                     final Vec2f p_q = p.minus(q);
@@ -846,8 +864,8 @@ public final class VectorUtil {
                     final float pq_dot_s = p_q.dot(s);
                     // if ( ( 0 <= qp_dot_r && qp_dot_r <= r.dot(r) ) ||
                     //      ( 0 <= pq_dot_s && pq_dot_s <= s.dot(s) ) )
-                    if ( ( eps <= qp_dot_r && qp_dot_r - r.dot(r) <= eps ) ||
-                         ( eps <= pq_dot_s && pq_dot_s - s.dot(s) <= eps ) )
+                    if ( ( epsilon <= qp_dot_r && qp_dot_r - r.dot(r) <= epsilon ) ||
+                         ( epsilon <= pq_dot_s && pq_dot_s - s.dot(s) <= epsilon ) )
                     {
                         // 1.1) 0 <= (q - p) · r <= r · r or 0 <= (p - q) · s <= s · s, the two lines are overlapping
                         return true;
@@ -877,7 +895,7 @@ public final class VectorUtil {
             final float u = qpxr / rxs;
 
             // if ( (0 <= t && t <= 1) && (0 <= u && u <= 1) )
-            if ( (eps <= t && t - 1 <= eps) && (eps <= u && u - 1 <= eps) )
+            if ( (epsilon <= t && t - 1 <= epsilon) && (epsilon <= u && u - 1 <= epsilon) )
             {
                 // 3) r × s ≠ 0 and 0 ≤ t ≤ 1 and 0 ≤ u ≤ 1, the two line segments meet at the point p + t * r = q + u * s.
                 return true;
@@ -894,20 +912,56 @@ public final class VectorUtil {
      * <p>
      * Implementation uses float precision.
      * </p>
+     * @param p vertex 1 of first segment
+     * @param p2 vertex 2 of first segment
+     * @param q vertex 1 of second segment
+     * @param q2 vertex 2 of second segment
+     * @param epsilon optional epsilon. If {@code 0} just compare against {@code 0}, otherwise compare against this {@code epsilon}. See {@link FloatUtil#EPSILON}
+     * @param doCollinear consider collinear case, i.e. overlapping segments as an intersection returning {@code true}
+     * @return true if line segments are intersecting, otherwise false
      */
-    public static boolean testSeg2SegIntersection(final Vert2fImmutable p, final Vert2fImmutable p2, final Vert2fImmutable q, final Vert2fImmutable q2)
+    public static boolean testSeg2SegIntersection(final Vert2fImmutable p, final Vert2fImmutable p2, final Vert2fImmutable q, final Vert2fImmutable q2,
+                                                  final float epsilon, final boolean doCollinear)
     {
         // Operations: 11+, 8*, 2 branches
-        final float eps = FloatUtil.EPSILON;
         final float rx = p2.x() - p.x(); // p2.minus(p)
         final float ry = p2.y() - p.y();
         final float sx = q2.x() - q.x(); // q2.minus(q)
         final float sy = q2.y() - q.y();
         final float rxs = rx * sy - ry * sx; // r.cross(s)
 
-        if ( FloatUtil.isZero(rxs) ) {
-            // Not considering collinear case as an intersection
-            return false;
+        if ( FloatUtil.isZero(rxs, epsilon) ) {
+            if( doCollinear ) {
+                final float q_px = q.x() - p.x(); // q.minus(p)
+                final float q_py = q.y() - p.y();
+                final float qpxr = q_px * ry - q_py * rx; // q_p.cross(r)
+                if ( FloatUtil.isZero(qpxr, epsilon) ) // disabled collinear case
+                {
+                    // 1) r x s = 0 and (q - p) x r = 0, the two lines are collinear.
+                    final float p_qx = p.x() - q.x(); // p.minus(q)
+                    final float p_qy = p.y() - q.y();
+                    final float qp_dot_r = q_px * rx + q_py * ry; // q_p.dot(r);
+                    final float pq_dot_s = p_qx * sx + p_qy * sy; // p_q.dot(s);
+                    final float r_dot_r = rx * rx + ry * ry; // r.dot(r);
+                    final float s_dot_s = sx * sx + sy * sy; // r.dot(r);
+                    // if ( ( 0 <= qp_dot_r && qp_dot_r <= r.dot(r) ) ||
+                    //      ( 0 <= pq_dot_s && pq_dot_s <= s.dot(s) ) )
+                    if ( ( epsilon <= qp_dot_r && qp_dot_r - r_dot_r <= epsilon ) ||
+                         ( epsilon <= pq_dot_s && pq_dot_s - s_dot_s <= epsilon ) )
+                    {
+                        // 1.1) 0 <= (q - p) · r <= r · r or 0 <= (p - q) · s <= s · s, the two lines are overlapping
+                        return true;
+                    }
+                    // 1.2 the two lines are collinear but disjoint.
+                    return false;
+                } else {
+                    // 2) r × s = 0 and (q − p) × r ≠ 0, the two lines are parallel and non-intersecting.
+                    return false;
+                }
+            } else {
+                // Not considering collinear case as an intersection
+                return false;
+            }
         } else {
             // r x s != 0
             final float q_px = q.x() - p.x(); // q.minus(p)
@@ -924,7 +978,7 @@ public final class VectorUtil {
             final float u = qpxr / rxs;
 
             // if ( (0 <= t && t <= 1) && (0 <= u && u <= 1) )
-            if ( (eps <= t && t - 1 <= eps) && (eps <= u && u - 1 <= eps) )
+            if ( (epsilon <= t && t - 1 <= epsilon) && (epsilon <= u && u - 1 <= epsilon) )
             {
                 // 3) r × s ≠ 0 and 0 ≤ t ≤ 1 and 0 ≤ u ≤ 1, the two line segments meet at the point p + t * r = q + u * s.
                 return true;
@@ -942,14 +996,16 @@ public final class VectorUtil {
      * @param c vertex 3 of the triangle
      * @param d vertex 1 of first segment
      * @param e vertex 2 of first segment
+     * @param epsilon optional epsilon. If {@code 0} just compare against {@code 0}, otherwise compare against this {@code epsilon}. See {@link FloatUtil#EPSILON}
+     * @param doCollinear consider collinear case, i.e. overlapping segments as an intersection returning {@code true}
      * @return true if the segment intersects at least one segment of the triangle, false otherwise
-     * @see #testSeg2SegIntersection(Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, Vert2fImmutable)
+     * @see #testSeg2SegIntersection(Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, Vert2fImmutable, float, boolean)
      */
     public static boolean testTri2SegIntersection(final Vert2fImmutable a, final Vert2fImmutable b, final Vert2fImmutable c,
-                                                  final Vert2fImmutable d, final Vert2fImmutable e){
-        return testSeg2SegIntersection(a, b, d, e) ||
-               testSeg2SegIntersection(b, c, d, e) ||
-               testSeg2SegIntersection(a, c, d, e) ;
+                                                  final Vert2fImmutable d, final Vert2fImmutable e, final float epsilon, final boolean doCollinear) {
+        return testSeg2SegIntersection(a, b, d, e, epsilon, doCollinear) ||
+               testSeg2SegIntersection(b, c, d, e, epsilon, doCollinear) ||
+               testSeg2SegIntersection(a, c, d, e, epsilon, doCollinear) ;
     }
 
     /**
diff --git a/src/jogl/classes/jogamp/graph/curve/tess/Loop.java b/src/jogl/classes/jogamp/graph/curve/tess/Loop.java
index a318bd555..a69fbd6b5 100644
--- a/src/jogl/classes/jogamp/graph/curve/tess/Loop.java
+++ b/src/jogl/classes/jogamp/graph/curve/tess/Loop.java
@@ -346,20 +346,20 @@ public class Loop {
     }
     private boolean intersectsOutline(final GraphOutline outline, final Vertex a1, final Vertex a2, final Vertex b) {
         final ArrayList<GraphVertex> vertices = outline.getGraphPoint();
-        if( vertices.size() < 2 ) {
+        final int sz = vertices.size();
+        if( sz < 2 ) {
             return false;
         }
-        final int sz = vertices.size();
         Vertex v0 = vertices.get(0).getPoint();
         for(int i=1; i< sz; i++){
             final Vertex v1 = vertices.get(i).getPoint();
             if( !( v0 == b || v1 == b ) ) {
                 if( !( v0 == a1 || v1 == a1 ) &&
-                    VectorUtil.testSeg2SegIntersection(a1, b, v0, v1) ) {
+                    VectorUtil.testSeg2SegIntersection(a1, b, v0, v1, FloatUtil.EPSILON, false) ) {
                     return true;
                 }
                 if( !( v0 == a2 || v1 == a2 ) &&
-                    VectorUtil.testSeg2SegIntersection(a2, b, v0, v1) ) {
+                    VectorUtil.testSeg2SegIntersection(a2, b, v0, v1, FloatUtil.EPSILON, false) ) {
                     return true;
                 }
             }
@@ -377,21 +377,21 @@ public class Loop {
     }
     private boolean intersectsOutlineDbg(final GraphOutline outline, final Vertex a1, final Vertex a2, final Vertex b) {
         final ArrayList<GraphVertex> vertices = outline.getGraphPoint();
-        if( vertices.size() < 2 ) {
+        final int sz = vertices.size();
+        if( sz < 2 ) {
             return false;
         }
-        final int sz = vertices.size();
         Vertex v0 = vertices.get(0).getPoint();
         for(int i=1; i< sz; i++){
             final Vertex v1 = vertices.get(i).getPoint();
             if( !( v0 == b || v1 == b ) ) {
                 if( !( v0 == a1 || v1 == a1 ) &&
-                    VectorUtil.testSeg2SegIntersection(a1, b, v0, v1) ) {
+                    VectorUtil.testSeg2SegIntersection(a1, b, v0, v1, FloatUtil.EPSILON, false) ) {
                     System.err.printf("Loop.intersection.b-a1.1: %d/%d %s to%n-a1 %s, with%n-v0 %s%n-v1 %s%n", i, sz-1, b, a1, v0, v1);
                     return true;
                 }
                 if( !( v0 == a2 || v1 == a2 ) &&
-                    VectorUtil.testSeg2SegIntersection(a2, b, v0, v1) ) {
+                    VectorUtil.testSeg2SegIntersection(a2, b, v0, v1, FloatUtil.EPSILON, false) ) {
                     System.err.printf("Loop.intersection.b-a2.1: %d/%d %s to%n-a2 %s, with%n-v0 %s%n-v1 %s%n", i, sz-1, b, a2, v0, v1);
                     return true;
                 }