Search in sources :

Example 6 with BVNode

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

the class MFreeFactory method findNodesContaining.

// public static ArrayList<MFreeElement3d> createPartitionedElements(
// MFreeShapeFunction fun,
// List<MFreeNode3d> nodes, DirectedGraph<int[],Integer> connectivityGraph) {
// 
// ArrayList<MFreeElement3d> elems =
// new ArrayList<MFreeElement3d>(connectivityGraph.numVertices());
// 
// for (Vertex<int[],Integer> vtx : connectivityGraph.getVertices()) {
// 
// int[] idxs = vtx.getData();
// if (idxs.length > 0) {
// MFreeNode3d[] enodes = new MFreeNode3d[idxs.length];
// for (int i = 0; i < idxs.length; i++) {
// enodes[i] = nodes.get(idxs[i]);
// }
// MFreeElement3d elem = new MFreeElement3d(fun, enodes);
// elem.setAllTermsActive(true);
// elems.add(elem);
// }
// }
// 
// return elems;
// 
// }
// public static ArrayList<MFreeElement3d> createPairedElements(
// MFreeShapeFunction fun,
// List<MFreeNode3d> nodes, boolean[][] iChart) {
// // elements from node pairs
// ArrayList<MFreeElement3d> elemList = new ArrayList<MFreeElement3d>();
// for (int i = 0; i < nodes.size(); i++) {
// MFreeNode3d nodeA = nodes.get(i);
// MFreeElement3d e = new MFreeElement3d(fun, new MFreeNode3d[] { nodeA });
// e.setTermActive(0, 0, true);
// elemList.add(e);
// for (int j = i + 1; j < nodes.size(); j++) {
// if (iChart[i][j]) {
// e =
// new MFreeElement3d(fun, new MFreeNode3d[] { nodeA, nodes.get(j) });
// e.setTermActive(0, 1, true);
// e.setTermActive(1, 0, true);
// e.setTermActive(0, 0, false);
// e.setTermActive(1, 1, false);
// elemList.add(e);
// }
// }
// }
// 
// return elemList;
// }
// 
// public static void createNodeMeshes(MFreeModel3d model,
// Collection<MFreeNode3d> nodes, PolygonalMesh surface) {
// 
// // set nodal influence regions
// PolygonalMesh icoSphere = MeshFactory.createIcosahedralSphere(1, 2);
// 
// if (nodes == null) {
// nodes = model.getNodes();
// }
// 
// HashMap<MFreeNode3d,PolygonalMesh> meshMap = new HashMap<MFreeNode3d,PolygonalMesh>();
// AffineTransform3d trans = new AffineTransform3d();
// for (MFreeNode3d node : nodes) {
// PolygonalMesh nmesh = null;
// if (node.isRadial()) {
// nmesh = new PolygonalMesh(icoSphere);
// double r = node.getInfluenceRadius();
// trans.setIdentity();
// trans.setTranslation(node.getRestPosition());
// trans.applyScaling(r, r, r);
// nmesh.transform(trans);
// 
// if (surface != null) {
// nmesh = MeshFactory.getIntersection(nmesh, surface);
// }
// meshMap.put(node, nmesh);
// }
// }
// 
// // I do this after generating all meshes so that isInDomain doesn't start
// // using the meshes before all are ready (speed issue)
// for (MFreeNode3d node : nodes) {
// PolygonalMesh nmesh = meshMap.get(node);
// if (nmesh != null) {
// node.setBoundaryMesh(nmesh);
// }
// // model.addMesh("node_" + node.getNumber(), nmesh);
// }
// }
// public static void createPairedElemMeshes(List<MFreeElement3d> elemList,
// BVTree nodeTree) {
// 
// // only intersections
// for (MFreeElement3d elem : elemList) {
// if (elem.numNodes() == 1) {
// elem.setBoundaryMesh(elem.getNode(0).getBoundaryMesh());
// } else {
// PolygonalMesh mesh =
// new PolygonalMesh(elem.getNode(0).getBoundaryMesh());
// for (int i = 1; i < elem.numNodes(); i++) {
// mesh =
// MeshFactory.getIntersection(mesh, elem
// .getNode(i).getBoundaryMesh());
// }
// mesh = (PolygonalMesh)convertToMFreeMesh(mesh, nodeTree, DEFAULT_TOLERANCE);
// elem.setBoundaryMesh(mesh);
// }
// }
// 
// }
// public static void createElemMeshes(
// MFreeModel3d model, Collection<MFreeElement3d> elemList, PolygonalMesh surface) {
// 
// if (elemList == null) {
// elemList = model.getElements();
// }
// 
// // pre-build BSP trees for all nodes
// HashMap<Integer,BSPTree> meshMap = new HashMap<Integer,BSPTree>();
// HashMap<BSPTree,MFreeNode3d> meshMapInv = new HashMap<BSPTree,MFreeNode3d>();
// 
// HashSet<MFreeNode3d> nodeset = new HashSet<>();
// for (MFreeElement3d elem : elemList) {
// for (MFreeNode3d node : elem.getNodes()) {
// nodeset.add(node);
// }
// }
// ArrayList<MFreeNode3d> nodes = new ArrayList<>(nodeset);
// 
// boolean[][] connectivityChart = buildIntersectionChart(nodes);
// DirectedGraph<int[],Integer> connectivityGraph = IntersectionFactory.buildConnectivityGraph(connectivityChart);
// 
// HashMap<int[],BSPTree> nullMap = new HashMap<int[],BSPTree>(1);
// for (int i = 0; i < nodes.size(); i++) {
// MFreeNode3d node = nodes.get(i);
// BSPTree tree = new BSPTree(node.getBoundaryMesh());
// meshMap.put(i, tree);
// meshMapInv.put(tree, node);
// }
// 
// DirectedGraph<BSPTree,BSPTree> meshGraph = connectivityGraph.exchangeData(nullMap, meshMap);
// IntersectionFactory.buildSpatialPartition(meshGraph, null);
// DirectedGraph<BSPTree,MFreeNode3d> nodeGraph = meshGraph.exchangeEdgeData(meshMapInv);
// Vertex<BSPTree,MFreeNode3d> root = nodeGraph.getVertex(0);
// 
// for (MFreeElement3d elem : elemList) {
// Vertex<BSPTree,MFreeNode3d> vtx = root;
// for (MFreeNode3d node : elem.getNodes()) {
// for (DirectedEdge<BSPTree,MFreeNode3d> edge : vtx.getForwardEdges()) {
// if (edge.getData() == node) {
// vtx = edge.traverseForwards();
// break;
// }
// }
// }
// 
// PolygonalMesh mesh = vtx.getData().generateMesh();
// if (mesh.numFaces() > 0) {
// elem.setBoundaryMesh(mesh);
// // model.addMesh("elem_" + elem.getNumber(), mesh);
// }
// 
// }
// }
// public static ArrayList<MFreeElement3d> findPairdElementsContaining(
// Point3d pnt, BVTree bvtree, double tol) {
// 
// ArrayList<MFreeElement3d> deps = new ArrayList<MFreeElement3d>();
// ArrayList<BVNode> bvNodes = new ArrayList<BVNode>(16);
// bvtree.intersectPoint(bvNodes, pnt);
// 
// if (bvNodes.size() == 0) {
// return deps;
// }
// 
// for (BVNode n : bvNodes) {
// Boundable[] elements = n.getElements();
// for (int i = 0; i < elements.length; i++) {
// MFreeElement3d elem = (MFreeElement3d)elements[i];
// 
// boolean isInside = true;
// for (MFreeNode3d node : elem.getNodes()) {
// if (!node.isInDomain(pnt, tol)) {
// isInside = false;
// break;
// }
// }
// if (isInside) {
// deps.add(elem);
// }
// }
// }
// return deps;
// }
// public static ArrayList<MFreeElement3d> findPartitionedElementsContaining(
// Point3d pnt, DirectedGraph<MFreeElement3d,MFreeNode3d> connectivity,
// BVTree bvtree, double tol) {
// 
// ArrayList<MFreeElement3d> deps = new ArrayList<MFreeElement3d>();
// ArrayList<BVNode> bvNodes = new ArrayList<BVNode>(16);
// bvtree.intersectPoint(bvNodes, pnt);
// 
// if (bvNodes.size() == 0) {
// return deps;
// }
// 
// for (BVNode n : bvNodes) {
// Boundable[] elements = n.getElements();
// for (int i = 0; i < elements.length; i++) {
// MFreeElement3d elem = (MFreeElement3d)elements[i];
// 
// boolean isInside = true;
// for (MFreeNode3d node : elem.getNodes()) {
// if (!node.isInDomain(pnt, tol)) {
// isInside = false;
// break;
// }
// }
// 
// // exclude deeper intersections
// if (isInside) {
// Vertex<MFreeElement3d,MFreeNode3d> vtx =
// connectivity.findVertex(elem);
// for (DirectedEdge<MFreeElement3d,MFreeNode3d> edge : vtx
// .getForwardEdges()) {
// MFreeNode3d node = edge.getData();
// if (node.isInDomain(pnt, tol)) {
// isInside = false;
// break;
// }
// }
// }
// if (isInside) {
// deps.add(elem);
// }
// }
// }
// return deps;
// }
// public static DirectedGraph<MFreeElement3d,MFreeNode3d> convertConnectivity(
// List<MFreeNode3d> nodes, List<MFreeElement3d> elems,
// DirectedGraph<int[],Integer> graph) {
// 
// DirectedGraph<MFreeElement3d,MFreeNode3d> out = graph.cloneStructure();
// 
// // copy over nodes
// for (int i = 0; i < graph.numDirectedEdges(); i++) {
// DirectedEdge<int[],Integer> idxEdge = graph.getDirectedEdge(i);
// DirectedEdge<MFreeElement3d,MFreeNode3d> nodeEdge =
// out.getDirectedEdge(i);
// nodeEdge.setData(nodes.get(idxEdge.getData()));
// }
// 
// // fill in vertices by traversing along edges
// Vertex<MFreeElement3d,MFreeNode3d> base = out.getVertex(0);
// for (MFreeElement3d elem : elems) {
// MFreeNode3d[] nodeArray = elem.getNodes();
// Vertex<MFreeElement3d,MFreeNode3d> elemVtx =
// out.traverseEdgesForward(base, nodeArray);
// elemVtx.setData(elem);
// }
// 
// return out;
// 
// }
// public static ArrayList<MFreeElement3d> findPairedElementsContaining(
// Point3d pnt, BVTree bvtree, double tol) {
// 
// ArrayList<MFreeElement3d> deps = new ArrayList<MFreeElement3d>();
// ArrayList<BVNode> bvNodes = new ArrayList<BVNode>(16);
// bvtree.intersectPoint(bvNodes, pnt);
// 
// if (bvNodes.size() == 0) {
// return deps;
// }
// 
// for (BVNode n : bvNodes) {
// Boundable[] elements = n.getElements();
// for (int i = 0; i < elements.length; i++) {
// MFreeElement3d elem = (MFreeElement3d)elements[i];
// 
// boolean isInside = true;
// for (MFreeNode3d node : elem.getNodes()) {
// if (!node.isInDomain(pnt, tol)) {
// isInside = false;
// break;
// }
// }
// if (isInside) {
// deps.add(elem);
// }
// }
// }
// return deps;
// }
private static MFreeNode3d[] findNodesContaining(Point3d pnt, BVTree bvtree, double tol) {
    DynamicArray<MFreeNode3d> deps = new DynamicArray<>(MFreeNode3d.class);
    ArrayList<BVNode> bvNodes = new ArrayList<BVNode>(16);
    bvtree.intersectPoint(bvNodes, pnt);
    if (bvNodes.size() == 0) {
        return deps.getArray();
    }
    for (BVNode n : bvNodes) {
        Boundable[] elements = n.getElements();
        for (int i = 0; i < elements.length; i++) {
            RestNode rnode = (RestNode) elements[i];
            MFreeNode3d node = rnode.getNode();
            if (node.isInDomain(pnt, tol)) {
                deps.add(node);
            }
        }
    }
    return deps.getArray();
}
Also used : DynamicArray(maspack.util.DynamicArray) BVNode(maspack.geometry.BVNode) ArrayList(java.util.ArrayList) Boundable(maspack.geometry.Boundable)

