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

Example 46 with Vector3d

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

the class Face method computeCoords.

/**
 * Computes the barycentric coordinates of a point to the plane
 * @param pnt the point to consider
 * @param coords the returned coordinates
 */
public void computeCoords(Point3d pnt, Vector2d coords) {
    Vector3d v0 = new Vector3d();
    Vector3d v1 = new Vector3d();
    Vector3d v2 = new Vector3d();
    Point3d p0, p1, p2;
    HalfEdge he = he0;
    p0 = he.head.pnt;
    he = he.next;
    p1 = he.head.pnt;
    he = he.next;
    p2 = he.head.pnt;
    v0.sub(p1, p0);
    v1.sub(p2, p0);
    v2.sub(pnt, p0);
    double d00 = v0.dot(v0);
    double d01 = v0.dot(v1);
    double d11 = v1.dot(v1);
    double d20 = v2.dot(v0);
    double d21 = v2.dot(v1);
    double denom = d00 * d11 - d01 * d01;
    coords.x = (d11 * d20 - d01 * d21) / denom;
    coords.y = (d00 * d21 - d01 * d20) / denom;
}

Example 47 with Vector3d

use of maspack.matrix.Vector3d 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 48 with Vector3d

use of maspack.matrix.Vector3d 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 49 with Vector3d

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

the class BVFeatureQueryTest method nearestFaceTiming.

private void nearestFaceTiming(PolygonalMesh mesh) {
    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);
    X.setRandom();
    BVFeatureQuery query = new BVFeatureQuery();
    mesh.setMeshToWorld(X);
    obbTree.setBvhToWorld(X);
    aabbTree.setBvhToWorld(X);
    int numcases = 100;
    int timingcnt = 1000;
    Point3d pnt = new Point3d();
    Vector3d dir = new Vector3d();
    Point3d near = new Point3d();
    Vector2d coords = new Vector2d();
    Vector3d duv = new Vector3d();
    FunctionTimer obbFaceTimer = new FunctionTimer();
    FunctionTimer aabbFaceTimer = new FunctionTimer();
    // FunctionTimer oldFaceTimer = new FunctionTimer();
    FunctionTimer obbRayTimer = new FunctionTimer();
    FunctionTimer aabbRayTimer = new FunctionTimer();
    // FunctionTimer oldRayTimer = new FunctionTimer();
    TriangleIntersector ti = new TriangleIntersector();
    for (int i = 0; i < numcases; i++) {
        pnt.setRandom();
        pnt.scale(2 * diameter);
        pnt.add(center);
        pnt.transform(X, pnt);
        dir.setRandom();
        dir.normalize();
        obbFaceTimer.restart();
        for (int j = 0; j < timingcnt; j++) {
            query.nearestFaceToPoint(near, coords, obbTree, pnt);
        }
        obbFaceTimer.stop();
        aabbFaceTimer.restart();
        for (int j = 0; j < timingcnt; j++) {
            query.nearestFaceToPoint(near, coords, aabbTree, pnt);
        }
        aabbFaceTimer.stop();
        // oldFaceTimer.restart();
        // for (int j=0; j<timingcnt; j++) {
        // obbTree.nearestFace (pnt, null, near, coords, ti);
        // }
        // oldFaceTimer.stop();
        obbRayTimer.restart();
        for (int j = 0; j < timingcnt; j++) {
            query.nearestFaceAlongRay(near, duv, obbTree, center, dir);
        }
        obbRayTimer.stop();
        aabbRayTimer.restart();
        for (int j = 0; j < timingcnt; j++) {
            query.nearestFaceAlongRay(near, duv, aabbTree, center, dir);
        }
        aabbRayTimer.stop();
    // oldRayTimer.restart();
    // for (int j=0; j<timingcnt; j++) {
    // obbTree.intersect (center, dir, duv, ti);
    // }
    // oldRayTimer.stop();
    }
    int cnt = numcases * timingcnt;
    System.out.println("nearestFace with OBB: " + obbFaceTimer.result(cnt));
    System.out.println("nearestFace with AABB: " + aabbFaceTimer.result(cnt));
    // System.out.println (
    // "nearestFace with old OBB: " + oldFaceTimer.result(cnt));
    System.out.println("nearestRay with OBB: " + obbRayTimer.result(cnt));
    System.out.println("nearestRay with AABB: " + aabbRayTimer.result(cnt));
// System.out.println (
// "nearestRay with old OBB: " + oldRayTimer.result(cnt));
}

Example 50 with Vector3d

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

the class BVFeatureQueryTest method getNearestFacesToRay.

private ArrayList<NearestFaceInfo> getNearestFacesToRay(PolygonalMesh mesh, Point3d pnt, Vector3d dir, double tol) {
    Point3d lpnt = new Point3d();
    Vector3d ldir = new Vector3d();
    lpnt.inverseTransform(mesh.getMeshToWorld(), pnt);
    ldir.inverseTransform(mesh.getMeshToWorld(), dir);
    ArrayList<Face> faces = mesh.getFaces();
    NearestFaceInfo[] faceInfo = new NearestFaceInfo[faces.size()];
    TriangleIntersector intersector = new TriangleIntersector();
    double mind = Double.POSITIVE_INFINITY;
    for (int i = 0; i < faces.size(); i++) {
        NearestFaceInfo ninfo = new NearestFaceInfo();
        faceInfo[i] = ninfo;
        double d = ninfo.computeDistanceToRay(faces.get(i), lpnt, ldir, intersector);
        ninfo.myNear.transform(mesh.getMeshToWorld());
        if (d >= 0 && d < Double.POSITIVE_INFINITY) {
            if (d < mind) {
                mind = d;
            }
        }
    }
    ArrayList<NearestFaceInfo> nearestFaces = new ArrayList<NearestFaceInfo>();
    if (mind < Double.POSITIVE_INFINITY) {
        for (int i = 0; i < faces.size(); i++) {
            double d = faceInfo[i].myDist;
            if (d >= 0 && d < Double.POSITIVE_INFINITY) {
                if (Math.abs(d - mind) < tol) {
                    nearestFaces.add(faceInfo[i]);
                }
            }
        }
    }
    return nearestFaces;
}