Examples with Plane maspack.matrix.Plane used on opensource projects

Example 16 with Plane

use of maspack.matrix.Plane in project artisynth_core by artisynth.

the class DistanceGridSurfCalc method findSurfaceTangent.

int findSurfaceTangent(Point3d pt, TanSearchInfo sinfo, TetPlaneIntersection isect, Point3d pold, int sgnDotGrad, Point3d pa, Plane plane) {
    double[] c = new double[10];
    TetDesc tdesc = isect.myTetDesc;
    myGrid.computeQuadCoefs(c, tdesc);
    Point3d paCell = new Point3d();
    transformToQuadCell(paCell, pa, tdesc);
    if (myDebug > 0) {
        System.out.println("  checking for tangent in " + tdesc);
        System.out.println("  pa=" + pa);
    }
    Plane planeCell = new Plane();
    transformToQuadCell(planeCell, plane, tdesc);
    double[] b = new double[6];
    Vector3d r = new Vector3d();
    PlaneType planeType = computeBCoefs(b, r, c, planeCell);
    if (pold != null) {
        Point3d pc = new Point3d();
        Vector3d grad = new Vector3d();
        Vector3d dela0 = new Vector3d();
        transformToQuadCell(pc, pold, tdesc);
        // compute gradient and project it into the plane
        myGrid.computeQuadGradient(grad, c, pc.x, pc.y, pc.z, myGrid.myQuadGridToLocal);
        grad.scaledAdd(-grad.dot(plane.normal), plane.normal);
        dela0.sub(pold, pa);
        int newSgnDotGrad = (dela0.dot(grad) > 0 ? 1 : -1);
        if (newSgnDotGrad * sgnDotGrad < 0) {
            // sign change, so accept pold
            pt.set(pold);
            if (myDebug > 0) {
                System.out.println("    grad direction change; using entry point");
            }
            return DONE;
        }
    }
    if (findSurfaceTangentCell(pt, tdesc, b, r, planeType, paCell)) {
        transformFromQuadCell(pt, pt, tdesc);
        return DONE;
    } else {
        // find the intersection points, if any, of the curve with each
        // of the boundary edges
        Point3d[] ip = new Point3d[] { isect.myP0, isect.myP1, isect.myP2, isect.myP3 };
        Point3d pl0 = ip[0];
        Point3d pl1 = null;
        Point3d pc0 = new Point3d();
        Point3d pc1 = new Point3d();
        transformToQuadCell(pc0, pl0, tdesc);
        double EPS = 1e-12;
        double[] svals = new double[2];
        // index of edge containing the best next point
        int bestEdgeIdx = -1;
        double bestDistToA = -1;
        double bestS = -1;
        if (myDebug > 0) {
            System.out.println("    isect=" + isect.myTetDesc + " numSides=" + isect.myNumSides);
        }
        for (int i = 0; i < isect.myNumSides; i++) {
            pl1 = i < isect.myNumSides - 1 ? ip[i + 1] : ip[0];
            transformToQuadCell(pc1, pl1, tdesc);
            int nr = 0;
            switch(planeType) {
                case YZ:
                    {
                        nr = intersectSurfaceAndRay(svals, b, pc0.y, pc1.y - pc0.y, pc0.z, pc1.z - pc0.z, 0.0, 1.0);
                        break;
                    }
                case ZX:
                    {
                        nr = intersectSurfaceAndRay(svals, b, pc0.z, pc1.z - pc0.z, pc0.x, pc1.x - pc0.x, 0.0, 1.0);
                        break;
                    }
                case XY:
                    {
                        nr = intersectSurfaceAndRay(svals, b, pc0.x, pc1.x - pc0.x, pc0.y, pc1.y - pc0.y, 0.0, 1.0);
                        break;
                    }
            }
            if (myDebug > 0) {
                System.out.println("    nr=" + nr + " pc0=" + pc0.toString("%10.5f") + " pc1=" + pc1.toString("%10.5f"));
            }
            if (nr > 0) {
                Point3d pi = new Point3d();
                Vector3d grad = new Vector3d();
                Vector3d dela0 = new Vector3d();
                for (int j = 0; j < nr; j++) {
                    // compute gradient at the intersection point, and
                    // project it into the plane
                    pi.combine(1 - svals[j], pc0, svals[j], pc1);
                    myGrid.computeQuadGradient(grad, c, pi.x, pi.y, pi.z, myGrid.myQuadGridToLocal);
                    grad.scaledAdd(-grad.dot(plane.normal), plane.normal);
                    pi.combine(1 - svals[j], pl0, svals[j], pl1);
                    dela0.sub(pi, pa);
                    if (pold != null && pi.distance(pold) <= myGrid.getRadius() * EPS) {
                        if (myDebug > 0) {
                            System.out.println("      same as initial point");
                        }
                        // same as initial point, ignore
                        continue;
                    }
                    if (dela0.dot(grad) * sgnDotGrad < 0) {
                        // be part of the same curve section, ignore
                        if (myDebug > 0) {
                            System.out.println("      tangent dir switched sides");
                        }
                        continue;
                    }
                    double distToA = pa.distance(pi);
                    // otherwise, want to move away
                    if (bestEdgeIdx == -1 || (sgnDotGrad > 0 && distToA < bestDistToA) || (sgnDotGrad < 0 && distToA > bestDistToA)) {
                        bestEdgeIdx = i;
                        bestDistToA = distToA;
                        bestS = svals[j];
                        sinfo.lastGrad.set(grad);
                        if (myDebug > 0) {
                            System.out.println("      best edge, bestS=" + bestS);
                        }
                        pt.set(pi);
                    }
                }
            }
            pl0 = pl1;
            pc0.set(pc1);
        }
        if (bestEdgeIdx != -1) {
            // need to find the feature associated with this point
            // sinfo.lastFeat = isect.getFeature (bestS, bestEdgeIdx);
            sinfo.lastFeat = findNearestFeature(isect.myTetDesc, pt, 1e-10);
            return CONTINUE;
        } else {
            return NONE;
        }
    }
}

