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

Example 11 with IntersectionPoint

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

the class SurfaceMeshIntersector method nearestInsideMip.

/**
 * Given a mesh intersection point associated with an edge, find the
 * nearest intersection point along the "inside" direction of the
 * associated edge. If no such point is found, return <code>null</code>.
 * "Inside" direction means the direction facing into the penetration
 * volume between the two meshes.
 *
 * @param p intersection point being queried
 * @param edge edge on which the intersection point lies
 * @param mesh mesh to which the intersection point belongs
 * is directed from the tail to the head.
 * @return nearest intersection point along the "inside" direction of the edge
 */
IntersectionPoint nearestInsideMip(IntersectionPoint p, HalfEdge edge, Vertex3d insideVtx, PolygonalMesh mesh) {
    EdgeInfo einfo = myEdgeInfos.get(edge.getPrimary());
    boolean debug = debugMipEdge.equals(edge.vertexStr());
    if (einfo != null && einfo.numMips() > 0) {
        int k = einfo.indexOfMip(p);
        if (debug) {
            NumberFormat fmt = new NumberFormat("%2d ");
            for (int j = 0; j < einfo.numMips(); j++) {
                IntersectionPoint mp = einfo.getMip(j);
                String prefix = (j == k ? "* " : "  ");
                prefix += fmt.format(getContourIndex(mp.contour));
                prefix += (mp.isCoincident() ? "C " : "  ");
                System.out.println(prefix + mp.toString("%g"));
            }
            System.out.println("k=" + k);
        }
        if (debug) {
            for (int ii = p.contourIndex - 3; ii < p.contourIndex + 4; ii++) {
                System.out.println(toString(p.contour.getWrapped(ii)));
            }
        }
        if (k != -1) {
            if (edge.getPrimary().getHead() == insideVtx) {
                // search towards the head
                while (k < einfo.numMips() - 1) {
                    IntersectionPoint mip = einfo.getMip(++k);
                    if (mip.contour.dividesMesh(mesh)) {
                        return mip;
                    }
                }
            } else {
                // search towards the tail
                while (k > 0) {
                    IntersectionPoint mip = einfo.getMip(--k);
                    if (mip.contour.dividesMesh(mesh)) {
                        return mip;
                    }
                }
            }
        } else {
            throw new InternalErrorException("intersection point not recorded by its edge");
        }
    }
    if (debug) {
        System.out.println("No nearest inside mip");
    }
    return null;
}

Example 12 with IntersectionPoint

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

the class SurfaceMeshIntersector method triangulateFace.

