Examples with TriTriIntersection maspack.geometry.TriTriIntersection used on opensource projects

Example 1 with TriTriIntersection

use of maspack.geometry.TriTriIntersection in project artisynth_core by artisynth.

the class MeshCollider method createContactPlanes.

/**
 * From a complete set of intersections, get all of the regions so that each
 * region is defined by a set of continuous intersections.
 *
 * @param regions (output) the complete set of intersection regions.
 * @param intersections
 * The complete set of intersections.
 * @param regionTol tolerance
 */
static void createContactPlanes(ArrayList<ContactPlane> regions, ArrayList<TriTriIntersection> intersections, double regionTol) {
    if (intersections.size() == 0) {
        // no intersections, so no planes to compute
        return;
    }
    AccelerationGrid<TriTriIntersection> accgrid = new AccelerationGrid<TriTriIntersection>();
    Point3d min = new Point3d(intersections.get(0).points[0]), max = new Point3d(intersections.get(0).points[0]);
    for (TriTriIntersection isect : intersections) for (Point3d p : isect.points) {
        min.min(p);
        max.max(p);
    }
    double dist = Math.max(min.distance(max), EPS);
    min.x -= dist * 0.1;
    min.y -= dist * 0.1;
    min.z -= dist * 0.1;
    max.x += dist * 0.1;
    max.y += dist * 0.1;
    max.z += dist * 0.1;
    accgrid.set(min, max, intersections.size() * 2);
    for (TriTriIntersection isect : intersections) for (Point3d p : isect.points) accgrid.add_element(isect, p, p);
    for (TriTriIntersection isect : intersections) {
        ContactPlane region = new ContactPlane();
        if (accgrid.elementidxs.get(isect) != null) {
            traverseRegion(accgrid, region, isect, regionTol);
            regions.add(region);
        }
    }
// System.out.println("region time " + (System.currentTimeMillis() - t0));
}

Example 2 with TriTriIntersection

use of maspack.geometry.TriTriIntersection in project artisynth_core by artisynth.

the class MeshCollider method getContactPlaneInfo.

/**
 * Get information about a specific region of intersections.
 */