Example 17 with Plane

use of maspack.matrix.Plane in project artisynth_core by artisynth.

the class DistanceGridSurfCalc method findQuadSurfaceTangent.

public boolean findQuadSurfaceTangent(Point3d pt, Point3d p0, Point3d pa, Vector3d nrm) {
    // transform p0, pa and nrm into grid local coordinates
    Point3d p0Loc = new Point3d();
    myGrid.myLocalToWorld.inverseTransformPnt(p0Loc, p0);
    Vector3d nrmLoc = new Vector3d(nrm);
    myGrid.myLocalToWorld.inverseTransformCovec(nrmLoc, nrm);
    nrmLoc.normalize();
    // generate a plane from the normal and p0
    // NEED
    Plane planeLoc = new Plane(nrmLoc, p0Loc);
    // find the distance of p0 to the surface, along with the associated
    // gradient direction (grad).
    Vector3d grad = new Vector3d();
    double d = myGrid.getQuadDistanceAndGradient(grad, null, p0Loc);
    if (d == DistanceGrid.OUTSIDE_GRID) {
        // shouldn't happen - p0 should be inside by definition
        if (myDebug > 0) {
            System.out.println("Not found: p0 outside the grid");
        }
        return false;
    }
    // project grad into plane
    planeLoc.projectVector(grad, grad);
    grad.normalize();
    if (myDebug > 0) {
        System.out.println("d=" + d + " grad=" + grad);
    }
    Vector3d nrmCell = new Vector3d();
    myGrid.myGridToLocal.inverseTransformCovec(nrmCell, nrmLoc);
    nrmCell.normalize();
    ComputePlane cplane = new ComputePlane();
    switch(nrmCell.maxAbsIndex()) {
        case 0:
            cplane.type = PlaneType.YZ;
            break;
        case 1:
            cplane.type = PlaneType.ZX;
            break;
        case 2:
            cplane.type = PlaneType.XY;
            break;
    }
    cplane.sign = (nrmCell.get(nrmCell.maxAbsIndex()) > 0 ? 1 : -1);
    Point3d psLoc = new Point3d();
    TetDesc tdesc = findQuadSurfaceIntersectionLoc(psLoc, p0Loc, grad);
    if (tdesc == null) {
        // fallback - set pt to p0
        pt.set(p0);
        return false;
    }
    // need to obtain a plane intersection for tdesc
    TetPlaneIntersection isect = getTetPlaneIntersection(tdesc, psLoc, planeLoc, cplane);
    // tdesc might have changed
    if (isect == null) {
        if (myDebug > 0) {
            System.out.println("Not found: ps tet does not intersect plane");
        }
        return false;
    }
    // tdesc might have changed
    tdesc = isect.myTetDesc;
    // Get the surface gradient at the surface intersection point ps
    d = myGrid.getQuadDistanceAndGradient(grad, null, psLoc);
    if (d == DistanceGrid.OUTSIDE_GRID) {
        // shouldn't happen - ps should be inside by definition
        if (myDebug > 0) {
            System.out.println("Not found: ps is outside grid");
        }
        return false;
    }
    // project grad into plane
    planeLoc.projectVector(grad, grad);
    Vector3d dela0 = new Vector3d();
    Point3d paLoc = new Point3d();
    myGrid.myLocalToWorld.inverseTransformPnt(paLoc, pa);
    dela0.sub(psLoc, paLoc);
    // set sgnDotGrad to 1 or -1 depending on whether the gradient is
    // pointing away or towards pa along the line (pa, ps).
    int sgnDotGrad = (dela0.dot(grad) > 0 ? 1 : -1);
    if (myDebug > 0) {
        System.out.println("found ps=" + psLoc.toString("%g") + " tet=" + tdesc + " sgnDotGrad=" + sgnDotGrad);
        Vector3d del = new Vector3d();
        del.sub(psLoc, p0Loc);
        System.out.println("p0=" + p0Loc + " del=" + del);
        Point3d pg = new Point3d();
        myGrid.myQuadGridToLocal.inverseTransformPnt(pg, psLoc);
        System.out.println("ps in quad grid coords=" + pg.toString("%g"));
        System.out.println("pa=" + pa);
    }
    TanSearchInfo sinfo = new TanSearchInfo();
    int code = findSurfaceTangent(pt, sinfo, isect, null, sgnDotGrad, paLoc, planeLoc);
    if (code == DONE) {
        // tangent found
        myGrid.myLocalToWorld.transformPnt(pt, pt);
        if (myDebug > 0) {
            System.out.println("Found (initial ps): pt=" + pt.toString("%10.5f"));
        }
        return true;
    }
    if (code == NONE && myDebug > 0) {
        System.out.println("Not found: no continuation point");
    }
    myVisitedTets.clear();
    myVisitedTets.add(tdesc);
    while (code != NONE) {
        int ntets = getFeatureAdjacentTets(tdesc, sinfo.lastFeat, planeLoc, myVisitedTets);
        TetPlaneIntersection ibest = null;
        if (myDebug > 0) {
            System.out.println("checking " + ntets + " adjacent tets for feature " + sinfo.lastFeat);
            System.out.print("  ");
            for (int k = 0; k < ntets; k++) {
                System.out.print(getIsect(k).myTetDesc + " ");
            }
            System.out.println("");
        }
        if (ntets > 1) {
            // find the tet that best intersects the current curve tangent
            double maxLen = -1;
            int maxTet = -1;
            for (int k = 0; k < ntets; k++) {
                double len = getIsect(k).tanIntersectOverlap(pt, sinfo.lastGrad, cplane.type);
                if (len > maxLen) {
                    maxLen = len;
                    maxTet = k;
                }
            }
            ibest = getIsect(maxTet);
        } else if (ntets > 0) {
            ibest = getIsect(0);
        } else {
            if (myDebug > 0) {
                System.out.println("Not found: no adjacent tets intersect plane");
            }
            code = NONE;
            continue;
        }
        tdesc = ibest.myTetDesc;
        myVisitedTets.add(tdesc);
        code = findSurfaceTangent(pt, sinfo, ibest, pt, sgnDotGrad, paLoc, planeLoc);
        int tidx = 0;
        while (code == NONE && tidx < ntets) {
            isect = getIsect(tidx++);
            if (isect != ibest) {
                tdesc = isect.myTetDesc;
                myVisitedTets.add(tdesc);
                code = findSurfaceTangent(pt, sinfo, isect, pt, sgnDotGrad, paLoc, planeLoc);
            }
        }
        if (code == DONE) {
            myGrid.myLocalToWorld.transformPnt(pt, pt);
            if (myDebug > 0) {
                System.out.println("Found: pt=" + pt.toString("%10.5f"));
            }
            return true;
        } else if (code == NONE && myDebug > 0) {
            System.out.println("Not found: no continuation point in any adjacent tet");
        }
    }
    myGrid.myLocalToWorld.transformPnt(pt, psLoc);
    return false;
}