Example 7 with BVNode

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

the class MFreeModel3d method findNearestNode.

/**
 * Finds the nearest node to a specified point that is within
 * a specified maximum distance. If no node is within the
 * specified maximum distance, <code>null</code> is returned.
 *
 * @param pnt Point for which the nearest node should be located
 * @param maxDist Maximum distance that the node must be from the
 * point. If <code>maxDist</code> &lt; 0, then <code>null</code>
 * will be returned.
 * @return Nearest point within the prescribed distance, or <code>null</code>
 * if there is no such point
 */
public FemNode3d findNearestNode(Point3d pnt, double maxDist) {
    if (maxDist < 0) {
        return null;
    }
    BVTree bvtree = getElementBVTree();
    ArrayList<BVNode> nodes = new ArrayList<BVNode>();
    bvtree.intersectSphere(nodes, pnt, maxDist);
    FemNode3d nearest = null;
    double dist = 1 + 2 * maxDist;
    for (BVNode n : nodes) {
        Boundable[] elements = n.getElements();
        for (int i = 0; i < elements.length; i++) {
            FemElement3d e = (FemElement3d) elements[i];
            for (int k = 0; k < e.numNodes(); k++) {
                double d = e.getNodes()[k].getPosition().distance(pnt);
                if (d < dist && d <= maxDist) {
                    dist = d;
                    nearest = e.getNodes()[k];
                }
            }
        }
    }
    return nearest;
}
Also used : BVTree(maspack.geometry.BVTree) FemElement3d(artisynth.core.femmodels.FemElement3d) BVNode(maspack.geometry.BVNode) ArrayList(java.util.ArrayList) FemNode3d(artisynth.core.femmodels.FemNode3d) Boundable(maspack.geometry.Boundable) Point(artisynth.core.mechmodels.Point)

