Examples with Point3d maspack.matrix.Point3d used on opensource projects

Example 41 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class BVFeatureQueryTest method nearestVertexAndEdge.

private void nearestVertexAndEdge(MeshBase mesh, BVTree bvh, RigidTransform3d X, Point3d center, double diameter) {
    BVFeatureQuery query = new BVFeatureQuery();
    mesh.setMeshToWorld(X);
    bvh.setBvhToWorld(X);
    int testcnt = 1000;
    Point3d pnt = new Point3d();
    Vector3d dir = new Vector3d();
    NearestEdgeInfo einfo = new NearestEdgeInfo();
    double tol = EPS * (center.norm() + diameter);
    for (int i = 0; i < testcnt; i++) {
        pnt.setRandom();
        pnt.scale(2 * diameter);
        pnt.add(center);
        pnt.transform(X, pnt);
        dir.setRandom();
        dir.normalize();
        ArrayList<Vertex3d> nearestVertices = getNearestVerticesToPoint(mesh, pnt);
        Vertex3d vtx = query.nearestVertexToPoint(bvh, pnt);
        if (!nearestVertices.contains(vtx)) {
            System.out.println("Computed vertex:");
            System.out.println("Vertex " + vtx.getIndex());
            System.out.println("Possible vertices:");
            for (int k = 0; k < nearestVertices.size(); k++) {
                System.out.println("Vertex " + nearestVertices.get(i).getIndex());
            }
            throw new TestException("Vertex " + vtx.getIndex() + " computed as nearest does not match brute force calculation");
        }
        ArrayList<NearestEdgeInfo> nearestEdges = getNearestEdgesToPoint(mesh, pnt, tol);
        einfo.myEdge = query.nearestEdgeToPoint(einfo.myNear, einfo.myCoord, bvh, pnt);
        if (einfo.myEdge == null) {
            if (nearestEdges.size() > 0) {
                throw new TestException("No nearest edge found, but brute force discovered " + nearestEdges.size());
            }
        } else {
            if (!einfo.edgeFaceIsContained(nearestEdges, tol)) {
                System.out.println("Computed face:");
                System.out.println("Edge " + einfo.edgeString() + " near: " + einfo.myNear.toString("%10.6f") + ", coord " + einfo.myCoord.value + ", d=" + einfo.myDist);
                System.out.println("Possible edges:");
                for (int k = 0; k < nearestEdges.size(); k++) {
                    NearestEdgeInfo ei = nearestEdges.get(k);
                    System.out.println("Edge " + ei.edgeString() + " near: " + ei.myNear.toString("%10.6f") + ", coords " + ei.myCoord.value + ", d=" + ei.myDist);
                }
                throw new TestException("Edge " + einfo.edgeString() + " computed as nearest does not match brute force calculation");
            }
        }
    }
}

Example 42 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class BVFeatureQueryTest method nearestFaceTest.

