Examples with IntersectionPoint maspack.collision.IntersectionPoint used on opensource projects

Example 26 with IntersectionPoint

use of maspack.collision.IntersectionPoint in project artisynth_core by artisynth.

the class SurfaceMeshIntersector method markEmptySegBegin.

void markEmptySegBegin(IntersectionPoint mip, HalfEdge edge, boolean clockwise) {
    Face face = edge.getFace();
    PolygonalMesh mesh = (PolygonalMesh) face.getMesh();
    int meshNum = (mesh == myMesh0 ? 0 : 1);
    int mark = IntersectionPoint.EMPTY_BEGIN;
    EdgeInfo einfo = getEdgeInfo(edge.getPrimary());
    if (mip.edge.getHead().getMesh() != mesh) {
        boolean tailToHead = (edge == einfo.myEdge);
        if (!clockwise) {
            tailToHead = !tailToHead;
        }
        EdgeInfo.PointData xdata = null;
        double s = einfo.project(mip);
        int idx;
        if (tailToHead) {
            double si = 0;
            for (idx = 0; idx < einfo.numXips(); idx++) {
                double sx = einfo.getXipData(idx).s;
                if (sx - myPositionTol >= s) {
                    break;
                }
                si = sx;
            }
            if (s > si + myPositionTol) {
                mark |= IntersectionPoint.EMPTY_PROJECTED;
            }
        } else {
            double si = einfo.myUlen;
            for (idx = einfo.numXips() - 1; idx >= 0; idx--) {
                double sx = einfo.getXipData(idx).s;
                if (sx + myPositionTol <= s) {
                    break;
                }
                si = sx;
            }
            idx++;
            if (s < si - myPositionTol) {
                mark |= IntersectionPoint.EMPTY_PROJECTED;
            }
        }
        if (useEmptySegmentProjection) {
            einfo.addXip(idx, mip, s);
        }
        mip.setNearEdge(meshNum, edge);
    }
    if (useEmptySegmentProjection) {
        mip.setEmptyMark(meshNum, mark);
    }
}

Example 27 with IntersectionPoint

use of maspack.collision.IntersectionPoint in project artisynth_core by artisynth.

the class SurfaceMeshIntersector method sortCoincidentPoints.

void sortCoincidentPoints(IntersectionPoint p) {
    HalfEdge edge = p.edge;
    EdgeInfo einfo = myEdgeInfos.get(p.edge);
    // find the lohi indices of the coincident points
    int[] lohi = findCoincidentBounds(p, einfo);
    IntersectionPoint plo = p;
    // p.edge.faceStr().equals("34-37");
    boolean debug = false;
    if (debug) {
        System.out.println("Sort coincident " + edge.faceStr());
        System.out.println("lohi=" + lohi[0] + " " + lohi[1]);
    }
    int numc = lohi[1] - lohi[0] + 1;
    int lo = lohi[0];
    IntersectionPoint[] points = new IntersectionPoint[numc];
    if (numc == 2) {
        points[0] = einfo.getMip(lo);
        points[1] = einfo.getMip(lo + 1);
    } else {
        int cnt = 1;
        for (IntersectionPoint prev = p.prev(); prev != null; prev = prev.prev()) {
            if (prev.primaryCoincident == p.primaryCoincident) {
                plo = prev;
                cnt++;
            }
            // equal wouldn't work here, as coincident points not always equal:
            if (prev.distance(p) > myPositionTol) {
                break;
            }
        }
        if (debug) {
            System.out.println("plo=" + plo.contourIndex);
        }
        IntersectionPoint phi = p;
        for (IntersectionPoint next = p.next(); next != null; next = next.next()) {
            if (next.primaryCoincident == p.primaryCoincident) {
                phi = next;
                cnt++;
            }
            // equal wouldn't work here, as coincident points not always equal:
            if (next.distance(p) > myPositionTol) {
                break;
            }
        }
        if (debug) {
            System.out.println("phi=" + phi.contourIndex);
        }
        if (cnt != numc) {
            System.out.println("ERROR: cnt=" + cnt + " lohi=[" + lohi[0] + "," + lohi[1] + "]");
            if (false) {
                System.out.println("problem written to contactTestFail.out");
                try {
                    SurfaceMeshIntersectorTest.writeProblem("contactTestFail.out", myMesh0, myMesh1, null);
                } catch (Exception e) {
                    e.printStackTrace();
                }
            }
        }
        if (debug || cnt > 2) {
            System.out.println("HIGH CNT=" + cnt);
        }
        int k = 0;
        IntersectionPoint primary = p.primaryCoincident;
        for (IntersectionPoint q = plo; k < cnt; q = q.next()) {
            if (q.primaryCoincident == primary) {
                // q.primaryCoincident = IntersectionPoint.COINCIDENT;
                points[k++] = q;
            }
        }
    }
    int direction = 0;
    for (int i = 0; i < numc - 1; i++) {
        int res = commonEdgeConvexityTest(points[i], points[i + 1], debug);
        if (res != -1) {
            direction = determineEdgeDirection(points[i], points[i + 1], res == 1, i);
            if (debug)
                System.out.println("commonEdge res=" + res + " mips " + points[i].contourIndex + " " + points[i + 1].contourIndex);
            break;
        }
    }
    if (direction == 0) {
        for (int i = 0; i < numc - 1; i++) {
            int res = commonVertexConvexityTest(points[i], points[i + 1], debug);
            if (res != -1) {
                direction = determineEdgeDirection(points[i], points[i + 1], res == 1, i);
                if (debug)
                    System.out.println("commonVertex res=" + res + " mips " + points[i].contourIndex + " " + points[i + 1].contourIndex);
                break;
            }
        }
    }
    if (direction == 0) {
        int res = distanceConvexityTest(points[0], points[1], debug);
        if (debug)
            System.out.println("distanceConvexity res=" + res + " mips " + points[0].contourIndex + " " + points[1].contourIndex);
        direction = determineEdgeDirection(points[0], points[1], res == 1, 0);
    }
    for (int k = 0; k < numc; k++) {
        IntersectionPoint q;
        if (direction == 1) {
            q = points[numc - 1 - k];
        } else {
            q = points[k];
        }
        einfo.setMip(lo + k, q);
        q.primaryCoincident = IntersectionPoint.COINCIDENT;
    }
}