Example 18 with Plane

use of maspack.matrix.Plane in project artisynth_core by artisynth.

the class DistanceGridSurfCalc method findQuadSurfaceIntersection.

public boolean findQuadSurfaceIntersection(Point3d pi, Point3d p0, Point3d pa, Vector3d nrm) {
    // transform p0, pa and nrm into grid local coordinates
    Point3d p0Loc = new Point3d();
    myGrid.myLocalToWorld.inverseTransformPnt(p0Loc, p0);
    Point3d paLoc = new Point3d();
    myGrid.myLocalToWorld.inverseTransformPnt(paLoc, pa);
    Vector3d nrmLoc = new Vector3d(nrm);
    myGrid.myLocalToWorld.inverseTransformCovec(nrmLoc, nrm);
    nrmLoc.normalize();
    // generate a plane from the normal and p0
    Plane planeLoc = new Plane(nrmLoc, p0Loc);
    // project paLoc onto the plane
    planeLoc.project(paLoc, paLoc);
    Vector3d dirLoc = new Vector3d();
    dirLoc.sub(paLoc, p0Loc);
    if (dirLoc.normSquared() == 0) {
        return false;
    }
    dirLoc.normalize();
    Point3d psLoc = new Point3d();
    TetDesc tdesc = findQuadSurfaceIntersectionLoc(psLoc, p0Loc, dirLoc);
    if (tdesc == null) {
        return false;
    } else {
        myGrid.myLocalToWorld.transformPnt(pi, psLoc);
        return true;
    }
}

