slvs 0.6.0

//-----------------------------------------------------------------------------
// Routines for ray-casting: intersecting a line segment or an infinite line
// with a surface or shell. Ray-casting against a shell is used for point-in-
// shell testing, and the intersection of edge line segments against surfaces
// is used to get rough surface-curve intersections, which are later refined
// numerically.
//
// Copyright 2008-2013 Jonathan Westhues.
//-----------------------------------------------------------------------------
#include "solvespace.h"

// Dot product tolerance for perpendicular; this is on the direction cosine,
// so it's about 0.001 degrees.
const double SShell::DOTP_TOL = 1e-5;

extern int FLAG;


double SSurface::DepartureFromCoplanar() const {
    int i, j;
    int ia, ja, ib = 0, jb = 0, ic = 0, jc = 0;
    double best;

    // Grab three points to define a plane; first choose (0, 0) arbitrarily.
    ia = ja = 0;
    // Then the point farthest from pt a.
    best = VERY_NEGATIVE;
    for(i = 0; i <= degm; i++) {
        for(j = 0; j <= degn; j++) {
            if(i == ia && j == ja) continue;

            double dist = (ctrl[i][j]).Minus(ctrl[ia][ja]).Magnitude();
            if(dist > best) {
                best = dist;
                ib = i;
                jb = j;
            }
        }
    }
    // Then biggest magnitude of ab cross ac.
    best = VERY_NEGATIVE;
    for(i = 0; i <= degm; i++) {
        for(j = 0; j <= degn; j++) {
            if(i == ia && j == ja) continue;
            if(i == ib && j == jb) continue;

            double mag =
                ((ctrl[ia][ja].Minus(ctrl[ib][jb]))).Cross(
                 (ctrl[ia][ja].Minus(ctrl[i ][j ]))).Magnitude();
            if(mag > best) {
                best = mag;
                ic = i;
                jc = j;
            }
        }
    }

    Vector n = ((ctrl[ia][ja].Minus(ctrl[ib][jb]))).Cross(
                (ctrl[ia][ja].Minus(ctrl[ic][jc])));
    n = n.WithMagnitude(1);
    double d = (ctrl[ia][ja]).Dot(n);

    // Finally, calculate the deviation from each point to the plane.
    double farthest = VERY_NEGATIVE;
    for(i = 0; i <= degm; i++) {
        for(j = 0; j <= degn; j++) {
            double dist = fabs(n.Dot(ctrl[i][j]) - d);
            if(dist > farthest) {
                farthest = dist;
            }
        }
    }
    return farthest;
}