static void getContactPlaneInfo(ContactPlane region, PolygonalMesh mesh0, PolygonalMesh mesh1, double pointTol) {
    Vector3d sectnormal = new Vector3d(), tmpnormal = new Vector3d();
    BVFeatureQuery query = new BVFeatureQuery();
    RigidTransform3d trans0 = mesh0.getMeshToWorld();
    RigidTransform3d trans1 = mesh1.getMeshToWorld();
    // calculate a weighted average of the face normals
    for (TriTriIntersection isect : region.intersections) {
        region.points = new ArrayList<Point3d>();
        region.points.add(isect.points[0]);
        region.points.add(isect.points[1]);
        double length = isect.points[0].distance(isect.points[1]);
        tmpnormal.transform(trans0, isect.face0.getNormal());
        tmpnormal.negate();
        sectnormal.scaledAdd(length, tmpnormal, sectnormal);
        tmpnormal.transform(trans1, isect.face1.getNormal());
        sectnormal.scaledAdd(length, tmpnormal, sectnormal);
    }
    // calculate the weighted intersection center
    Point3d center = new Point3d();
    double weight = 0;
    for (TriTriIntersection isect : region.intersections) {
        double length = isect.points[0].distance(isect.points[1]);
        center.scaledAdd(length, isect.points[0], center);
        center.scaledAdd(length, isect.points[1], center);
        weight += 2 * length;
    }
    center.scale(1.0 / weight);
    region.centroid = center;
    // calculate the weighted normal
    Vector3d cp0 = new Vector3d(), cp1 = new Vector3d();
    region.normal.setZero();
    for (TriTriIntersection isect : region.intersections) {
        cp0.sub(isect.points[0], center);
        cp1.sub(isect.points[1], center);
        tmpnormal.cross(cp0, cp1);
        if (tmpnormal.dot(sectnormal) < 0)
            tmpnormal.negate();
        region.normal.add(tmpnormal);
    }
    if (region.normal.dot(sectnormal) < 0)
        region.normal.negate();
    // handle degenerate cases
    if (region.normal.containsNaN() || region.normal.norm() < EPS) {
        region.normal.setZero();
        Point3d p0 = new Point3d();
        Point3d p1 = new Point3d();
        Vector3d c0 = new Vector3d();
        Vector3d c1 = new Vector3d();
        for (TriTriIntersection isect : region.intersections) {
            for (Point3d p : isect.points) {
                p0.inverseTransform(trans0, p);
                p1.inverseTransform(trans1, p);
                Vertex3d u0 = isect.face0.getVertex(0);
                Vertex3d u1 = isect.face0.getVertex(1);
                Vertex3d u2 = isect.face0.getVertex(2);
                Vertex3d v0 = isect.face1.getVertex(0);
                Vertex3d v1 = isect.face1.getVertex(1);
                Vertex3d v2 = isect.face1.getVertex(2);
                getCoordinates(c0, u0.pnt, u1.pnt, u2.pnt, p0);
                getCoordinates(c1, v0.pnt, v1.pnt, v2.pnt, p1);
                int[] type0 = classifyPoint(c0);
                int[] type1 = classifyPoint(c1);
                if (type0[0] == 2) {
                    if (type1[0] == 2) {
                        // vertex,vertex
                        region.normal.add(vertexVertexNormal(trans0, trans1, isect.face0, isect.face1, type0[1], type1[1]));
                    } else if (type1[0] == 1) {
                        // vertex,edge
                        region.normal.add(vertexEdgeNormal(trans0, trans1, isect.face0, isect.face1, type0[1], type1[1]));
                    } else {
                        // vertex,face
                        region.normal.add(vertexFaceNormal(trans0, trans1, isect.face0, isect.face1, type0[1]));
                    }
                } else if (type0[0] == 1) {
                    if (type1[0] == 2) {
                        // edge,vertex
                        region.normal.sub(vertexEdgeNormal(trans1, trans0, isect.face1, isect.face0, type1[1], type0[1]));
                    } else if (type1[0] == 1) {
                        // edge,edge
                        region.normal.add(edgeEdgeNormal(trans0, trans1, isect.face0, isect.face1, type0[1], type1[1]));
                    } else {
                        // edge,face
                        region.normal.add(edgeFaceNormal(trans0, trans1, isect.face0, isect.face1, type0[1]));
                    }
                } else {
                    if (type1[0] == 2) {
                        // face,vertex
                        region.normal.sub(vertexFaceNormal(trans1, trans0, isect.face1, isect.face0, type1[1]));
                    } else if (type1[0] == 1) {
                        // face,edge
                        region.normal.sub(edgeFaceNormal(trans1, trans0, isect.face1, isect.face0, type1[1]));
                    } else {
                        // face,face
                        region.normal.add(faceFaceNormal(trans0, trans1, isect.face0, isect.face1));
                    }
                }
            }
        }
    }
    region.normal.normalize();
    // calculate the contact depth for the region
    boolean foundPenetratingVertice = false;
    Point3d p = new Point3d();
    Point3d nearest = new Point3d();
    Vector3d diff = new Vector3d();
    Vector2d coords = new Vector2d();
    Vertex3d v;
    Face nf;
    Point3d plocal = new Point3d();
    LinkedHashSet<Vertex3d> regionvertices0 = new LinkedHashSet<Vertex3d>();
    LinkedHashSet<Vertex3d> regionvertices1 = new LinkedHashSet<Vertex3d>();
    region.depth = 0;
    for (TriTriIntersection isect : region.intersections) {
        for (int i = 0; i < 3; i++) {
            // face0 vertex depths
            v = isect.face0.getVertex(i);
            p.transform(trans0, v.pnt);
            plocal.inverseTransform(trans1, p);
            plocal.sub(isect.face1.getVertex(0).pnt);
            if (plocal.dot(isect.face1.getNormal()) <= 0) {
                regionvertices0.add(v);
            }
            // face1 vertex depths
            v = isect.face1.getVertex(i);
            p.transform(trans1, v.pnt);
            plocal.inverseTransform(trans0, p);
            plocal.sub(isect.face0.getVertex(0).pnt);
            if (plocal.dot(isect.face0.getNormal()) <= 0) {
                regionvertices1.add(v);
            }
        }
    }
    for (Vertex3d v0 : regionvertices0) {
        p.transform(trans0, v0.pnt);
        // XXX Sanchez, Jun 22, 2014
        // Changed to isInside.  Sometimes a vertex is outside
        // the mesh but determined to be "penetrating" due to
        // normal (e.g. when nearest to an edge)
        // nf = myQuery.nearestFaceToPoint (nearest, coords, mesh1, p);
        boolean inside = query.isInsideOrientedMesh(mesh1, p, 0);
        if (inside) {
            query.getFaceForInsideOrientedTest(nearest, coords);
            nearest.transform(trans1);
            diff.sub(p, nearest);
            diff.inverseTransform(trans1);
            foundPenetratingVertice = true;
            // -diff.dot (nf.getNormal());
            double dist = diff.norm();
            if (dist > region.depth)
                region.depth = dist;
        }
    }
    for (Vertex3d v1 : regionvertices1) {
        p.transform(trans1, v1.pnt);
        // nf = myQuery.nearestFaceToPoint (nearest, coords, mesh0, p);
        boolean inside = query.isInsideOrientedMesh(mesh0, p, 0);
        if (inside) {
            query.getFaceForInsideOrientedTest(nearest, coords);
            nearest.transform(trans0);
            diff.sub(p, nearest);
            diff.inverseTransform(trans0);
            foundPenetratingVertice = true;
            // -diff.dot (nf.getNormal());
            double dist = diff.norm();
            if (dist > region.depth)
                region.depth = dist;
        }
    }
    if (!foundPenetratingVertice) {
        double min = Double.POSITIVE_INFINITY, max = Double.NEGATIVE_INFINITY;
        for (int i = 0; i < region.points.size(); i++) {
            double d = region.points.get(i).dot(region.normal);
            if (d < min)
                min = d;
            if (d > max)
                max = d;
        }
        region.depth = max - min;
    }
    // eliminate redundant points
    // use point tolerance
    region.points.clear();
    for (TriTriIntersection isect : region.intersections) {
        for (Point3d pcandidate : isect.points) {
            boolean add = true;
            for (Point3d other : region.points) if (pcandidate.epsilonEquals(other, pointTol)) {
                add = false;
                break;
            }
            if (add) {
                region.points.add(pcandidate);
            }
        }
    }
    // take extrema along n axes
    if (numextremaaxes > 0) {
        // final ArrayList<Vector3d> axes = new ArrayList<Vector3d>();
        Vector3d crosszup = new Vector3d(0, 0, 1);
        crosszup.cross(region.normal, crosszup);
        double crosszupnorm = crosszup.norm();
        RigidTransform3d normtoworld;
        if (crosszup.norm() > EPS) {
            normtoworld = new RigidTransform3d(new Vector3d(), new AxisAngle(crosszup, Math.asin(crosszupnorm)));
        } else {
            normtoworld = new RigidTransform3d();
        }
        boolean[] keep = new boolean[region.points.size()];
        for (int j = 0; j < region.points.size(); j++) keep[j] = false;
        Vector3d offset = new Vector3d();
        Vector3d axis = new Vector3d();
        for (int i = 0; i < numextremaaxes; i++) {
            double min = Double.POSITIVE_INFINITY, max = Double.NEGATIVE_INFINITY;
            int mini = 0, maxi = 0;
            double angle = Math.PI * i / numextremaaxes;
            axis.set(Math.cos(angle), Math.sin(angle), 0);
            axis.transform(normtoworld);
            for (int j = 0; j < region.points.size(); j++) {
                offset.sub(region.points.get(j), center);
                double dot = offset.dot(axis);
                if (dot < min) {
                    min = dot;
                    mini = j;
                }
                if (dot > max) {
                    max = dot;
                    maxi = j;
                }
            }
            keep[mini] = true;
            keep[maxi] = true;
        }
        for (int j = (region.points.size() - 1); j >= 0; j--) {
            if (!keep[j])
                region.points.remove(j);
        }
    }
}

