Add Newton iterations to intersect a line with a surface at a

point, and to intersect three surfaces at a point. So now when we
split an edge, we can refine the split point to lie exactly on the
trim curve, so I can do certain Booleans on curved surfaces.

But surface-line intersection is globally broken, since I don't
correctly detect the number of intersections or provide a good
first guess. I maybe should test by bounding boxes and subdivision.

[git-p4: depot-paths = "//depot/solvespace/": change = 1920]

dsc.h

@@ -52,6 +52,10 @@ public:
     static Vector AtIntersectionOfPlaneAndLine(Vector n, double d,
                                                Vector p0, Vector p1,
                                                bool *parallel);
+    static Vector AtIntersectionOfPlanes(Vector na, double da,
+                                         Vector nb, double db,
+                                         Vector nc, double dc,
+                                         bool *parallel);
 
     double Element(int i);
     bool Equals(Vector v, double tol=LENGTH_EPS);

srf/boolean.cpp

@@ -1,6 +1,6 @@
 #include "solvespace.h"
 
-static int I, N;
+int I, N, FLAG;
 
 void SShell::MakeFromUnionOf(SShell *a, SShell *b) {
     MakeFromBoolean(a, b, AS_UNION);
@@ -10,6 +10,14 @@ void SShell::MakeFromDifferenceOf(SShell *a, SShell *b) {
     MakeFromBoolean(a, b, AS_DIFFERENCE);
 }
 
+//-----------------------------------------------------------------------------
+// Take our original pwl curve. Wherever an edge intersects a surface within
+// either agnstA or agnstB, split the piecewise linear element. Then refine
+// the intersection so that it lies on all three relevant surfaces: the
+// intersecting surface, srfA, and srfB. (So the pwl curve should lie at
+// the intersection of srfA and srfB.) Return a new pwl curve with everything
+// split.
+//-----------------------------------------------------------------------------
 static Vector LineStart, LineDirection;
 static int ByTAlongLine(const void *av, const void *bv)
 {
@@ -21,10 +29,11 @@ static int ByTAlongLine(const void *av, const void *bv)
 
     return (ta > tb) ? 1 : -1;
 }
-SCurve SCurve::MakeCopySplitAgainst(SShell *agnstA, SShell *agnstB) {
+SCurve SCurve::MakeCopySplitAgainst(SShell *agnstA, SShell *agnstB,
+                                    SSurface *srfA, SSurface *srfB)
+{
     SCurve ret;
     ret = *this;
-    ret.interCurve = false;
     ZERO(&(ret.pts));
 
     Vector *p = pts.First();
@@ -51,6 +60,14 @@ SCurve SCurve::MakeCopySplitAgainst(SShell *agnstA, SShell *agnstB) {
             Vector prev = Vector::From(VERY_POSITIVE, 0, 0);
             SInter *pi;
             for(pi = il.First(); pi; pi = il.NextAfter(pi)) {
+                // The intersection was generated by intersecting the pwl
+                // edge against a surface; so it must be refined to lie
+                // exactly on the original curve.
+                double u, v;
+                (pi->srf)->ClosestPointTo(pi->p, &u, &v, false);
+                (pi->srf)->PointOnSurfaces(srfA, srfB, &u, &v);
+                pi->p = (pi->srf)->PointAt(u, v);
+
                 // On-edge intersection will generate same split point for
                 // both surfaces, so don't create zero-length edge.
                 if(!prev.Equals(pi->p)) {
@@ -67,10 +84,14 @@ SCurve SCurve::MakeCopySplitAgainst(SShell *agnstA, SShell *agnstB) {
     return ret;
 }
 
-void SShell::CopyCurvesSplitAgainst(SShell *aga, SShell *agb, SShell *into) {
+void SShell::CopyCurvesSplitAgainst(bool opA, SShell *agnst, SShell *into) {    
     SCurve *sc;
     for(sc = curve.First(); sc; sc = curve.NextAfter(sc)) {
-        SCurve scn = sc->MakeCopySplitAgainst(aga, agb);
+        SCurve scn = sc->MakeCopySplitAgainst(agnst, NULL,
+                                surface.FindById(sc->surfA),
+                                surface.FindById(sc->surfB));
+        scn.source = opA ? SCurve::FROM_A : SCurve::FROM_B;
+
         hSCurve hsc = into->curve.AddAndAssignId(&scn);
         // And note the new ID so that we can rewrite the trims appropriately
         sc->newH = hsc;
@@ -289,7 +310,7 @@ SSurface SSurface::MakeCopyTrimAgainst(SShell *agnst, SShell *parent,
     for(ss = agnst->surface.First(); ss; ss = agnst->surface.NextAfter(ss)) {
         SCurve *sc;
         for(sc = into->curve.First(); sc; sc = into->curve.NextAfter(sc)) {
-            if(!(sc->interCurve)) continue;
+            if(sc->source != SCurve::FROM_INTERSECTION) continue;
             if(opA) {
                 if(sc->surfB.v != h.v || sc->surfA.v != ss->h.v) continue;
             } else {
@@ -351,7 +372,11 @@ SSurface SSurface::MakeCopyTrimAgainst(SShell *agnst, SShell *parent,
                eb = ret.PointAt(buv.x, buv.y);
         Vector ptout = n.Cross((eb.Minus(ea)));
 
+        if(I == 1 && fabs(((se->b).Minus(se->a).Magnitude()) - 0.166) < 0.01) {
+            FLAG = 1;
+        }
         int c_shell = agnst->ClassifyPoint(pt, ptout);
+        FLAG = 0;
 
         int tag = 0;
         tag |= INSIDE(ORIG, INDIR);
@@ -376,6 +401,11 @@ SSurface SSurface::MakeCopyTrimAgainst(SShell *agnst, SShell *parent,
     
         if(KeepEdge(type, opA, tag)) {
             final.AddEdge(se->a, se->b, se->auxA, se->auxB);
+        } else {
+            if(I == 1) {
+                dbp("orig vs. shell: %d (l=%g)",
+                    c_shell, ((se->b).Minus(se->a)).Magnitude());
+            }
         }
     }
 
@@ -426,7 +456,7 @@ SSurface SSurface::MakeCopyTrimAgainst(SShell *agnst, SShell *parent,
     }
     final.l.RemoveTagged();
 
-//    if(I == 0) DEBUGEDGELIST(&final, &ret);
+//    if(I == 1) DEBUGEDGELIST(&final, &ret);
 
     // Use our reassembled edges to trim the new surface.
     ret.TrimFromEdgeList(&final);
@@ -477,8 +507,8 @@ void SShell::MakeFromBoolean(SShell *a, SShell *b, int type) {
     // Copy over all the original curves, splitting them so that a
     // piecwise linear segment never crosses a surface from the other
     // shell.
-    a->CopyCurvesSplitAgainst(b, NULL, this);
+    a->CopyCurvesSplitAgainst(true,  b, this);
-    b->CopyCurvesSplitAgainst(a, NULL, this);
+    b->CopyCurvesSplitAgainst(false, a, this);
 
     // Generate the intersection curves for each surface in A against all
     // the surfaces in B (which is all of the intersection curves).
@@ -490,8 +520,8 @@ void SShell::MakeFromBoolean(SShell *a, SShell *b, int type) {
         a->CopySurfacesTrimAgainst(b, this, type, true);
         b->CopySurfacesTrimAgainst(a, this, type, false);
     } else {
+        I = 0;
         a->CopySurfacesTrimAgainst(b, this, type, true);
-        I = -1;
         b->CopySurfacesTrimAgainst(a, this, type, false);
     }
 

srf/ratpoly.cpp

@@ -1,6 +1,12 @@
 #include "../solvespace.h"
 
-double Bernstein(int k, int deg, double t)
+// Converge it to better than LENGTH_EPS; we want two points, each
+// independently projected into uv and back, to end up equal with the
+// LENGTH_EPS. Best case that requires LENGTH_EPS/2, but more is better
+// and convergence should be fast by now.
+#define RATPOLY_EPS (LENGTH_EPS/(1e1))
+
+static double Bernstein(int k, int deg, double t)
 {
     if(k > deg || k < 0) return 0;
 
@@ -523,13 +529,13 @@ Vector SSurface::NormalAt(double u, double v) {
     return tu.Cross(tv);
 }
 
-void SSurface::ClosestPointTo(Vector p, double *u, double *v) {
+void SSurface::ClosestPointTo(Vector p, double *u, double *v, bool converge) {
     int i, j;
 
     if(degm == 1 && degn == 1) {
         *u = *v = 0; // a plane, perfect no matter what the initial guess
     } else {
-        double minDist = 1e10;
+        double minDist = VERY_POSITIVE;
         double res = 7.0;
         for(i = 0; i < (int)res; i++) {
             for(j = 0; j <= (int)res; j++) {
@@ -548,15 +554,13 @@ void SSurface::ClosestPointTo(Vector p, double *u, double *v) {
 
     // Initial guess is in u, v
     Vector p0;
-    for(i = 0; i < 50; i++) {
+    for(i = 0; i < (converge ? 15 : 3); i++) {
         p0 = PointAt(*u, *v);
-        // Converge it to better than LENGTH_EPS; we want two points, each
+        if(converge) {
-        // independently projected into uv and back, to end up equal with
+            if(p0.Equals(p, RATPOLY_EPS)) {
-        // the LENGTH_EPS. Best case that requires LENGTH_EPS/2, but more
-        // is better and convergence should be fast by now.
-        if(p0.Equals(p, LENGTH_EPS/1e2)) {
                 return;
             }
+        }
 
         Vector tu, tv;
         TangentsAt(*u, *v, &tu, &tv);
@@ -569,15 +573,96 @@ void SSurface::ClosestPointTo(Vector p, double *u, double *v) {
         *u += du / (tu.MagSquared());
         *v += dv / (tv.MagSquared());
     }
+
+    if(converge) {
         dbp("didn't converge");
         dbp("have %.3f %.3f %.3f", CO(p0));
         dbp("want %.3f %.3f %.3f", CO(p));
         dbp("distance = %g", (p.Minus(p0)).Magnitude());
+    }
+
     if(isnan(*u) || isnan(*v)) {
         *u = *v = 0;
     }
 }
 
+bool SSurface::PointIntersectingLine(Vector p0, Vector p1, double *u, double *v)
+{
+    int i;
+    for(i = 0; i < 15; i++) {
+        Vector pi, p, tu, tv;
+        p = PointAt(*u, *v);
+        TangentsAt(*u, *v, &tu, &tv);
+        
+        Vector n = (tu.Cross(tv)).WithMagnitude(1);
+        double d = p.Dot(n);
+
+        bool parallel;
+        pi = Vector::AtIntersectionOfPlaneAndLine(n, d, p0, p1, &parallel);
+        if(parallel) break;
+
+        // Check for convergence
+        if(pi.Equals(p, RATPOLY_EPS)) return true;
+
+        // Adjust our guess and iterate
+        Vector dp = pi.Minus(p);
+        double du = dp.Dot(tu), dv = dp.Dot(tv);
+        *u += du / (tu.MagSquared());
+        *v += dv / (tv.MagSquared());
+    }
+//    dbp("didn't converge (surface intersecting line)");
+    return false;
+}
+
+void SSurface::PointOnSurfaces(SSurface *s1, SSurface *s2,
+                                                double *up, double *vp)
+{
+    double u[3] = { *up, 0, 0 }, v[3] = { *vp, 0, 0 };
+    SSurface *srf[3] = { this, s1, s2 };
+
+    // Get initial guesses for (u, v) in the other surfaces
+    Vector p = PointAt(*u, *v);
+    (srf[1])->ClosestPointTo(p, &(u[1]), &(v[1]), false);
+    (srf[2])->ClosestPointTo(p, &(u[2]), &(v[2]), false);
+
+    int i, j;
+    for(i = 0; i < 15; i++) {
+        // Approximate each surface by a plane
+        Vector p[3], tu[3], tv[3], n[3];
+        double d[3];
+        for(j = 0; j < 3; j++) {
+            p[j] = (srf[j])->PointAt(u[j], v[j]);
+            (srf[j])->TangentsAt(u[j], v[j], &(tu[j]), &(tv[j]));
+            n[j] = ((tu[j]).Cross(tv[j])).WithMagnitude(1);
+            d[j] = (n[j]).Dot(p[j]);
+        }
+
+        // If a = b and b = c, then does a = c? No, it doesn't.
+        if((p[0]).Equals(p[1], RATPOLY_EPS) && 
+           (p[1]).Equals(p[2], RATPOLY_EPS) &&
+           (p[2]).Equals(p[0], RATPOLY_EPS))
+        {
+            *up = u[0];
+            *vp = v[0];
+            return;
+        }
+
+        bool parallel;
+        Vector pi = Vector::AtIntersectionOfPlanes(n[0], d[0],
+                                                   n[1], d[1],
+                                                   n[2], d[2], &parallel);
+        if(parallel) break;
+
+        for(j = 0; j < 3; j++) {
+            Vector dp = pi.Minus(p[j]);
+            double du = dp.Dot(tu[j]), dv = dp.Dot(tv[j]);
+            u[j] += du / (tu[j]).MagSquared();
+            v[j] += dv / (tv[j]).MagSquared();
+        }
+    }
+    dbp("didn't converge (three surfaces intersecting)");
+}
+
 void SSurface::GetAxisAlignedBounding(Vector *ptMax, Vector *ptMin) {
     *ptMax = Vector::From(VERY_NEGATIVE, VERY_NEGATIVE, VERY_NEGATIVE);
     *ptMin = Vector::From(VERY_POSITIVE, VERY_POSITIVE, VERY_POSITIVE);
@@ -756,29 +841,28 @@ void SShell::MakeFromExtrusionOf(SBezierLoopSet *sbls, Vector t0, Vector t1,
             SCurve sc;
             ZERO(&sc);
             sb->MakePwlInto(&(sc.pts), t0);
+            sc.surfA = hs0;
+            sc.surfB = hsext;
             hSCurve hc0 = curve.AddAndAssignId(&sc);
-            STrimBy stb0 = STrimBy::EntireCurve(this, hc0, false);
 
             ZERO(&sc);
             sb->MakePwlInto(&(sc.pts), t1);
+            sc.surfA = hs1;
+            sc.surfB = hsext;
             hSCurve hc1 = curve.AddAndAssignId(&sc);
-            STrimBy stb1 = STrimBy::EntireCurve(this, hc1, true);
 
+            STrimBy stb0, stb1;
             // The translated curves trim the flat top and bottom surfaces.
+            stb0 = STrimBy::EntireCurve(this, hc0, false);
+            stb1 = STrimBy::EntireCurve(this, hc1, true);
             (surface.FindById(hs0))->trim.Add(&stb0);
-            (curve.FindById(hc0))->surfA = hs0;
-
             (surface.FindById(hs1))->trim.Add(&stb1);
-            (curve.FindById(hc1))->surfA = hs1;
 
             // The translated curves also trim the surface of extrusion.
             stb0 = STrimBy::EntireCurve(this, hc0, true);
-            (surface.FindById(hsext))->trim.Add(&stb0);
-            (curve.FindById(hc0))->surfB = hsext;
-
             stb1 = STrimBy::EntireCurve(this, hc1, false);
+            (surface.FindById(hsext))->trim.Add(&stb0);
             (surface.FindById(hsext))->trim.Add(&stb1);
-            (curve.FindById(hc1))->surfB = hsext;
 
             // And form the trim line
             Vector pt = sb->Finish();

srf/surface.h

@@ -6,6 +6,8 @@
 double Bernstein(int k, int deg, double t);
 double BernsteinDerivative(int k, int deg, double t);
 
+class SSurface;
+
 // Utility data structure, a two-dimensional BSP to accelerate polygon
 // operations.
 class SBspUv {
@@ -111,8 +113,14 @@ class SCurve {
 public:
     hSCurve         h;
 
-    hSCurve         newH;       // when merging with booleans
+    // In a Boolean, C = A op B. The curves in A and B get copied into C, and
-    bool            interCurve; // it's a newly-calculated intersection
+    // therefore must get new hSCurves assigned. For the curves in A and B,
+    // we use newH to record their new handle in C.
+    hSCurve         newH;
+    static const int FROM_A             = 100;
+    static const int FROM_B             = 200;
+    static const int FROM_INTERSECTION  = 300;
+    int             source;
 
     bool            isExact;
     SBezier         exact;
@@ -123,7 +131,8 @@ public:
     hSSurface       surfB;
 
     static SCurve FromTransformationOf(SCurve *a, Vector t, Quaternion q);
-    SCurve MakeCopySplitAgainst(SShell *agnstA, SShell *agnstB);
+    SCurve MakeCopySplitAgainst(SShell *agnstA, SShell *agnstB,
+                                SSurface *srfA, SSurface *srfB);
 
     void Clear(void);
 };
@@ -148,8 +157,10 @@ public:
 // An intersection point between a line and a surface
 class SInter {
 public:
+    int         tag;
     Vector      p;
-    hSSurface   surface;
+    SSurface    *srf;
+    hSSurface   hsrf;
     Vector      surfNormal; // of the intersecting surface, at pinter
     bool        onEdge;     // pinter is on edge of trim poly
 };
@@ -181,12 +192,14 @@ public:
     void TrimFromEdgeList(SEdgeList *el);
     void IntersectAgainst(SSurface *b, SShell *agnstA, SShell *agnstB, 
                           SShell *into);
-    void AddExactIntersectionCurve(SBezier *sb, hSSurface hsb,
+    void AddExactIntersectionCurve(SBezier *sb, SSurface *srfB,
                           SShell *agnstA, SShell *agnstB, SShell *into);
     void AllPointsIntersecting(Vector a, Vector b,
                                     List<SInter> *l, bool seg, bool trimmed);
 
-    void ClosestPointTo(Vector p, double *u, double *v);
+    void ClosestPointTo(Vector p, double *u, double *v, bool converge=true);
+    bool PointIntersectingLine(Vector p0, Vector p1, double *u, double *v);
+    void PointOnSurfaces(SSurface *s1, SSurface *s2, double *u, double *v);
     Vector PointAt(double u, double v);
     void TangentsAt(double u, double v, Vector *tu, Vector *tv);
     Vector NormalAt(double u, double v);
@@ -217,7 +230,7 @@ public:
     static const int AS_DIFFERENCE = 11;
     static const int AS_INTERSECT  = 12;
     void MakeFromBoolean(SShell *a, SShell *b, int type);
-    void CopyCurvesSplitAgainst(SShell *agnstA, SShell *agnstB, SShell *into);
+    void CopyCurvesSplitAgainst(bool opA, SShell *agnst, SShell *into);
     void CopySurfacesTrimAgainst(SShell *against, SShell *into, int t, bool a);
     void MakeIntersectionCurvesAgainst(SShell *against, SShell *into);
     void MakeClassifyingBsps(void);

srf/surfinter.cpp

@@ -1,25 +1,28 @@
 #include "solvespace.h"
 
-void SSurface::AddExactIntersectionCurve(SBezier *sb, hSSurface hsb,
+extern int FLAG;
+
+void SSurface::AddExactIntersectionCurve(SBezier *sb, SSurface *srfB,
                             SShell *agnstA, SShell *agnstB, SShell *into)
 {
     SCurve sc;
     ZERO(&sc);
     sc.surfA = h;
-    sc.surfB = hsb;
+    sc.surfB = srfB->h;
     sb->MakePwlInto(&(sc.pts));
 
+/*
     Vector *prev = NULL, *v;
     for(v = sc.pts.First(); v; v = sc.pts.NextAfter(v)) {
       if(prev) SS.nakedEdges.AddEdge(*prev, *v);
         prev = v;
-    }
+    } */
 
     // Now split the line where it intersects our existing surfaces
-    SCurve split = sc.MakeCopySplitAgainst(agnstA, agnstB);
+    SCurve split = sc.MakeCopySplitAgainst(agnstA, agnstB, this, srfB);
     sc.Clear();
 
-    split.interCurve = true;
+    split.source = SCurve::FROM_INTERSECTION;
     into->curve.AddAndAssignId(&split);
 }
 
@@ -79,7 +82,7 @@ void SSurface::IntersectAgainst(SSurface *b, SShell *agnstA, SShell *agnstB,
                 SBezier bezier;
                 bezier = SBezier::From((si->p).Minus(al), (si->p).Plus(al));
 
-                AddExactIntersectionCurve(&bezier, b->h, agnstA, agnstB, into);
+                AddExactIntersectionCurve(&bezier, b, agnstA, agnstB, into);
             }
 
             inters.Clear();
@@ -96,7 +99,7 @@ void SSurface::IntersectAgainst(SSurface *b, SShell *agnstA, SShell *agnstB,
                     Vector::AtIntersectionOfPlaneAndLine(n, d, p0, p1, NULL);
             }
 
-            AddExactIntersectionCurve(&bezier, b->h, agnstA, agnstB, into);
+            AddExactIntersectionCurve(&bezier, b, agnstA, agnstB, into);
         }
     }
 
@@ -152,8 +155,14 @@ void SSurface::AllPointsIntersecting(Vector a, Vector b,
             }
 
             Inter inter;
-            ClosestPointTo(pi, &inter.p.x, &inter.p.y);
+            ClosestPointTo(pi, &inter.p.x, &inter.p.y, false);
+            if(PointIntersectingLine(a, b, &inter.p.x, &inter.p.y)) {
                 inters.Add(&inter);
+            } else {
+                // No convergence; but this might not be an error, since
+                // the line can intersect the convex hull more times than
+                // it intersects the surface itself.
+            }
         }
     }
 
@@ -189,7 +198,8 @@ void SSurface::AllPointsIntersecting(Vector a, Vector b,
         SInter si;
         si.p = pxyz;
         si.surfNormal = NormalAt(puv.x, puv.y);
-        si.surface = h;
+        si.hsrf = h;
+        si.srf = this;
         si.onEdge = (c != SBspUv::INSIDE);
         l->Add(&si);
     }
@@ -236,6 +246,11 @@ int SShell::ClassifyPoint(Vector p, Vector pout) {
         bool onEdge = false;
         edge_inters = 0;
 
+        if(FLAG) {
+            dbp("inters=%d cnt=%d", l.n, cnt);
+            SS.nakedEdges.AddEdge(p, p.Plus(ray.WithMagnitude(50)));
+        }
+
         SInter *si;
         for(si = l.First(); si; si = l.NextAfter(si)) {
             double t = ((si->p).Minus(p)).DivPivoting(ray);
@@ -244,6 +259,8 @@ int SShell::ClassifyPoint(Vector p, Vector pout) {
                 continue;
             }
 
+            if(FLAG) SS.nakedEdges.AddEdge(p, si->p);
+
             double d = ((si->p).Minus(p)).Magnitude();
 
             // Handle edge-on-edge

util.cpp

@@ -640,6 +640,48 @@ Vector Vector::AtIntersectionOfPlaneAndLine(Vector n, double d,
     return p0.Plus(dp.ScaledBy(t));
 }
 
+static double det2(double a1, double b1,
+                   double a2, double b2)
+{
+    return (a1*b2) - (b1*a2);
+}
+static double det3(double a1, double b1, double c1,
+                   double a2, double b2, double c2,
+                   double a3, double b3, double c3)
+{
+    return a1*det2(b2, c2, b3, c3) -
+           b1*det2(a2, c2, a3, c3) +
+           c1*det2(a2, b2, a3, b3);
+}
+Vector Vector::AtIntersectionOfPlanes(Vector na, double da,
+                                      Vector nb, double db,
+                                      Vector nc, double dc,
+                                      bool *parallel)
+{
+    double det  = det3(na.x, na.y, na.z,
+                       nb.x, nb.y, nb.z,
+                       nc.x, nc.y, nc.z);
+    if(fabs(det) < 1e-10) { // arbitrary tolerance, not so good
+        *parallel = true;
+        return Vector::From(0, 0, 0);
+    }
+    *parallel = false;
+
+    double detx = det3(da,   na.y, na.z,
+                       db,   nb.y, nb.z,
+                       dc,   nc.y, nc.z);
+
+    double dety = det3(na.x, da,   na.z,
+                       nb.x, db,   nb.z,
+                       nc.x, dc,   nc.z);
+
+    double detz = det3(na.x, na.y, da,
+                       nb.x, nb.y, db,
+                       nc.x, nc.y, dc  );
+
+    return Vector::From(detx/det, dety/det, detz/det);
+}
+
 Point2d Point2d::Plus(Point2d b) {
     Point2d r;
     r.x = x + b.x;