Example 8 with BVNode

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

the class MFreeMuscleModel method computeAverageFiberDirection.

public static boolean computeAverageFiberDirection(Vector3d dir, Point3d pos, double rad, PolylineMesh mesh) {
    BVTree bvh = mesh.getBVTree();
    ArrayList<BVNode> nodes = new ArrayList<BVNode>();
    Matrix3d cov = new Matrix3d();
    SVDecomposition3d svd = new SVDecomposition3d();
    Vector3d tmp = new Vector3d();
    Matrix3d tmp2 = new Matrix3d();
    bvh.intersectSphere(nodes, pos, rad);
    dir.setZero();
    int nsegs = 0;
    // for computing sign of direction vector
    Vector3d segmentSum = new Vector3d();
    // System.out.println("p=[");
    for (BVNode n : nodes) {
        Boundable[] elements = n.getElements();
        for (int i = 0; i < elements.length; i++) {
            LineSegment seg = (LineSegment) elements[i];
            seg = getSegmentInsideSphere(seg, pos, rad);
            if (seg != null) {
                tmp.sub(seg.myVtx1.pnt, seg.myVtx0.pnt);
                if (tmp.norm() >= 1e-8 * rad) {
                    // System.out.println(seg.myVtx0.getPosition() + " " +
                    // seg.myVtx1.getPosition());
                    nsegs++;
                    // prepare to average directions using SVD
                    computeCov(tmp2, tmp);
                    cov.add(tmp2);
                    segmentSum.add(tmp);
                }
            }
        }
    }
    if (nsegs > 0) {
        // we are technically including both +/- directions, so
        // we have twice the number of points
        cov.scale(2.0 / (2.0 * nsegs - 1));
        try {
            svd.factor(cov);
        } catch (Exception e) {
        // System.err.println(e.getMessage());
        }
        // principal components
        tmp2 = svd.getU();
        tmp2.getColumn(0, dir);
        dir.normalize();
        // most line segments
        if (dir.dot(segmentSum) < 0) {
            dir.scale(-1);
        }
        return true;
    } else {
        return false;
    }
}
Also used : BVTree(maspack.geometry.BVTree) Matrix3d(maspack.matrix.Matrix3d) SymmetricMatrix3d(maspack.matrix.SymmetricMatrix3d) Vector3d(maspack.matrix.Vector3d) BVNode(maspack.geometry.BVNode) ArrayList(java.util.ArrayList) SVDecomposition3d(maspack.matrix.SVDecomposition3d) Boundable(maspack.geometry.Boundable) IOException(java.io.IOException) LineSegment(maspack.geometry.LineSegment)