// private void getNearestFaceFromObbtree (
// NearestFaceInfo ninfo, OBBTree obbTree, Point3d pnt) {
// TriangleIntersector ti = new TriangleIntersector();
// ninfo.myFace = obbTree.nearestFace (
// pnt, null, ninfo.myNear, ninfo.myCoords, ti);
// }
// private void getNearestFaceIntersectedByRayFromObbtree (
// NearestFaceInfo ninfo, OBBTree obbTree, Point3d pnt, Vector3d dir) {
// //TriangleIntersector ti = new TriangleIntersector();
// BVFeatureQuery query = new BVFeatureQuery();
// Vector3d coords = new Vector3d();
// //ninfo.myFace = obbTree.intersect (pnt, dir, coords, ti);
// ninfo.myFace = query.nearestFaceAlongRay (
// null, coords, obbTree, pnt, dir);
// if (ninfo.myFace != null) {
// ninfo.myDist = coords.x;
// ninfo.myCoords.x = coords.y;
// ninfo.myCoords.y = coords.z;
// ninfo.myFace.computePoint (ninfo.myNear, ninfo.myCoords);
// }
// }
private void nearestFaceTest(PolygonalMesh mesh, BVTree bvh, RigidTransform3d X, Point3d center, double diameter) {
    BVFeatureQuery query = new BVFeatureQuery();
    mesh.setMeshToWorld(X);
    bvh.setBvhToWorld(X);
    int testcnt = 1000;
    Point3d pnt = new Point3d();
    Vector3d dir = new Vector3d();
    NearestFaceInfo ninfo = new NearestFaceInfo();
    NearestFaceInfo rinfo = new NearestFaceInfo();
    double tol = EPS * (center.norm() + diameter);
    for (int i = 0; i < testcnt; i++) {
        pnt.setRandom();
        pnt.scale(2 * diameter);
        pnt.add(center);
        pnt.transform(X, pnt);
        dir.setRandom();
        dir.normalize();
        ArrayList<NearestFaceInfo> nearestFaces = getNearestFacesToPoint(mesh, pnt, tol);
        ninfo.myFace = query.nearestFaceToPoint(ninfo.myNear, ninfo.myCoords, bvh, pnt);
        if (!ninfo.pointFaceIsContained(nearestFaces, tol)) {
            System.out.println("Computed face:");
            System.out.println("Face " + ninfo.myFace.getIndex() + " near: " + ninfo.myNear.toString("%10.6f") + ", coords " + ninfo.myCoords);
            System.out.println("Possible faces:");
            for (int k = 0; k < nearestFaces.size(); k++) {
                NearestFaceInfo ni = nearestFaces.get(k);
                System.out.println("Face " + ni.myFace.getIndex() + " near: " + ni.myNear.toString("%10.6f"));
            }
            throw new TestException("Face " + ninfo.myFace.getIndex() + " computed as nearest does not match brute force calculation");
        }
        ArrayList<NearestFaceInfo> nearestRayFaces = getNearestFacesToRay(mesh, pnt, dir, tol);
        rinfo.myFace = query.nearestFaceAlongRay(rinfo.myNear, rinfo.myCoords3, bvh, pnt, dir);
        if (rinfo.myFace == null) {
            if (nearestRayFaces.size() != 0) {
                System.out.println("Possible faces:");
                for (int k = 0; k < nearestRayFaces.size(); k++) {
                    NearestFaceInfo ni = nearestRayFaces.get(k);
                    System.out.printf(" Face %d dist=%g near=%s\n", ni.myFace.getIndex(), ni.myDist, ni.myNear.toString("%10.6f"));
                }
                throw new TestException("nearestFaceIntersectedByRay returned null; " + "brute force did not");
            }
        } else if (!rinfo.lineFaceIsContained(nearestRayFaces, tol)) {
            System.out.println("Computed face:");
            System.out.println("Face " + rinfo.myFace.getIndex() + " near: " + rinfo.myNear.toString("%10.6f") + ", coords " + rinfo.myCoords3);
            System.out.println("Possible faces:");
            for (int k = 0; k < nearestRayFaces.size(); k++) {
                NearestFaceInfo ni = nearestRayFaces.get(k);
                System.out.println("Face " + ni.myFace.getIndex() + " near: " + ni.myNear.toString("%10.6f") + ", coords " + ni.myCoords3);
            }
            throw new TestException("Face " + rinfo.myFace.getIndex() + " computed as nearest does not match brute force calculation");
        }
    // NearestFaceInfo oinfo = new NearestFaceInfo();
    // if (bvh instanceof OBBTree) {
    // OBBTree obbTree = (OBBTree)bvh;
    // getNearestFaceFromObbtree (oinfo, obbTree, pnt);
    // if (oinfo.myFace != ninfo.myFace) {
    // if (!ninfo.myNear.epsilonEquals (oinfo.myNear, tol)) {
    // System.out.println ("Query and OBBtree results differ:");
    // System.out.println (
    // "Face "+ninfo.myFace.getIndex()+
    // ", near="+ ninfo.myNear.toString ("%10.6f"));
    // System.out.println (
    // "Face "+oinfo.myFace.getIndex()+
    // ", near="+ oinfo.myNear.toString ("%10.6f"));
    // }
    // }
    // if (!oinfo.pointFaceIsContained (nearestFaces, tol)) {
    // System.out.println ("Computed face from Obbtree:");
    // System.out.println (
    // "Face "+ oinfo.myFace.getIndex() + " near: " +
    // oinfo.myNear.toString ("%10.6f"));
    // System.out.println ("Possible faces:");
    // for (int k=0; k<nearestFaces.size(); k++) {
    // NearestFaceInfo ni = nearestFaces.get(k);
    // System.out.println (
    // "Face "+ ni.myFace.getIndex() + " near: " +
    // ni.myNear.toString ("%10.6f"));
    // }
    // throw new TestException (
    // "Face "+oinfo.myFace.getIndex()+
    // " computed as nearest does not match brute force calculation");
    // }
    // getNearestFaceIntersectedByRayFromObbtree (oinfo, obbTree, pnt, dir);
    // if ((oinfo.myFace==null) != (rinfo.myFace==null)) {
    // if (oinfo.myFace!=null) {
    // System.out.println ("obbTree dist=" + oinfo.myDist);
    // }
    // else if (rinfo.myFace!=null) {
    // System.out.println ("query dist=" + rinfo.myDist);
    // }
    // throw new TestException (
    // "Different results computing nearest face to ray via query "+
    // "(face="+rinfo.myFace+") and OBBTree (face="+oinfo.myFace+")");
    // }
    // if (rinfo.myFace != oinfo.myFace) {
    // if (!rinfo.myNear.epsilonEquals (oinfo.myNear, tol)) {
    // System.out.println ("Query and OBBtree results differ:");
    // System.out.println (
    // "Face "+rinfo.myFace.getIndex()+
    // ", near="+ rinfo.myNear.toString ("%10.6f"));
    // System.out.println (
    // "Face "+oinfo.myFace.getIndex()+
    // ", near="+ oinfo.myNear.toString ("%10.6f"));
    // }
    // }
    // }
    }
}