Example 28 with IntersectionPoint

use of maspack.collision.IntersectionPoint in project artisynth_core by artisynth.

the class SurfaceMeshIntersector method createTrianglePoly.

Vertex3dList createTrianglePoly(Face face, PenetrationRegion region, HashMap<Vertex3d, Vertex3d> vertexMap, boolean clockwise, boolean debug) {
    Vertex3dList poly = new Vertex3dList(/*closed=*/
    true);
    Vertex3d curVtx = null;
    Vertex3d firstVtx = null;
    HalfEdge edge0 = face.firstHalfEdge();
    HalfEdge edge = edge0;
    EdgeInfo einfo = myEdgeInfos.get(edge.getPrimary());
    PolygonalMesh mesh = (PolygonalMesh) face.getMesh();
    int meshNum = (mesh == myMesh0 ? 0 : 1);
    do {
        if (einfo != null) {
            for (int mi = 0; mi < einfo.numXips(); mi++) {
                IntersectionPoint p = einfo.getXip(mi, edge, clockwise);
                int emptyMark;
                if ((emptyMark = p.getEmptyMark(meshNum)) != 0) {
                    if (p.findSegmentFace(mesh) != face && (emptyMark & IntersectionPoint.EMPTY_PROJECTED) != 0) {
                        if (p.myVertex != curVtx) {
                            curVtx = p.myVertex;
                            poly.add(curVtx);
                            if (debug) {
                                System.out.println(" ADD P(b) " + p.contourIndex + " " + curVtx.pnt.toString("%20.16f"));
                            }
                            if (firstVtx == null) {
                                firstVtx = curVtx;
                            }
                        }
                    }
                }
            }
        }
        if (!useEmptySegmentProjection) {
            if (edge.opposite != null && !region.myFaces.contains(edge.opposite.getFace())) {
                curVtx = addEmptyFaceVertices(poly, edge, null, null, region, curVtx, clockwise, debug);
            }
        }
        Vertex3d newVtx = getNewVertex(edge, vertexMap, clockwise);
        if (newVtx != curVtx && newVtx != firstVtx) {
            curVtx = newVtx;
            if (debug) {
                Vertex3d oldVtx = (clockwise ? edge.getTail() : edge.getHead());
                System.out.println(" ADD vtx(c) " + oldVtx.getIndex() + " " + curVtx.pnt.toString("%20.16f"));
            }
            poly.add(curVtx);
        }
        edge = getNextEdge(edge, clockwise);
        einfo = myEdgeInfos.get(edge.getPrimary());
    } while (edge != edge0);
    if (clockwise) {
        poly.reverse();
    }
    return poly;
}

Example 29 with IntersectionPoint

use of maspack.collision.IntersectionPoint in project artisynth_core by artisynth.

the class SurfaceMeshIntersector method backupToNonCoincident.

/**
 * Starting with an intersection point mip, back up until we find a point
 * whose preceeding point is *not* coincident, or is null
 */
IntersectionPoint backupToNonCoincident(IntersectionPoint mip) {
    IntersectionPoint mip0 = mip;
    IntersectionPoint prev = mip.prev();
    if (prev != null && prev.distance(mip0) <= myPositionTol) {
        do {
            mip = prev;
            prev = mip.prev();
            if (prev == null || prev.distance(mip0) > myPositionTol) {
                return mip;
            }
        } while (mip != mip0);
    }
    return mip0;
}

Example 30 with IntersectionPoint