void SSurface::WeightControlPoints() {
    int i, j;
    for(i = 0; i <= degm; i++) {
        for(j = 0; j <= degn; j++) {
            ctrl[i][j] = (ctrl[i][j]).ScaledBy(weight[i][j]);
        }
    }
}
void SSurface::UnWeightControlPoints() {
    int i, j;
    for(i = 0; i <= degm; i++) {
        for(j = 0; j <= degn; j++) {
            ctrl[i][j] = (ctrl[i][j]).ScaledBy(1.0/weight[i][j]);
        }
    }
}
void SSurface::CopyRowOrCol(bool row, int this_ij, SSurface *src, int src_ij) {
    if(row) {
        int j;
        for(j = 0; j <= degn; j++) {
            ctrl  [this_ij][j] = src->ctrl  [src_ij][j];
            weight[this_ij][j] = src->weight[src_ij][j];
        }
    } else {
        int i;
        for(i = 0; i <= degm; i++) {
            ctrl  [i][this_ij] = src->ctrl  [i][src_ij];
            weight[i][this_ij] = src->weight[i][src_ij];
        }
    }
}
void SSurface::BlendRowOrCol(bool row, int this_ij, SSurface *a, int a_ij,
                                                    SSurface *b, int b_ij)
{
    if(row) {
        int j;
        for(j = 0; j <= degn; j++) {
            Vector c = (a->ctrl  [a_ij][j]).Plus(b->ctrl  [b_ij][j]);
            double w = (a->weight[a_ij][j]   +   b->weight[b_ij][j]);
            ctrl  [this_ij][j] = c.ScaledBy(0.5);
            weight[this_ij][j] = w / 2;
        }
    } else {
        int i;
        for(i = 0; i <= degm; i++) {
            Vector c = (a->ctrl  [i][a_ij]).Plus(b->ctrl  [i][b_ij]);
            double w = (a->weight[i][a_ij]   +   b->weight[i][b_ij]);
            ctrl  [i][this_ij] = c.ScaledBy(0.5);
            weight[i][this_ij] = w / 2;
        }
    }
}
void SSurface::SplitInHalf(bool byU, SSurface *sa, SSurface *sb) {
    sa->degm = sb->degm = degm;
    sa->degn = sb->degn = degn;

    // by de Casteljau's algorithm in a projective space; so we must work
    // on points (w*x, w*y, w*z, w) so create a temporary copy
    SSurface st;
    st = *this;
    st.WeightControlPoints();

    switch(byU ? degm : degn) {
        case 1:
            sa->CopyRowOrCol (byU, 0, &st, 0);
            sb->CopyRowOrCol (byU, 1, &st, 1);

            sa->BlendRowOrCol(byU, 1, &st, 0, &st, 1);
            sb->BlendRowOrCol(byU, 0, &st, 0, &st, 1);
            break;

        case 2:
            sa->CopyRowOrCol (byU, 0, &st, 0);
            sb->CopyRowOrCol (byU, 2, &st, 2);

            sa->BlendRowOrCol(byU, 1, &st, 0, &st, 1);
            sb->BlendRowOrCol(byU, 1, &st, 1, &st, 2);

            sa->BlendRowOrCol(byU, 2, sa,   1, sb,   1);
            sb->BlendRowOrCol(byU, 0, sa,   1, sb,   1);
            break;

        case 3: {
            sa->CopyRowOrCol (byU, 0, &st, 0);
            sb->CopyRowOrCol (byU, 3, &st, 3);

            sa->BlendRowOrCol(byU, 1, &st, 0, &st, 1);
            sb->BlendRowOrCol(byU, 2, &st, 2, &st, 3);
            st. BlendRowOrCol(byU, 0, &st, 1, &st, 2); // use row/col 0 as scratch

            sa->BlendRowOrCol(byU, 2, sa,   1, &st,  0);
            sb->BlendRowOrCol(byU, 1, sb,   2, &st,  0);

            sa->BlendRowOrCol(byU, 3, sa,   2, sb,   1);
            sb->BlendRowOrCol(byU, 0, sa,   2, sb,   1);
            break;
        }

        default: ssassert(false, "Unexpected degree of spline");
    }

    sa->UnWeightControlPoints();
    sb->UnWeightControlPoints();
}

//-----------------------------------------------------------------------------
// Find all points where the indicated finite (if segment) or infinite (if not
// segment) line intersects our surface. Report them in uv space in the list.
// We first do a bounding box check; if the line doesn't intersect, then we're
// done. If it does, then we check how small our surface is. If it's big,
// then we subdivide into quarters and recurse. If it's small, then we refine
// by Newton's method and record the point.
//-----------------------------------------------------------------------------
void SSurface::AllPointsIntersectingUntrimmed(Vector a, Vector b,
                                              int *cnt, int *level,
                                              List<Inter> *l, bool asSegment,
                                              SSurface *sorig)
{
    // Test if the line intersects our axis-aligned bounding box; if no, then
    // no possibility of an intersection
    if(LineEntirelyOutsideBbox(a, b, asSegment)) return;

    if(*cnt > 2000) {
        dbp("!!! too many subdivisions (level=%d)!", *level);
        dbp("degm = %d degn = %d", degm, degn);
        return;
    }
    (*cnt)++;

    // If we might intersect, and the surface is small, then switch to Newton
    // iterations.
    if(DepartureFromCoplanar() < 0.2*SS.ChordTolMm()) {
        Vector p = (ctrl[0   ][0   ]).Plus(
                    ctrl[0   ][degn]).Plus(
                    ctrl[degm][0   ]).Plus(
                    ctrl[degm][degn]).ScaledBy(0.25);
        Inter inter;
        sorig->ClosestPointTo(p, &(inter.p.x), &(inter.p.y), /*mustConverge=*/false);
        if(sorig->PointIntersectingLine(a, b, &(inter.p.x), &(inter.p.y))) {
            Vector p = sorig->PointAt(inter.p.x, inter.p.y);
            // Debug check, verify that the point lies in both surfaces
            // (which it ought to, since the surfaces should be coincident)
            double u, v;
            ClosestPointTo(p, &u, &v);
            l->Add(&inter);
        } else {
            // Might not converge if line is almost tangent to surface...
        }
        return;
    }

    // But the surface is big, so split it, alternating by u and v
    SSurface surf0, surf1;
    SplitInHalf((*level & 1) == 0, &surf0, &surf1);

    int nextLevel = (*level) + 1;
    (*level) = nextLevel;
    surf0.AllPointsIntersectingUntrimmed(a, b, cnt, level, l, asSegment, sorig);
    (*level) = nextLevel;
    surf1.AllPointsIntersectingUntrimmed(a, b, cnt, level, l, asSegment, sorig);
}