void triangulateFace(ArrayList<Vertex3d> triVtxs, Face face, PenetrationRegion region, boolean clockwise, ArrayList<Vertex3dList> outerHoles, ArrayList<Vertex3dList> innerHoles, HashMap<Vertex3d, Vertex3d> vertexMap) {
    PolygonalMesh mesh = (PolygonalMesh) face.getMesh();
    int meshNum = (region.myMesh == myMesh0 ? 0 : 1);
    Polygon3dCalc calc = new Polygon3dCalc(face.getWorldNormal(), myPositionTol);
    HalfEdge he0 = face.firstHalfEdge();
    HalfEdge he = he0;
    boolean debug = isDebugFace(face);
    int oldSize = triVtxs.size();
    if (debug) {
        System.out.println("TRIANGULATING " + debugFaceIdx);
        System.out.println("normal=" + face.getWorldNormal());
        System.out.println("tol=" + myPositionTol);
        Vertex3d v0 = he0.getHead();
        Vertex3d v1 = he0.getNext().getHead();
        Vertex3d v2 = he0.getTail();
        System.out.println(" vtx " + v0.getIndex() + " inside=" + region.myVertices.contains(v0));
        System.out.println(" vtx " + v1.getIndex() + " inside=" + region.myVertices.contains(v1));
        System.out.println(" vtx " + v2.getIndex() + " inside=" + region.myVertices.contains(v2));
        int[] idxs = SurfaceMeshIntersectorTest.getFaceIndices(region.myFaces);
        ArraySort.sort(idxs);
        ArraySupport.print("region faces: ", idxs);
        printFace(face);
        printFaceXips(face);
        if (debugOpFaceIdx != -1) {
            System.out.println("Face " + debugOpFaceIdx + ":");
            Face opFace = (debugMesh0 ? myMesh0.getFace(debugOpFaceIdx) : myMesh1.getFace(debugOpFaceIdx));
            printFaceXips(opFace);
        }
    }
    HashSet<IntersectionPoint> visitedMips = new HashSet<IntersectionPoint>();
    ArrayList<Vertex3dList> outerPolys = null;
    do {
        EdgeInfo einfo = myEdgeInfos.get(he.getPrimary());
        if (debug) {
            System.out.println("checking edge" + he.vertexStr() + " " + (he == he.getPrimary()));
        }
        if (einfo != null && einfo.numXips() > 0) {
            if (debug) {
                System.out.print("mips:");
                for (int k = 0; k < einfo.numXips(); k++) {
                    System.out.print(" " + einfo.getXip(k).contourIndex + "(" + getContourIndex(einfo.getXip(k).contour) + ")");
                }
                System.out.println("");
            }
            for (int k = 0; k < einfo.numXips(); k++) {
                IntersectionPoint mip = einfo.getXip(k, he, clockwise);
                if (region.myContours.contains(mip.contour) && !visitedMips.contains(mip) && // mip.contour.findSegmentFace (mip.contourIndex,mesh)==face) {
                traceFaceBegin(mip, face, meshNum)) {
                    // entering face - follow contour to the end
                    if (debug) {
                        System.out.println("entering face at " + mip.contourIndex);
                    }
                    Vertex3dList poly = traceFacePolygon(mip, face, region, clockwise, calc, visitedMips, vertexMap);
                    if (poly.size() > 2) {
                        if (outerPolys == null) {
                            outerPolys = new ArrayList<Vertex3dList>();
                        }
                        outerPolys.add(poly);
                    }
                } else if (debug) {
                    if (!region.myContours.contains(mip.contour)) {
                        System.out.println("region does not contain contour " + getContourIndex(mip.contour));
                        System.out.print("region contours:");
                        for (IntersectionContour c : region.myContours) {
                            System.out.print(" " + getContourIndex(c));
                        }
                        System.out.println("");
                    }
                    if (visitedMips.contains(mip)) {
                        System.out.println("mip was already visited");
                    }
                    System.out.println("trace face begin=" + traceFaceBegin(mip, face, meshNum));
                }
            }
        }
        he = he.getNext();
    } while (he != he0);
    if (outerPolys == null && outerHoles == null) {
        // create a single polygon for the outer face
        Vertex3dList poly = createTrianglePoly(face, region, vertexMap, clockwise, debug);
        outerPolys = new ArrayList<Vertex3dList>();
        outerPolys.add(poly);
    } else if (outerHoles != null && outerHoles.size() > 0) {
        if (outerPolys == null) {
            outerPolys = new ArrayList<Vertex3dList>();
        }
        outerPolys.addAll(outerHoles);
    }
    if (outerPolys == null) {
        Vertex3dList poly = createTrianglePoly(face, region, vertexMap, clockwise, debug);
        outerPolys = new ArrayList<Vertex3dList>();
        outerPolys.add(poly);
    // Vertex3dList poly = new Vertex3dList(/*closed=*/true);
    // do {
    // Vertex3d vtx = vertexMap.get(he.getHead());
    // if (vtx == null) {
    // throw new InternalErrorException ("vtx is null");
    // }
    // poly.add (vtx);
    // he = he.getNext();
    // }
    // while (he != he0);
    // outerPolys = new ArrayList<Vertex3dList>();
    // // if (!inside) {
    // //    poly.reverse();
    // // }
    // outerPolys.add (poly);
    // throw new InternalErrorException ("shouldn't be here");
    }
    if (innerHoles != null && outerPolys.size() > 1) {
        // multiple outer polys, so need to find out which hole belongs to
        // which
        HashMap<Vertex3dList, ArrayList<Vertex3dList>> polyHoleMap = new HashMap<Vertex3dList, ArrayList<Vertex3dList>>();
        ArrayList<Vertex3dList> containedHoles = null;
        for (Vertex3dList hole : innerHoles) {
            Polygon3dFeature nfeat = new Polygon3dFeature();
            // XXX ideally we want an interior point, in case the
            // outer point touches one of the contours
            Point3d ph = hole.get(0).pnt;
            double dmin = Double.POSITIVE_INFINITY;
            Vertex3dList nearestOuter = null;
            for (Vertex3dList outer : outerPolys) {
                calc.nearestFeature(nfeat, ph, outer, /*side=*/
                0);
                if (nfeat.getDistance() < dmin) {
                    dmin = nfeat.getDistance();
                    nearestOuter = outer;
                }
            }
            containedHoles = polyHoleMap.get(nearestOuter);
            if (containedHoles == null) {
                containedHoles = new ArrayList<Vertex3dList>();
                polyHoleMap.put(nearestOuter, containedHoles);
            }
            containedHoles.add(hole);
        }
        for (Vertex3dList outer : outerPolys) {
            containedHoles = polyHoleMap.get(outer);
            if (!calc.triangulate(triVtxs, outer, containedHoles)) {
                System.out.println("Can't triangulate: face " + face.getIndex() + " mesh0=" + (face.getMesh() == myMesh0));
                triangulationError = true;
            }
        }
    } else {
        // no inner holes, or just one outer poly
        for (Vertex3dList outer : outerPolys) {
            calc.debug = debug;
            if (debug) {
                System.out.println("outer size=" + outer.size());
                for (Vertex3dNode vn : outer) {
                    Vertex3d vtx = vn.getVertex();
                    System.out.println("  " + vtx.getIndex() + " " + vtx.pnt.toString("%12.8f"));
                }
            }
            if (outer.size() == 0) {
                throw new InternalErrorException("outer polygon has size 0, face " + face.getIndex());
            }
            if (!calc.triangulate(triVtxs, outer, innerHoles)) {
                System.out.println("Can't triangulate: face " + face.getIndex() + " mesh0=" + (face.getMesh() == myMesh0));
                triangulationError = true;
                System.out.println("outer poly");
                for (Vertex3dNode n : outer) {
                    System.out.println(n.getVertex().pnt);
                }
                for (IntersectionContour c : region.myContours) {
                    System.out.println("contour " + getContourIndex(c));
                    for (IntersectionPoint p : c) {
                        System.out.println(toString(p));
                    }
                }
                if (innerHoles != null) {
                    for (int hi = 0; hi < innerHoles.size(); hi++) {
                        System.out.println("hole " + hi);
                        for (Vertex3dNode n : innerHoles.get(hi)) {
                            System.out.println(n.getVertex().pnt);
                        }
                    }
                }
            }
        }
    }
    if (debug) {
        System.out.println("face: ADDED " + (triVtxs.size() - oldSize) / 3 + " at " + (oldSize) / 3 + " numOuter=" + outerPolys.size());
    }
}

