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

Example 1 with PenetrationRegion

use of maspack.collision.PenetrationRegion 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 2 with PenetrationRegion

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

the class CollisionRenderer method createPenetrationRenderObject.

RenderObject createPenetrationRenderObject(CollisionHandler handler, ArrayList<PenetratingPoint> points, ArrayList<PenetrationRegion> regions) {
    RenderObject rd = new RenderObject();
    HashMap<Vertex3d, Double> depthMap = new HashMap<Vertex3d, Double>();
    double maxd = 0;
    for (PenetratingPoint pp : points) {
        depthMap.put(pp.vertex, pp.distance);
        if (pp.distance > maxd) {
            maxd = pp.distance;
        }
    }
    ScalarRange range = handler.myBehavior.myPenetrationDepthRange;
    range.updateInterval(0, maxd);
    float[] rgb = new float[3];
    for (int i = 0; i < 256; i++) {
        handler.myManager.myColorMap.getRGB(i / 255.0, rgb);
        rd.addColor(rgb);
    }
    Point3d wpnt = new Point3d();
    Vector3d wnrm = new Vector3d();
    for (PenetrationRegion region : regions) {
        for (Face face : region.getFaces()) {
            HalfEdge he = face.firstHalfEdge();
            Vertex3d v0 = he.getHead();
            Vertex3d v1 = he.getNext().getHead();
            Vertex3d v2 = he.getTail();
            v0.getWorldPoint(wpnt);
            int pi0 = rd.addPosition(wpnt);
            v1.getWorldPoint(wpnt);
            int pi1 = rd.addPosition(wpnt);
            v2.getWorldPoint(wpnt);
            int pi2 = rd.addPosition(wpnt);
            int ci0 = getColorIndex(range, v0, depthMap);
            int ci1 = getColorIndex(range, v1, depthMap);
            int ci2 = getColorIndex(range, v2, depthMap);
            face.getWorldNormal(wnrm);
            int ni = rd.addNormal(wnrm);
            int v0idx = rd.addVertex(pi0, ni, ci0, -1);
            int v1idx = rd.addVertex(pi1, ni, ci1, -1);
            int v2idx = rd.addVertex(pi2, ni, ci2, -1);
            rd.addTriangle(v0idx, v1idx, v2idx);
        }
    }
    return rd;
}