//-----------------------------------------------------------------------------
// Find all points where a line through a and b intersects our surface, and
// add them to the list. If seg is true then report only intersections that
// lie within the finite line segment (not including the endpoints); otherwise
// we work along the infinite line. And we report either just intersections
// inside the trim curve, or any intersection with u, v in [0, 1]. And we
// either disregard or report tangent points.
//-----------------------------------------------------------------------------
void SSurface::AllPointsIntersecting(Vector a, Vector b,
                                     List<SInter> *l,
                                     bool asSegment, bool trimmed, bool inclTangent)
{
    if(LineEntirelyOutsideBbox(a, b, asSegment)) return;

    Vector ba = b.Minus(a);
    double bam = ba.Magnitude();

    List<Inter> inters = {};

    // All the intersections between the line and the surface; either special
    // cases that we can quickly solve in closed form, or general numerical.
    Vector center, axis, start, finish;
    double radius;
    if(degm == 1 && degn == 1) {
        // Against a plane, easy.
        Vector n = NormalAt(0, 0).WithMagnitude(1);
        double d = n.Dot(PointAt(0, 0));
        // Trim to line segment now if requested, don't generate points that
        // would just get discarded later.
        if(!asSegment ||
           (n.Dot(a) > d + LENGTH_EPS && n.Dot(b) < d - LENGTH_EPS) ||
           (n.Dot(b) > d + LENGTH_EPS && n.Dot(a) < d - LENGTH_EPS))
        {
            Vector p = Vector::AtIntersectionOfPlaneAndLine(n, d, a, b, NULL);
            Inter inter;
            ClosestPointTo(p, &(inter.p.x), &(inter.p.y));
            inters.Add(&inter);
        }
    } else if(IsCylinder(&axis, &center, &radius, &start, &finish)) {
        // This one can be solved in closed form too.
        Vector ab = b.Minus(a);
        if(axis.Cross(ab).Magnitude() < LENGTH_EPS) {
            // edge is parallel to axis of cylinder, no intersection points
            return;
        }
        // A coordinate system centered at the center of the circle, with
        // the edge under test horizontal
        Vector u, v, n = axis.WithMagnitude(1);
        u = (ab.Minus(n.ScaledBy(ab.Dot(n)))).WithMagnitude(1);
        v = n.Cross(u);
        Point2d ap = (a.Minus(center)).DotInToCsys(u, v, n).ProjectXy(),
                bp = (b.Minus(center)).DotInToCsys(u, v, n).ProjectXy(),
                sp = (start. Minus(center)).DotInToCsys(u, v, n).ProjectXy(),
                fp = (finish.Minus(center)).DotInToCsys(u, v, n).ProjectXy();

        double thetas = atan2(sp.y, sp.x), thetaf = atan2(fp.y, fp.x);

        Point2d ip[2];
        int ip_n = 0;
        if(fabs(fabs(ap.y) - radius) < LENGTH_EPS) {
            // tangent
            if(inclTangent) {
                ip[0] = Point2d::From(0, ap.y);
                ip_n = 1;
            }
        } else if(fabs(ap.y) < radius) {
            // two intersections
            double xint = sqrt(radius*radius - ap.y*ap.y);
            ip[0] = Point2d::From(-xint, ap.y);
            ip[1] = Point2d::From( xint, ap.y);
            ip_n = 2;
        }
        int i;
        for(i = 0; i < ip_n; i++) {
            double t = (ip[i].Minus(ap)).DivProjected(bp.Minus(ap));
            // This is a point on the circle; but is it on the arc?
            Point2d pp = ap.Plus((bp.Minus(ap)).ScaledBy(t));
            double theta = atan2(pp.y, pp.x);
            double dp = WRAP_SYMMETRIC(theta  - thetas, 2*PI),
                   df = WRAP_SYMMETRIC(thetaf - thetas, 2*PI);
            double tol = LENGTH_EPS/radius;

            if((df > 0 && ((dp < -tol) || (dp > df + tol))) ||
               (df < 0 && ((dp >  tol) || (dp < df - tol))))
            {
                continue;
            }

            Vector p = a.Plus((b.Minus(a)).ScaledBy(t));

            Inter inter;
            ClosestPointTo(p, &(inter.p.x), &(inter.p.y));
            inters.Add(&inter);
        }
    } else {
        // General numerical solution by subdivision, fallback
        int cnt = 0, level = 0;
        AllPointsIntersectingUntrimmed(a, b, &cnt, &level, &inters, asSegment, this);
    }

    // Remove duplicate intersection points
    inters.ClearTags();
    int i, j;
    for(i = 0; i < inters.n; i++) {
        for(j = i + 1; j < inters.n; j++) {
            if(inters[i].p.Equals(inters[j].p)) {
                inters[j].tag = 1;
            }
        }
    }
    inters.RemoveTagged();

    for(i = 0; i < inters.n; i++) {
        Point2d puv = inters[i].p;

        // Make sure the point lies within the finite line segment
        Vector pxyz = PointAt(puv.x, puv.y);
        double t = (pxyz.Minus(a)).DivProjected(ba);
        if(asSegment && (t > 1 - LENGTH_EPS/bam || t < LENGTH_EPS/bam)) {
            continue;
        }

        // And that it lies inside our trim region
        Point2d dummy = { 0, 0 };
        SBspUv::Class c = (bsp) ? bsp->ClassifyPoint(puv, dummy, this) : SBspUv::Class::OUTSIDE;
        if(trimmed && c == SBspUv::Class::OUTSIDE) {
            continue;
        }

        // It does, so generate the intersection
        SInter si;
        si.p = pxyz;
        si.surfNormal = NormalAt(puv.x, puv.y);
        si.pinter = puv;
        si.srf = this;
        si.onEdge = (c != SBspUv::Class::INSIDE);
        l->Add(&si);
    }

    inters.Clear();
}