Example 13 with IntersectionPoint

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

the class SurfaceMeshIntersector method intersectEdgeFace.

/**
 * Intersects an edge and a face. If there is an intersection, the resulting
 * point is returned in <code>mip</code> and the method returns
 * <code>true</code>.
 *
 * @param he edge to intersect
 * @param face face to intersect
 * @param mip returns intersection point if there is an intersection
 * @param edgeOnMesh0 if <code>true</code>, indicates that the edge belongs
 * to the first mesh being intersected.
 * @return <code>true</code> if the edge and face intersect.
 */
private boolean intersectEdgeFace(HalfEdge he, Face face, IntersectionPoint mip, boolean edgeOnMesh0) {
    // Do an efficient calculation first. If the edge/face pair is too close
    // to degenerate situation to determine intersection correctly, -1 is
    // returned and the computation is performed again using robust
    // predicates.
    int intersects = RobustPreds.intersectEdgeTriangle(mip, he, face, myMaxLength, edgeOnMesh0, /*worldCoords=*/
    true);
    // }
    if (intersects == 0) {
        return false;
    } else {
        mip.edge = he;
        mip.face = face;
        mip.intersectionCode = intersects;
        return true;
    }
}

Example 14 with IntersectionPoint

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

the class SurfaceMeshIntersector method createContourVertices.