Example 3 with TriTriIntersection

use of maspack.geometry.TriTriIntersection in project artisynth_core by artisynth.

the class CollisionRenderer method prerender.

public void prerender(CollisionHandler handler, RenderProps props) {
    RenderObject ro = new RenderObject();
    ro.clearAll();
    // constraints
    ro.createLineGroup();
    // segments
    ro.createLineGroup();
    // contours
    ro.createLineGroup();
    // contact info
    ro.createPointGroup();
    // intersection faces
    ro.createTriangleGroup();
    // create default dummy normal
    ro.addNormal(0, 0, 0);
    CollisionBehavior behav = handler.myBehavior;
    ContactInfo cinfo = handler.getLastContactInfo();
    if (behav.myDrawConstraints) {
        ro.lineGroup(CONSTRAINT_GRP);
        double nrmlLen = handler.getContactNormalLen();
        if (nrmlLen > 0) {
            addConstraintRenderInfo(ro, handler.myBilaterals0.values(), nrmlLen);
            addConstraintRenderInfo(ro, handler.myBilaterals1.values(), nrmlLen);
            addConstraintRenderInfo(ro, handler.myUnilaterals, nrmlLen);
        }
    }
    double normalLen = 0;
    if (behav.getDrawContactNormals()) {
        normalLen = handler.getContactNormalLen();
    }
    if (normalLen != 0 && cinfo != null) {
        ro.lineGroup(SEGMENT_GRP);
        Method method = handler.getMethod();
        if (method == Method.CONTOUR_REGION) {
            int numc = 0;
            for (ContactPlane region : cinfo.getContactPlanes()) {
                for (Point3d p : region.points) {
                    if (numc >= handler.myMaxUnilaterals) {
                        break;
                    }
                    addLineSeg(ro, p, region.normal, normalLen);
                }
            }
        } else if (method != Method.INACTIVE) {
            for (ContactConstraint cc : handler.myBilaterals0.values()) {
                maybeAddVertexFaceNormal(ro, cc, normalLen);
            }
            for (ContactConstraint cc : handler.myBilaterals1.values()) {
                maybeAddVertexFaceNormal(ro, cc, normalLen);
            }
        }
    }
    if (behav.myDrawIntersectionContours && props.getEdgeWidth() > 0 && cinfo != null) {
        ro.lineGroup(CONTOUR_GRP);
        // offset lines
        if (cinfo.getContours() != null) {
            for (IntersectionContour contour : cinfo.getContours()) {
                int vidx0 = ro.numVertices();
                for (IntersectionPoint p : contour) {
                    ro.addVertex(ro.addPosition((float) p.x, (float) p.y, (float) p.z));
                }
                int vidx1 = ro.numVertices() - 1;
                ro.addLineLoop(vidx0, vidx1);
            }
        } else if (cinfo.getIntersections() != null) {
            // use intersections to render lines
            for (TriTriIntersection tsect : cinfo.getIntersections()) {
                addLineSeg(ro, tsect.points[0], tsect.points[1]);
            }
        }
    }
    if (behav.myDrawIntersectionPoints && cinfo != null) {
        if (cinfo.getIntersections() != null) {
            for (TriTriIntersection tsect : cinfo.getIntersections()) {
                for (Point3d pnt : tsect.points) {
                    addPoint(ro, pnt);
                }
            }
        }
        for (PenetratingPoint cpp : cinfo.getPenetratingPoints(0)) {
            if (cpp.distance > 0) {
                addPoint(ro, cpp.vertex.getWorldPoint());
            }
        }
        for (PenetratingPoint cpp : cinfo.getPenetratingPoints(1)) {
            if (cpp.distance > 0) {
                addPoint(ro, cpp.vertex.getWorldPoint());
            }
        }
        if (behav.getMethod() == CollisionBehavior.Method.VERTEX_EDGE_PENETRATION) {
            if (cinfo.getEdgeEdgeContacts() != null) {
                for (EdgeEdgeContact eec : cinfo.getEdgeEdgeContacts()) {
                    addPoint(ro, eec.point0);
                    addPoint(ro, eec.point1);
                }
            }
        }
    }
    if (behav.myDrawIntersectionFaces && cinfo != null) {
        ArrayList<TriTriIntersection> intersections = cinfo.getIntersections();
        if (intersections != null) {
            buildFaceSegments(ro, handler, intersections);
        }
    }
    RenderObject oldRob = myRob;
    myRob = ro;
    // if (oldRob != null) {
    // oldRob.dispose();
    // }
    RenderObject rd = null;
    if (behav.myDrawPenetrationDepth != -1 && cinfo != null) {
        int num = behav.myDrawPenetrationDepth;
        Collidable b0 = behav.getCollidable(0);
        CollidableBody h0 = handler.getCollidable(0);
        if (!(b0 instanceof Group)) {
            if (h0 != b0 && h0.getCollidableAncestor() != b0) {
                // then we want the *other* collidable body, so switch num
                num = (num == 0 ? 1 : 0);
            }
        }
        ArrayList<PenetrationRegion> regions;
        ArrayList<PenetratingPoint> points;
        if (num == 0) {
            regions = cinfo.getRegions(0);
            points = cinfo.getPenetratingPoints(0);
        } else {
            regions = cinfo.getRegions(1);
            points = cinfo.getPenetratingPoints(1);
        }
        if (regions != null && regions.size() > 0) {
            rd = createPenetrationRenderObject(handler, points, regions);
        }
    }
    oldRob = myDepthRob;
    myDepthRob = rd;
// if (oldRob != null) {
// oldRob.dispose();
// }
}