void SShell::AllPointsIntersecting(Vector a, Vector b,
                                   List<SInter> *il,
                                   bool asSegment, bool trimmed, bool inclTangent)
{
    for(SSurface &ss : surface) {
        ss.AllPointsIntersecting(a, b, il,
            asSegment, trimmed, inclTangent);
    }
}



SShell::Class SShell::ClassifyRegion(Vector edge_n, Vector inter_surf_n,
                           Vector edge_surf_n) const
{
    double dot = inter_surf_n.DirectionCosineWith(edge_n);
    if(fabs(dot) < DOTP_TOL) {
        // The edge's surface and the edge-on-face surface
        // are coincident. Test the edge's surface normal
        // to see if it's with same or opposite normals.
        if(inter_surf_n.Dot(edge_surf_n) > 0) {
            return Class::COINC_SAME;
        } else {
            return Class::COINC_OPP;
        }
    } else if(dot > 0) {
        return Class::OUTSIDE;
    } else {
        return Class::INSIDE;
    }
}

//-----------------------------------------------------------------------------
// Does the given point lie on our shell? There are many cases; inside and
// outside are obvious, but then there's all the edge-on-edge and edge-on-face
// possibilities.
//
// To calculate, we intersect a ray through p with our shell, and classify
// using the closest intersection point. If the ray hits a surface on edge,
// then just reattempt in a different random direction.
//-----------------------------------------------------------------------------