use of maspack.collision.IntersectionPoint in project artisynth_core by artisynth.

the class SurfaceMeshIntersector method getContainedRegions.

/**
 * Given the first <code>numCheck</code> regions of a list
 * <code>nested</code> of single-face nested regions belonging to
 * <code>face</code>, find and return those which are contained within
 * <code>region</code>.
 */
ArrayList<PenetrationRegion> getContainedRegions(PenetrationRegion region, Face face, ArrayList<PenetrationRegion> nested, int numCheck, boolean clockwise) {
    double dtol = EPS * face.computeCircumference();
    // if (face.getIndex()==2) {
    // System.out.println ("checking " + getName(region));
    // }
    // The method works as follows: For each of the nested regions being
    // checked, we select a test point that is inside the region.  We then
    // use NearestPolygon3dFeature to find the nearest feature to the test
    // point among all of the polygonal segments formed from the contours of
    // <code>region</code> that cross the face. This nearest feature can then
    // be queried to see if the test point is inside or outside, given the
    // contour orientation specified by <code>clockwiseContour</code>.
    // Create and initialize nearest feature objects for each region of
    // <code>nested</code> being checked.
    NearestPolygon3dFeature[] nearestFeats = new NearestPolygon3dFeature[numCheck];
    for (int i = 0; i < numCheck; i++) {
        PenetrationRegion r = nested.get(i);
        NearestPolygon3dFeature nfeat = new NearestPolygon3dFeature();
        nearestFeats[i] = nfeat;
        Vector3d nrm = face.getWorldNormal();
        Point3d testp = createTestPoint(r.getFirstContour(), nrm, clockwise);
        nfeat.init(testp, nrm, dtol);
    }
    PolygonalMesh mesh = (PolygonalMesh) face.getMesh();
    // of <code>region</code>:
    for (IntersectionContour c : region.myContours) {
        int csize = c.size();
        // Try to find the first point where the contour enters this face and
        // store its index in kenter:
        int kenter = c.getFirstFaceEntryIndex(mesh, face);
        if (kenter == -1) {
            // entire contour.
            if (!c.isClosed()) {
                // getWrapped(k) will work OK.
                throw new InternalErrorException("Contour entirely within face but contour is not closed");
            }
            if (c.findSegmentFace(0, mesh) == face) {
                for (int k = 0; k <= csize; k++) {
                    for (int i = 0; i < numCheck; i++) {
                        if (k == 0) {
                            nearestFeats[i].restart();
                            nearestFeats[i].advance(c.get(0));
                        } else if (k < csize) {
                            nearestFeats[i].advance(c.getWrapped(k));
                        } else {
                            nearestFeats[i].close();
                        }
                    }
                }
            }
        } else {
            // Starting at kenter, check the test point against those polygonal
            // segments that occur when the contour crosses face:
            int k = kenter;
            Face lastFace = null;
            for (int j = 0; j < csize; j++) {
                for (int i = 0; i < numCheck; i++) {
                    if (j == 0) {
                        // contour c entering face for the first time
                        nearestFeats[i].restart();
                        nearestFeats[i].advance(c.get(k));
                        lastFace = face;
                    } else {
                        Face segFace = c.findSegmentFace(k, mesh);
                        if (lastFace == face) {
                            // continuing on the face
                            nearestFeats[i].advance(c.get(k));
                        } else if (segFace == face) {
                            // reentering the face
                            nearestFeats[i].restart();
                            nearestFeats[i].advance(c.get(k));
                        } else {
                        // not on the face
                        }
                        lastFace = segFace;
                    }
                }
                if (++k == csize) {
                    if (!c.isClosed()) {
                        // end of contour, so we are done
                        break;
                    }
                    k = 0;
                }
            }
        }
    }
    ArrayList<PenetrationRegion> contained = new ArrayList<PenetrationRegion>();
    for (int i = 0; i < numCheck; i++) {
        NearestPolygon3dFeature nfeat = nearestFeats[i];
        boolean isContained;
        if (nfeat.numVertices() == 1) {
            throw new InternalErrorException("region's contours intersect face " + face.getIndex() + " at only one point.");
        // Former code from when we though it might be possible for a face
        // to interect a mesh at only one point: Then try to use the local
        // intersection geometry around this point (returned by
        // nfeat.getVertex(0)) to determine if the test point is inside or
        // outside the penetration region.
        // isContained = !isPointOutside (
        // nfeat.getPoint(), face,
        // (IntersectionPoint)nfeat.getVertex(0), mesh,
        // clockwiseContour, myPositionTol);
        } else if (nfeat.isOutside(clockwise) != -1) {
            // XXX should check distance and redo if it is -1
            isContained = (nfeat.isOutside(clockwise) == 0);
        } else {
            System.out.println("isOutside not defined");
            isContained = false;
        }
        if (isContained) {
            contained.add(nested.get(i));
        }
    }
    return contained;
}