Example 4 with TriTriIntersection

use of maspack.geometry.TriTriIntersection in project artisynth_core by artisynth.

the class IntersectionFactory method buildIntersectionChart.

public static boolean[][] buildIntersectionChart(PolygonalMesh[] meshList, double tol) {
    int n = meshList.length;
    boolean[][] out = new boolean[n][n];
    ArrayList<TriTriIntersection> intersections = new ArrayList<TriTriIntersection>();
    // TriangleIntersector ti = new TriangleIntersector();
    BVFeatureQuery query = new BVFeatureQuery();
    BVIntersector intersector = new BVIntersector();
    for (int i = 0; i < n; i++) {
        PolygonalMesh mesh1 = meshList[i];
        out[i][i] = true;
        for (int j = i + 1; j < n; j++) {
            PolygonalMesh mesh2 = meshList[j];
            boolean intersects = false;
            // }
            if (intersector.intersectMeshMesh(intersections, mesh1, mesh2)) {
                intersects = true;
                intersections.clear();
            } else if (query.isInsideOrientedMesh(mesh1, mesh2.getVertex(0).getPosition(), tol) || query.isInsideOrientedMesh(mesh2, mesh1.getVertex(0).getPosition(), tol)) {
                intersects = true;
            }
            out[i][j] = intersects;
            out[j][i] = intersects;
        }
    }
    return out;
}