Example 43 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class BVFeatureQueryTest method nearestVertexAndEdgeTest.

private void nearestVertexAndEdgeTest(MeshBase mesh) {
    // NearestFeatureQuery query = new NearestFeatureQuery();
    double inf = Double.POSITIVE_INFINITY;
    Point3d max = new Point3d(-inf, -inf, -inf);
    Point3d min = new Point3d(inf, inf, inf);
    Point3d center = new Point3d();
    Point3d widths = new Point3d();
    mesh.updateBounds(min, max);
    center.add(min, max);
    center.scale(0.5);
    widths.sub(max, min);
    widths.scale(0.5);
    double diameter = max.distance(min);
    RigidTransform3d X = new RigidTransform3d();
    OBBTree obbTree = new OBBTree(mesh, 2);
    AABBTree aabbTree = new AABBTree(mesh, 2, 0.001 * diameter);
    nearestVertexAndEdge(mesh, obbTree, X, center, diameter);
    nearestVertexAndEdge(mesh, aabbTree, X, center, diameter);
    X.setRandom();
    nearestVertexAndEdge(mesh, obbTree, X, center, diameter);
    // aabbTree.print();
    nearestVertexAndEdge(mesh, aabbTree, X, center, diameter);
    X.setRandom();
    nearestVertexAndEdge(mesh, obbTree, X, center, diameter);
    nearestVertexAndEdge(mesh, aabbTree, X, center, diameter);
    X.setRandom();
    nearestVertexAndEdge(mesh, obbTree, X, center, diameter);
    nearestVertexAndEdge(mesh, aabbTree, X, center, diameter);
}

Example 44 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class BVFeatureQueryTest method getNearestEdgesToPoint.