// table of vectors in 6 arbitrary directions covering 4 of the 8 octants.
// use overlapping sets of 3 to reduce memory usage.
static const double Random[8] = {1.278, 5.0103, 9.427, -2.331, 7.13, 2.954, 5.034, -4.777};
 
bool SShell::ClassifyEdge(Class *indir, Class *outdir,
                          Vector ea, Vector eb,
                          Vector p,
                          Vector edge_n_in, Vector edge_n_out, Vector surf_n)
{
    List<SInter> l = {};

    // First, check for edge-on-edge
    int edge_inters = 0;
    Vector inter_surf_n[2], inter_edge_n[2];
    for(SSurface &srf : surface) {
        if(srf.LineEntirelyOutsideBbox(ea, eb, /*asSegment=*/true)) continue;

        SEdgeList *sel = &(srf.edges);
        SEdge *se;
        for(se = sel->l.First(); se; se = sel->l.NextAfter(se)) {
            if((ea.Equals(se->a) && eb.Equals(se->b)) ||
               (eb.Equals(se->a) && ea.Equals(se->b)) ||
                p.OnLineSegment(se->a, se->b))
            {
                if(edge_inters < 2) {
                    // Edge-on-edge case
                    Point2d pm;
                    srf.ClosestPointTo(p,  &pm, /*mustConverge=*/false);
                    // A vector normal to the surface, at the intersection point
                    inter_surf_n[edge_inters] = srf.NormalAt(pm);
                    // A vector normal to the intersecting edge (but within the
                    // intersecting surface) at the intersection point, pointing
                    // out.
                    inter_edge_n[edge_inters] =
                      (inter_surf_n[edge_inters]).Cross((se->b).Minus((se->a)));
                }

                edge_inters++;
            }
        }
    }

    if(edge_inters == 2) {
        //! @todo make this use the appropriate curved normals
        double dotp[2];
        for(int i = 0; i < 2; i++) {
            dotp[i] = edge_n_out.DirectionCosineWith(inter_surf_n[i]);
        }

        if(fabs(dotp[1]) < DOTP_TOL) {
            swap(dotp[0],         dotp[1]);
            swap(inter_surf_n[0], inter_surf_n[1]);
            swap(inter_edge_n[0], inter_edge_n[1]);
        }

        Class coinc = (surf_n.Dot(inter_surf_n[0])) > 0 ? Class::COINC_SAME : Class::COINC_OPP;

        if(fabs(dotp[0]) < DOTP_TOL && fabs(dotp[1]) < DOTP_TOL) {
            // This is actually an edge on face case, just that the face
            // is split into two pieces joining at our edge.
            *indir  = coinc;
            *outdir = coinc;
        } else if(fabs(dotp[0]) < DOTP_TOL && dotp[1] > DOTP_TOL) {
            if(edge_n_out.Dot(inter_edge_n[0]) > 0) {
                *indir  = coinc;
                *outdir = Class::OUTSIDE;
            } else {
                *indir  = Class::INSIDE;
                *outdir = coinc;
            }
        } else if(fabs(dotp[0]) < DOTP_TOL && dotp[1] < -DOTP_TOL) {
            if(edge_n_out.Dot(inter_edge_n[0]) > 0) {
                *indir  = coinc;
                *outdir = Class::INSIDE;
            } else {
                *indir  = Class::OUTSIDE;
                *outdir = coinc;
            }
        } else if(dotp[0] > DOTP_TOL && dotp[1] > DOTP_TOL) {
            *indir  = Class::INSIDE;
            *outdir = Class::OUTSIDE;
        } else if(dotp[0] < -DOTP_TOL && dotp[1] < -DOTP_TOL) {
            *indir  = Class::OUTSIDE;
            *outdir = Class::INSIDE;
        } else {
            // Edge is tangent to the shell at shell's edge, so can't be
            // a boundary of the surface.
            return false;
        }
        return true;
    }

    if(edge_inters != 0) dbp("bad, edge_inters=%d", edge_inters);

    // Next, check for edge-on-surface. The ray-casting for edge-inside-shell
    // would catch this too, but test separately, for speed (since many edges
    // are on surface) and for numerical stability, so we don't pick up
    // the additional error from the line intersection.

    for(SSurface &srf : surface) {
        if(srf.LineEntirelyOutsideBbox(ea, eb, /*asSegment=*/true)) continue;

        Point2d puv;
        srf.ClosestPointTo(p, &(puv.x), &(puv.y), /*mustConverge=*/false);
        Vector pp = srf.PointAt(puv);

        if((pp.Minus(p)).Magnitude() > LENGTH_EPS) continue;
        Point2d dummy = { 0, 0 };
        SBspUv::Class c = (srf.bsp) ? srf.bsp->ClassifyPoint(puv, dummy, &srf) : SBspUv::Class::OUTSIDE;
        if(c == SBspUv::Class::OUTSIDE) continue;

        // Edge-on-face (unless edge-on-edge above superceded)
        Point2d pin, pout;
        srf.ClosestPointTo(p.Plus(edge_n_in),  &pin,  /*mustConverge=*/false);
        srf.ClosestPointTo(p.Plus(edge_n_out), &pout, /*mustConverge=*/false);

        Vector surf_n_in  = srf.NormalAt(pin),
               surf_n_out = srf.NormalAt(pout);

        *indir  = ClassifyRegion(edge_n_in,  surf_n_in,  surf_n);
        *outdir = ClassifyRegion(edge_n_out, surf_n_out, surf_n);
        return true;
    }

    // Edge is not on face or on edge; so it's either inside or outside
    // the shell, and we'll determine which by raycasting.
    int cnt = 0;
    for(;;) {
        // Cast a ray in a random direction (two-sided so that we test if
        // the point lies on a surface, but use only one side for in/out
        // testing)
        Vector ray = Vector::From(Random[cnt], Random[cnt+1], Random[cnt+2]);

        AllPointsIntersecting(
            p.Minus(ray), p.Plus(ray), &l,
                /*asSegment=*/false, /*trimmed=*/true, /*inclTangent=*/false);

        // no intersections means it's outside
        *indir  = Class::OUTSIDE;
        *outdir = Class::OUTSIDE;
        double dmin = VERY_POSITIVE;
        bool onEdge = false;
        edge_inters = 0;

        SInter *si;
        for(si = l.First(); si; si = l.NextAfter(si)) {
            double t = ((si->p).Minus(p)).DivProjected(ray);
            if(t*ray.Magnitude() < -LENGTH_EPS) {
                // wrong side, doesn't count
                continue;
            }

            double d = ((si->p).Minus(p)).Magnitude();

            // We actually should never hit this case; it should have been
            // handled above.
            if(d < LENGTH_EPS && si->onEdge) {
                edge_inters++;
            }

            if(d < dmin) {
                dmin = d;
                // Edge does not lie on surface; either strictly inside
                // or strictly outside
                if((si->surfNormal).Dot(ray) > 0) {
                    *indir  = Class::INSIDE;
                    *outdir = Class::INSIDE;
                } else {
                    *indir  = Class::OUTSIDE;
                    *outdir = Class::OUTSIDE;
                }
                onEdge = si->onEdge;
            }
        }
        l.Clear();

        // If the point being tested lies exactly on an edge of the shell,
        // then our ray always lies on edge, and that's okay. Otherwise
        // try again in a different random direction.
        if(!onEdge) break;
        cnt++;
        if(cnt > 5) {
            dbp("can't find a ray that doesn't hit on edge!");
            dbp("on edge = %d, edge_inters = %d", onEdge, edge_inters);
            SS.nakedEdges.AddEdge(ea, eb);
            break;
        }
    }

    return true;
}