Example 9 with BVNode

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

the class MeshIntersectingProbe method findNextFace.

// does not add the vertex, goes in a loop around contour until we hit a face
private Face findNextFace(Point3d pnt, LinkedList<Point3d> contour, OBBTree obbt, Intersector2d ti, Vector3d vx, Vector3d vy, Point3d o, SplitStorage info) {
    Face face = null;
    Vector3d dir = new Vector3d();
    if (info.idx < contour.size() - 1) {
        dir.sub(contour.get(info.idx + 1), contour.get(info.idx));
    } else {
        dir.sub(contour.get(info.idx), contour.get(info.idx - 1));
    }
    dir.normalize();
    while (face == null && info.idx < contour.size() - 1) {
        Point3d pntNext = contour.get(info.idx + 1);
        Point2d pnt2d = Intersector2d.get2dCoordinate(pnt, vx, vy, o);
        Point2d pnt2dNext = Intersector2d.get2dCoordinate(pntNext, vx, vy, o);
        Vector2d diff2d = new Vector2d();
        ArrayList<BVNode> bvNodes = new ArrayList<BVNode>();
        // get close nodes
        obbt.intersectLineSegment(bvNodes, pnt, pntNext);
        double minDist = Double.MAX_VALUE;
        Point2d minPnt = null;
        for (BVNode node : bvNodes) {
            for (int i = 0; i < node.getNumElements(); i++) {
                Boundable ps = node.getElements()[i];
                if (ps instanceof Face) {
                    Face f = (Face) ps;
                    Vertex3d[] vtxs = f.getVertices();
                    Point2d p0 = Intersector2d.get2dCoordinate(vtxs[0].getWorldPoint(), vx, vy, o);
                    Point2d p1 = Intersector2d.get2dCoordinate(vtxs[1].getWorldPoint(), vx, vy, o);
                    Point2d p2 = Intersector2d.get2dCoordinate(vtxs[2].getWorldPoint(), vx, vy, o);
                    ArrayList<Point2d> points = new ArrayList<Point2d>();
                    ti.intersectTriangleLineSegment(p0, p1, p2, pnt2d, pnt2dNext, points);
                    // check points
                    for (Point2d p : points) {
                        diff2d.sub(p, pnt2d);
                        if (diff2d.norm() < minDist) {
                            face = f;
                            minDist = diff2d.norm();
                            minPnt = p;
                        }
                    }
                }
            }
        }
        if (face == null || minDist >= pnt2dNext.distance(pnt2d)) {
            face = null;
            // move to next point
            info.idx++;
            pnt = contour.get(info.idx);
            if (info.idx < contour.size() - 1) {
                dir.sub(contour.get(info.idx + 1), pnt);
            }
        } else {
            // snap to edge if within tolerance
            Point3d pos = Intersector2d.get3dCoordinate(minPnt, vx, vy, o);
            if (pos.distance(pnt) < ti.epsilon) {
                pos.set(pnt);
            }
            // check if we have to make a new vertex
            info.vtx = createOrGetVertex(pos, face.getVertices(), ti.epsilon);
        }
    }
    return face;
}
Also used : Vertex3d(maspack.geometry.Vertex3d) ArrayList(java.util.ArrayList) Vector2d(maspack.matrix.Vector2d) Vector3d(maspack.matrix.Vector3d) Point2d(maspack.matrix.Point2d) Point3d(maspack.matrix.Point3d) BVNode(maspack.geometry.BVNode) Boundable(maspack.geometry.Boundable) Face(maspack.geometry.Face)