/**
 * Creates mesh vertices corresponding to each contour intersection point in
 * a ContactInfo structure. These vertices can then be used in the formation
 * of a CSG mesh associated with the intersection. The vertex associated
 * with each intersection point is stored in the point's
 * <code>myVertex</code> field. A single common vertex is created for points
 * which are very close together. Also, for points which are very close to
 * an original vertex on one of the intersecting meshes, the newly created
 * vertex is projected onto the original vertex and an association from the
 * original to the new vertex is recorded in a vertex-vertex
 * <code>HashMap</code> which is returned by this method. Other new vertices
 * which are not associated with original vertices are stored in the
 * argument <code>contourVtxs</code>.
 */
private HashMap<Vertex3d, Vertex3d> createContourVertices(ArrayList<IntersectionContour> contours, ArrayList<Vertex3d> contourVtxs) {
    // Create vertices for each contour ...
    boolean debug = false;
    LinkedHashMap<Vertex3d, Vertex3d> vertexMap = new LinkedHashMap<Vertex3d, Vertex3d>();
    for (IntersectionContour c : contours) {
        IntersectionPoint mip = c.get(0);
        if (c.isClosed()) {
            // If the contour is closed, back up until we find a point whose
            // preceeding point is *not* coincident. There must be such a
            // point, because otherwise the contour would consist of a single
            // set of coincident points and would have been eliminated.
            mip = backupToNonCoincident(c.get(0));
        // IntersectionPoint prev = mip.prev();
        // if (prev.distance (mip0) <= myPositionTol) {
        // do {
        // mip = prev;
        // prev = mip.prev();
        // }
        // while (prev.distance (mip0) <= myPositionTol && mip != mip0);
        // }
        }
        // Create a new vertex at the starting intersection point.
        BooleanHolder vtxIsCoincident = new BooleanHolder();
        Vertex3d newVtx = createNewVertex(mip, vertexMap, vtxIsCoincident);
        if (!vtxIsCoincident.value) {
            contourVtxs.add(newVtx);
        }
        mip.myVertex = newVtx;
        mip.effectiveFace0 = c.findSegmentFace(mip, myMesh0);
        mip.effectiveFace1 = c.findSegmentFace(mip, myMesh1);
        mip.nearEdge0 = null;
        mip.nearEdge1 = null;
        mip.clearEmptyMarks();
        for (int i = 1; i < c.size(); i++) {
            mip = mip.next();
            // Question: do we want to cluster based on lastp, or newVtx.pnt?
            if (mip.distance(newVtx.pnt) > myPositionTol) {
                if (debug) {
                    System.out.println("mip " + mip.contourIndex + " new vertex");
                }
                newVtx = createNewVertex(mip, vertexMap, vtxIsCoincident);
                if (!vtxIsCoincident.value) {
                    contourVtxs.add(newVtx);
                }
            } else {
                if (debug) {
                    System.out.println("mip " + mip.contourIndex + " old vertex");
                }
                if (vtxIsCoincident.value) {
                    mapCoincidentVertices(mip, newVtx, vertexMap);
                }
            }
            mip.myVertex = newVtx;
            mip.effectiveFace0 = c.findSegmentFace(mip, myMesh0);
            mip.effectiveFace1 = c.findSegmentFace(mip, myMesh1);
            mip.nearEdge0 = null;
            mip.nearEdge1 = null;
            mip.clearEmptyMarks();
        }
    }
    return vertexMap;
}

Example 15 with IntersectionPoint

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

the class SurfaceMeshIntersector method sortCoincidentPointsNew.

/**
 * Sorts the coincident points associated with p. All coincident points
 * associated with p are found by locating all coincident points along the
 * edge that have the same 'coincidentGroup'. These points are then examined
 * in contour order, and then convexity tests are applied to see if this
 * contour ordering corresponds to a positive or negative direction along
 * the edge.  The points are then repositioned along the edge accordingly.
 *
 * <p>The convexity tests are applied to adjacent points, with the most
 * robust tests (but possibly indeterminant) tests applied first, until a
 * definite result is found.
 *
 * <p>Note that if there are more than two coincident points, it is not
 * generally possible to do this ordering until the entire contour is
 * generated, because the convexity tests need to be applied between
 * adjacent pairs, and we don't know the correct adjacency relationships
 * until all points have been added.
 */
void sortCoincidentPointsNew(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;
    }
}