Example 5 with TriTriIntersection

use of maspack.geometry.TriTriIntersection in project artisynth_core by artisynth.

the class CollisionRenderer method buildFaceSegments.

protected void buildFaceSegments(RenderObject ro, CollisionHandler handler, ArrayList<TriTriIntersection> intersections) {
    BVFeatureQuery query = new BVFeatureQuery();
    PolygonalMesh mesh0 = handler.getCollidable(0).getCollisionMesh();
    PolygonalMesh mesh1 = handler.getCollidable(1).getCollisionMesh();
    ArrayList<Face> faces = new ArrayList<Face>();
    // mark faces as visited and add segments
    for (TriTriIntersection isect : intersections) {
        isect.face0.setVisited();
        isect.face1.setVisited();
    // add partials?
    }
    // mark interior faces and add segments
    for (TriTriIntersection isect : intersections) {
        if (isect.face0.getMesh() != mesh0) {
            findInsideFaces(isect.face0, query, mesh0, faces);
            findInsideFaces(isect.face1, query, mesh1, faces);
        } else {
            findInsideFaces(isect.face0, query, mesh1, faces);
            findInsideFaces(isect.face1, query, mesh0, faces);
        }
    }
    for (TriTriIntersection isect : intersections) {
        isect.face0.clearVisited();
        isect.face1.clearVisited();
    }
    // add faces to render object and clear visited flag
    Vector3d nrm = new Vector3d();
    Point3d p0 = new Point3d();
    Point3d p1 = new Point3d();
    Point3d p2 = new Point3d();
    for (Face face : faces) {
        face.clearVisited();
        face.getWorldNormal(nrm);
        ro.addNormal((float) nrm.x, (float) nrm.y, (float) nrm.z);
        HalfEdge he = face.firstHalfEdge();
        he.head.getWorldPoint(p0);
        he = he.getNext();
        he.head.getWorldPoint(p1);
        he = he.getNext();
        he.head.getWorldPoint(p2);
        int v0idx = ro.vertex((float) p0.x, (float) p0.y, (float) p0.z);
        int v1idx = ro.vertex((float) p1.x, (float) p1.y, (float) p1.z);
        int v2idx = ro.vertex((float) p2.x, (float) p2.y, (float) p2.z);
        ro.addTriangle(v0idx, v1idx, v2idx);
    }
}