Fix oracle function and intersection bug in csg2d

libsrc/geom2d/csg2d.cpp

@@ -150,7 +150,6 @@ IntersectionType ClassifyNonOverlappingIntersection( double alpha, double beta )
   if (alpha_is_0 && beta_is_0)
     return (V_INTERSECTION);
 
-  cout << alpha << ',' << beta << ',' << alpha_is_0 << ',' << beta_is_0 << endl;
   return NO_INTERSECTION;
 }
 
@@ -274,7 +273,6 @@ IntersectionType IntersectSplineSegment( const Spline & s, const Point<2> & r0,
   int dim = fabs(vr[0]) > fabs(vr[1]) ? 0 : 1;
   beta = 1.0/vr[dim] * (s.GetPoint(t)[dim] - r0[dim]);
 
-  cout << "intersect splinesegment  " << alpha << ',' << beta << ',' << ClassifyNonOverlappingIntersection(alpha, beta) << endl;
   return ClassifyNonOverlappingIntersection(alpha, beta);
 }
 
@@ -295,8 +293,11 @@ IntersectionType IntersectSplineSegment1( const Spline & s, const Point<2> & r0,
   double c_ = a0;
 
   double det =  b_*b_ - 4*a_*c_;
-  if(det<0.0)
+  if(det<-EPSILON)
-    return NO_INTERSECTION;
+      return NO_INTERSECTION;
+
+  if(det<EPSILON)
+      det = 0;
 
   double sqrt_det =  sqrt(det);
   double vbeta[2];
@@ -341,7 +342,6 @@ IntersectionType IntersectSplineSegment1( const Spline & s, const Point<2> & r0,
 
   alpha = valpha[choice];
   beta = vbeta[choice];
-  cout << "intersect splinesegment1 " << alpha << ',' << beta << ',' << vtype[choice] << endl;
   return vtype[choice];
 }
 
@@ -721,7 +721,6 @@ void AddIntersectionPoint(Edge edgeP, Edge edgeQ, IntersectionType i, double alp
       I_P = edgeP.v0->Insert(I, alpha);
       I_Q = edgeQ.v0->Insert(I, beta);
       I_P->Link(I_Q);
-      cout << "Add X Intersection " << *I_P << alpha << ',' << beta << endl;
       break;
 
     case X_OVERLAP:
@@ -730,28 +729,23 @@ void AddIntersectionPoint(Edge edgeP, Edge edgeQ, IntersectionType i, double alp
 
       I_P = edgeP.v0->Insert(*Q1, alpha);
       I_P->Link( Q1);
-      cout << "Add X Overlap " << *I_P << alpha << ',' << beta << endl;
       break;
 
     case T_INTERSECTION_Q:
     case T_OVERLAP_Q:
       I_Q = edgeQ.v0->Insert(*P1, beta);
       P1->Link( I_Q);
-      cout << "Add T int/overlap Q " << *P1 << alpha << ',' << beta << endl;
       break;
 
     case T_INTERSECTION_P:
     case T_OVERLAP_P:
       I_P = edgeP.v0->Insert(*Q1, alpha);
       I_P->Link( Q1);
-      cout << "Add T int/overlap P " << *I_P << alpha << ',' << beta << endl;
       break;
 
     case V_INTERSECTION:
     case V_OVERLAP:
       P1->Link(Q1);
-      cout << "Add V int/overlap " << *P1 << alpha << ',' << beta << endl;
-      cout << *P1 << *P1->next  << *Q1 << *Q1->next << endl;
       break;
     default:
       break;
@@ -810,8 +804,8 @@ void ComputeIntersections(Edge edgeP , Loop & l2)
 {
   for (Edge edgeQ : l2.Edges(SOURCE))
   {
-    double alpha = 0.0;
+    double alpha = -1;
-    double beta = 0.0;
+    double beta = -1;
     IntersectionType i = intersect(edgeP, edgeQ, alpha, beta);
     AddIntersectionPoint(edgeP, edgeQ, i, alpha, beta);
     if(i==X_INTERSECTION && (edgeP.v0->spline || edgeQ.v0->spline))
@@ -821,7 +815,6 @@ void ComputeIntersections(Edge edgeP , Loop & l2)
 
       // search for possible second intersection
       i = intersect(edgeP, edgeQ, alpha1, beta1);
-      cout << "second intersection " << i << ',' << alpha1 << ',' << beta1 << ',' << alpha1-alpha << ',' << beta1-beta << endl;
       if(i!=NO_INTERSECTION && alpha+EPSILON<alpha1)
       {
         // Add midpoint of two intersection points to avoid false overlap detection of splines
@@ -886,61 +879,8 @@ enum RelativePositionType
   IS_P_p
 };
 
-RelativePositionType oracle(bool prev, Vertex* P1, Vertex* P2, Vertex* P3)
+inline RelativePositionType oracle_decide( double s1, double s2, double s3 )
 {
-  Vertex* Q;
-  Point<2> q;
-  if(prev)
-  {
-    Q = P2->neighbour->prev;
-    q = *Q;
-    if(Q->spline)
-      q = Q->spline->TangentPoint();
-  }
-  else
-  {
-    Q = P2->neighbour->next;
-    q = *Q;
-    if(P2->neighbour->spline)
-      q = P2->neighbour->spline->TangentPoint();
-  }
-
-  // is Q linked to P1 ?
-  if ( P1->is_intersection && (P1->neighbour == Q) )
-    return(IS_P_m);
-
-  // is Q linked to P2 ?
-  if ( P3->is_intersection && (P3->neighbour == Q) )
-    return(IS_P_p);
-
-  Point<2> p1 = *P1;
-  Point<2> p2 = *P2;
-  Point<2> p3 = *P3;
-
-  double s1, s2, s3;
-
-  if(P1->spline)
-    {
-      s1 = IsLeft(*P1->spline, q) ? 1 : -1;
-      p1 = P1->spline->TangentPoint();
-    }
-  else
-      s1 = Area(  q, p1, p2);
-
-  if(P2->spline)
-    {
-      s2 = IsLeft(*P2->spline, q) ? 1 : -1;
-      p2 = P2->spline->TangentPoint();
-    }
-  else
-      s2 = Area(  q, p2, p3);
-
-  // check relative position of Q with respect to chain (P1,P2,P3)
-  s3 = Area( p1, p2, p3);
-
-  cout << "Points for oracle " << q << p1 << p2 << p3 << endl;
-  cout << "areas " << s1 << ',' << s2 << ',' << s3 << endl;
-
   if (s3 > 0)
   {
     // chain makes a left turn
@@ -959,14 +899,139 @@ RelativePositionType oracle(bool prev, Vertex* P1, Vertex* P2, Vertex* P3)
   }
 }
 
+// no splines involved here
+// decides if Point q is left or right of chain (p1,p2,p3)
+RelativePositionType oracle_simple(Point<2> q, Point<2> p1, Point<2> p2, Point<2> p3)
+{
+  double s1 = Area(  q, p1, p2);
+  double s2 = Area(  q, p2, p3);
+  double s3 = Area( p1, p2, p3);
+
+  // check relative position of q with respect to chain (p1,p2,p3)
+  return oracle_decide(s1, s2, s3);
+}
+
+// (p1,p2) or (p2,p3) is a spline segment, compare with tangent (p1t,p2) instead of Segment (p1,p2)
+// BUT take care if tangent is collinear with (q,p2) (then use the segment (p1,p2) again)
+RelativePositionType oracle_spline_p(Point<2> q, Point<2> p1, Point<2> p1t, Point<2> p2, Point<2> p3, Point<2> p3t)
+{
+  double s1 = Area(  q, p1t, p2);
+  double s2 = Area(  q, p2, p3t);
+
+  if(fabs(s1) < EPSILON)
+    {
+      p1t = p1;
+      s1 = Area( q, p1t, p2 );
+    }
+
+  if(fabs(s2) < EPSILON)
+    {
+      p3t = p3;
+      s2 = Area( q, p2, p3t );
+    }
+
+  double s3 = Area( p1t, p2, p3t);
+
+  return oracle_decide(s1, s2, s3);
+}
+
+// (q,p2) is a spline segment, compare with tangent (qt,p2) instead of Segment (q,p2)
+// BUT take care if tangent at p2 is collinear with eiter (p1,p2) or (p2,p3) (then use the segment (q,p2) again)
+RelativePositionType oracle_spline_q(Point<2> q, Point<2> qt, Point<2> p1, Point<2> p2, Point<2> p3)
+{
+  double s1 = Area( qt, p1, p2);
+  double s2 = Area( qt, p2, p3);
+  double s3 = Area( p1, p2, p3);
+
+  if(fabs(s1) < EPSILON)
+      s1 = Area( q, p1, p2 );
+
+  if(fabs(s2) < EPSILON)
+      s2 = Area( q, p2, p3 );
+
+  return oracle_decide(s1, s2, s3);
+}
+
+// splines at (Q,P2) and either (P1,P2) or (P2,P3)
+// first use tangents to decide local orientation
+// if tangents of two splines match, use IsLeft(spline, other end point)
+// if tangent of spline and segment match, use simple methond (just end points)
+RelativePositionType oracle_spline(bool prev, Vertex *Q, Vertex *P1, Vertex *P2, Vertex *P3)
+{
+  Point<2> p1t =  *P1;
+  Point<2> p3t =  *P3;
+
+  auto sq = prev ? Q->spline : Q->prev->spline;
+  auto qt = sq->TangentPoint();
+  if(P1->spline) p1t = P1->spline->TangentPoint();
+  if(P2->spline) p3t = P2->spline->TangentPoint();
+
+  // Check using tangent directions first
+  double s1 = Area( qt, p1t, *P2 );
+  double s2 = Area( qt, *P2 , p3t);
+  double s3 = Area( p1t, *P2, p3t);
+
+  // tangents are facing in same direction
+  if(fabs(s1) < EPSILON)
+    {
+      if(P1->spline)
+          s1 = IsLeft(*P1->spline, *Q) ? 1 : -1;
+      else
+          s1 = Area( *Q, *P1, *P2 );
+    }
+
+  // tangents are facing in same direction
+  if(fabs(s2) < EPSILON)
+    {
+      if(P2->spline)
+          s2 = IsLeft(*P2->spline, *Q) ? 1 : -1;
+      else
+          s2 = Area( *Q, *P2, *P3 );
+    }
+
+  return oracle_decide(s1, s2, s3);
+}
+
+
 RelativePositionType oracle(bool prev, Vertex* P2)
 {
+  auto Q = prev ? P2->neighbour->prev : P2->neighbour->next;
+  auto sq = prev ? Q->spline : Q->prev->spline;
   Vertex* P1 = P2->prev;
   Vertex* P3 = P2->next;
 
-  return oracle(prev, P1, P2, P3);
+  // is Q linked to P1 ?
+  if ( P1->is_intersection && (P1->neighbour == Q) )
+    return(IS_P_m);
+
+  // is Q linked to P2 ?
+  if ( P3->is_intersection && (P3->neighbour == Q) )
+    return(IS_P_p);
+
+  // no splines -> simple variant
+  if(!P1->spline && !P2->spline && !Q->spline)
+      return oracle_simple(*Q, *P1, *P2, *P3);
+
+  Point<2> qt=*Q, p1t=*P1, p3t=*P3;
+
+  // splines -> also consider tangent points
+  if( sq) qt  =  Q->spline->TangentPoint();
+  if(P1->spline) p1t = P1->spline->TangentPoint();
+  if(P2->spline) p3t = P2->spline->TangentPoint();
+
+  // only spline at Q
+  if(!P1->spline && !P2->spline && Q->spline)
+      return oracle_spline_q(*Q, qt, *P1, *P2, *P3);
+
+  // only spline at P
+  if((P1->spline || !P2->spline) && !Q->spline)
+      return oracle_spline_p(*Q, *P1, p1t, *P2, *P3, p3t);
+
+  // spline at Q and P1 or P2
+  return oracle_spline(prev, Q, P1, P2, P3);
 }
 
+
 void LabelIntersections(Solid2d & sp, Solid2d & sq, Solid2d & sr, bool UNION)
 {
   auto & PP = sp.polys;
@@ -1016,7 +1081,6 @@ void LabelIntersections(Solid2d & sp, Solid2d & sq, Solid2d & sr, bool UNION)
       if ( ( (Q_m_type == IS_P_m) && (Q_p_type == LEFT) ) ||
           ( (Q_p_type == IS_P_m) && (Q_m_type == LEFT) ) )
         I->label = ON_RIGHT;
-      cout << "label " << *I << " = " << I->label << endl;
     }
 
   // 2) classify intersection chains
@@ -1028,7 +1092,6 @@ void LabelIntersections(Solid2d & sp, Solid2d & sq, Solid2d & sr, bool UNION)
       if (I->label == LEFT_ON ||
           I->label == RIGHT_ON)
       {
-        cout << "intersection chain " << *I << endl;
 
         // remember status of the first chain vertex and vertex itself
         RelativePositionType x;