private ArrayList<NearestEdgeInfo> getNearestEdgesToPoint(MeshBase mesh, Point3d pnt, double tol) {
    Point3d loc = new Point3d();
    loc.inverseTransform(mesh.getMeshToWorld(), pnt);
    NearestEdgeInfo[] edgeInfo;
    double mind = Double.POSITIVE_INFINITY;
    if (mesh instanceof PolygonalMesh) {
        ArrayList<Face> faces = ((PolygonalMesh) mesh).getFaces();
        int numEdges = 0;
        for (int i = 0; i < faces.size(); i++) {
            numEdges += faces.get(i).numEdges();
        }
        edgeInfo = new NearestEdgeInfo[numEdges];
        int k = 0;
        for (int i = 0; i < faces.size(); i++) {
            Face face = faces.get(i);
            HalfEdge he0 = face.firstHalfEdge();
            HalfEdge he = he0;
            do {
                NearestEdgeInfo einfo = new NearestEdgeInfo();
                double d = einfo.computeDistanceToPoint(he, loc);
                einfo.myNear.transform(mesh.getMeshToWorld());
                if (d < mind) {
                    mind = d;
                }
                edgeInfo[k++] = einfo;
                he = he.getNext();
            } while (he != he0);
        }
    } else if (mesh instanceof PolylineMesh) {
        int numEdges = 0;
        ArrayList<Polyline> lines = ((PolylineMesh) mesh).getLines();
        for (Polyline line : lines) {
            numEdges += line.numVertices() - 1;
        }
        edgeInfo = new NearestEdgeInfo[numEdges];
        int k = 0;
        for (int i = 0; i < lines.size(); i++) {
            Polyline line = lines.get(i);
            Vertex3d[] vtxs = line.getVertices();
            for (int j = 0; j < line.numVertices() - 1; j++) {
                NearestEdgeInfo einfo = new NearestEdgeInfo();
                double d = einfo.computeDistanceToPoint(new LineSegment(vtxs[j], vtxs[j + 1]), loc);
                einfo.myNear.transform(mesh.getMeshToWorld());
                if (d < mind) {
                    mind = d;
                }
                edgeInfo[k++] = einfo;
            }
        }
    } else {
        // nothing - no edges will be FOUND
        edgeInfo = new NearestEdgeInfo[0];
    }
    ArrayList<NearestEdgeInfo> nearestEdges = new ArrayList<NearestEdgeInfo>();
    for (int i = 0; i < edgeInfo.length; i++) {
        if (Math.abs(edgeInfo[i].myDist - mind) < tol) {
            nearestEdges.add(edgeInfo[i]);
        }
    }
    return nearestEdges;
}

Example 45 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class BVFeatureQueryTest method nearestFaceTest.

private void nearestFaceTest(PolygonalMesh mesh) {
    // //NearestFeatureQuery query = new NearestFeatureQuery();
    // double inf = Double.POSITIVE_INFINITY;
    // Point3d max = new Point3d (-inf, -inf, -inf);
    // Point3d min = new Point3d ( inf,  inf,  inf);
    // Point3d center = new Point3d();
    // Point3d widths = new Point3d();
    // mesh.updateBounds (min, max);
    // center.add (min, max);
    // center.scale (0.5);
    // widths.sub (max, min);
    // widths.scale (0.5);
    // double diameter = max.distance (min);
    RigidTransform3d X = new RigidTransform3d();
    OBBTree obbTree = new OBBTree(mesh, 2);
    AABBTree aabbTree = new AABBTree(mesh);
    Point3d center = new Point3d();
    double diameter = 2 * aabbTree.getRadius();
    aabbTree.getCenter(center);
    nearestFaceTest(mesh, obbTree, X, center, diameter);
    nearestFaceTest(mesh, aabbTree, X, center, diameter);
    X.setRandom();
    nearestFaceTest(mesh, obbTree, X, center, diameter);
    nearestFaceTest(mesh, aabbTree, X, center, diameter);
    X.setRandom();
    nearestFaceTest(mesh, obbTree, X, center, diameter);
    nearestFaceTest(mesh, aabbTree, X, center, diameter);
    X.setRandom();
    nearestFaceTest(mesh, obbTree, X, center, diameter);
    nearestFaceTest(mesh, aabbTree, X, center, diameter);
}