Example 19 with Plane

use of maspack.matrix.Plane in project artisynth_core by artisynth.

the class BVIntersectorTest method intersectAllFaces.

ArrayList<TriPlaneIntersection> intersectAllFaces(PolygonalMesh mesh, Plane plane) {
    TriangleIntersector intersector = new TriangleIntersector();
    if (!mesh.meshToWorldIsIdentity()) {
        plane = new Plane(plane);
        plane.inverseTransform(mesh.getMeshToWorld());
    }
    ArrayList<TriPlaneIntersection> intersections = new ArrayList<TriPlaneIntersection>();
    for (Face face : mesh.getFaces()) {
        HalfEdge he;
        he = face.firstHalfEdge();
        Point3d p0 = he.head.pnt;
        he = he.getNext();
        Point3d p1 = he.head.pnt;
        he = he.getNext();
        Point3d p2 = he.head.pnt;
        ArrayList<Point3d> points = intersector.intersectTrianglePlane(p0, p1, p2, plane);
        if (points != null && points.size() > 0) {
            // map intersection points to world coords
            for (int k = 0; k < points.size(); k++) {
                points.get(k).transform(mesh.getMeshToWorld());
            }
            intersections.add(new TriPlaneIntersection(face, plane, points));
        }
    }
    return intersections;
}