Example 10 with BVNode

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

the class MeshIntersectingProbe method findFaces.

// finds faces near a points
private ArrayList<Face> findFaces(Point3d p, OBBTree obbt, Vector3d vx, Vector3d vy, Point3d o, double tol) {
    ArrayList<Face> faces = new ArrayList<Face>();
    ArrayList<BVNode> nodes = new ArrayList<BVNode>();
    obbt.intersectPoint(nodes, p);
    Point2d p2d = Intersector2d.get2dCoordinate(p, vx, vy, o);
    Point3d uvw = new Point3d();
    for (BVNode obbn : nodes) {
        for (int i = 0; i < obbn.getNumElements(); i++) {
            Boundable ps = obbn.getElements()[i];
            if (ps instanceof Face) {
                Face f = (Face) ps;
                Vertex3d[] vtxs = f.getVertices();
                Point2d p0 = Intersector2d.get2dCoordinate(vtxs[0].getWorldPoint(), vx, vy, o);
                Point2d p1 = Intersector2d.get2dCoordinate(vtxs[1].getWorldPoint(), vx, vy, o);
                Point2d p2 = Intersector2d.get2dCoordinate(vtxs[2].getWorldPoint(), vx, vy, o);
                Intersector2d.getBarycentric(p2d, p0, p1, p2, uvw);
                if (uvw.x > -tol && uvw.y > -tol && uvw.z > -tol) {
                    faces.add(f);
                }
            }
        }
    }
    return faces;
}
Also used : Vertex3d(maspack.geometry.Vertex3d) Point2d(maspack.matrix.Point2d) BVNode(maspack.geometry.BVNode) Point3d(maspack.matrix.Point3d) ArrayList(java.util.ArrayList) Boundable(maspack.geometry.Boundable) Face(maspack.geometry.Face)

Aggregations

BVNode (maspack.geometry.BVNode)12 ArrayList (java.util.ArrayList)11 Boundable (maspack.geometry.Boundable)11 BVTree (maspack.geometry.BVTree)6 Point (artisynth.core.mechmodels.Point)4 Vector3d (maspack.matrix.Vector3d)4 Face (maspack.geometry.Face)3 Point3d (maspack.matrix.Point3d)3 FemNode3d (artisynth.core.femmodels.FemNode3d)2 IOException (java.io.IOException)2 IntersectionPoint (maspack.collision.IntersectionPoint)2 AABBTree (maspack.geometry.AABBTree)2 LineSegment (maspack.geometry.LineSegment)2 Vertex3d (maspack.geometry.Vertex3d)2 Matrix3d (maspack.matrix.Matrix3d)2 Point2d (maspack.matrix.Point2d)2 SVDecomposition3d (maspack.matrix.SVDecomposition3d)2 SymmetricMatrix3d (maspack.matrix.SymmetricMatrix3d)2 FemElement3d (artisynth.core.femmodels.FemElement3d)1 PolygonalMesh (maspack.geometry.PolygonalMesh)1