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

Example 21 with RigidTransform3d

use of maspack.matrix.RigidTransform3d 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);
}

Example 22 with RigidTransform3d

use of maspack.matrix.RigidTransform3d 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 23 with RigidTransform3d

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

the class BVIntersectorTest method testMeshIntersection.

public void testMeshIntersection(PolygonalMesh mesh1) {
    Point3d center = new Point3d();
    double radius = RenderableUtils.getRadiusAndCenter(center, mesh1);
    PolygonalMesh mesh2 = new PolygonalMesh(mesh1);
    OBBTree OBBTree1 = new OBBTree(mesh1);
    OBBTree OBBTree2 = new OBBTree(mesh2);
    AABBTree AABBTree1 = new AABBTree(mesh1);
    AABBTree AABBTree2 = new AABBTree(mesh2);
    int numtrials = 100;
    for (int i = 0; i < numtrials; i++) {
        RigidTransform3d X2W = new RigidTransform3d();
        RigidTransform3d X21 = new RigidTransform3d();
        RigidTransform3d X1W = new RigidTransform3d();
        X1W.R.setRandom();
        X1W.p.setRandom();
        X1W.p.scale(radius);
        X21.R.setRandom();
        X21.p.setRandom();
        X21.p.scale(radius);
        X2W.mul(X1W, X21);
        testMeshIntersection(mesh1, OBBTree1, mesh2, OBBTree2, X1W, X2W);
        testMeshIntersection(mesh1, AABBTree1, mesh2, OBBTree2, X1W, X2W);
        testMeshIntersection(mesh1, OBBTree1, mesh2, AABBTree2, X1W, X2W);
        testMeshIntersection(mesh1, AABBTree1, mesh2, AABBTree2, X1W, X2W);
    }
}

Example 24 with RigidTransform3d

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

the class BVIntersectorTest method testMeshIntersection.

public void testMeshIntersection(PolygonalMesh mesh1, BVTree bvh1, PolygonalMesh mesh2, BVTree bvh2, RigidTransform3d X1W, RigidTransform3d X2W) {
    RigidTransform3d X21 = new RigidTransform3d();
    X21.mulInverseLeft(X1W, X2W);
    mesh1.setMeshToWorld(X1W);
    mesh2.setMeshToWorld(X2W);
    bvh1.setBvhToWorld(X1W);
    bvh2.setBvhToWorld(X2W);
    BVIntersector intersector = new BVIntersector();
    ArrayList<TriTriIntersection> intersections = new ArrayList<TriTriIntersection>();
    intersector.intersectMeshMesh(intersections, bvh1, bvh2);
    ArrayList<TriTriIntersection> bruteForceIntersections = intersectAllFaces(mesh1, mesh2);
    ArrayList<TriTriIntersection> leafIntersections = intersectLeafNodes(bvh1, bvh2, X21, intersector);
    ArrayList<TriTriIntersection> obbtreeIntersections = new ArrayList<TriTriIntersection>();
    // mesh1.getObbtree().intersectFully (
    // mesh2.getObbtree(), obbtreeIntersections, new TriangleIntersector());
    // // convert points of obbIntersections
    // for (TriangleTriangleIntersection isect : obbtreeIntersections) {
    // for (Point3d pnt : isect.points) {
    // pnt.inverseTransform (X1W);
    // }
    // }
    HashSet<TriTriIntersect> bruteForceSet = createTriTriIntersectionSet(bruteForceIntersections);
    HashSet<TriTriIntersect> leafSet = createTriTriIntersectionSet(leafIntersections);
    // HashSet<TriTriIntersect> obbtreeSet =
    // createTriTriIntersectionSet (obbtreeIntersections);
    HashSet<TriTriIntersect> intersectionSet = createTriTriIntersectionSet(intersections);
    double tol = (bvh1.getRadius() + bvh2.getRadius()) * EPS;
    checkTriTriIntersectionsEqual("intersections", intersectionSet, "brute force intersections", bruteForceSet, tol);
    checkTriTriIntersectionsEqual("intersections", intersectionSet, "leaf intersections", leafSet, tol);
// checkTriTriIntersectionsEqual (
// "intersections", intersectionSet,
// "obbtree intersections", obbtreeSet, tol);
}

Example 25 with RigidTransform3d

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

the class BVIntersectorTest method timeMeshIntersection.

public void timeMeshIntersection(PolygonalMesh mesh1) {
    Point3d center = new Point3d();
    double radius = RenderableUtils.getRadiusAndCenter(center, mesh1);
    PolygonalMesh mesh2 = new PolygonalMesh(mesh1);
    BVIntersector intersector = new BVIntersector();
    TriangleIntersector ti = new TriangleIntersector();
    ArrayList<TriTriIntersection> intersections = new ArrayList<TriTriIntersection>();
    OBBTree obbTree1 = new OBBTree(mesh1);
    OBBTree obbTree2 = new OBBTree(mesh2);
    AABBTree aabbTree1 = new AABBTree(mesh1);
    AABBTree aabbTree2 = new AABBTree(mesh2);
    FunctionTimer obbTimer = new FunctionTimer();
    FunctionTimer aabbTimer = new FunctionTimer();
    // FunctionTimer oldTimer = new FunctionTimer();
    RigidTransform3d X2W = new RigidTransform3d();
    RigidTransform3d X1W = new RigidTransform3d();
    int numcases = 50;
    int timingcnt = 500;
    for (int i = 0; i < numcases; i++) {
        X2W.R.setRandom();
        X2W.p.setRandom();
        X2W.p.scale(radius);
        X1W.R.setRandom();
        X1W.p.setRandom();
        X1W.p.scale(radius);
        obbTree1.setBvhToWorld(X1W);
        aabbTree1.setBvhToWorld(X1W);
        obbTree2.setBvhToWorld(X2W);
        aabbTree2.setBvhToWorld(X2W);
        mesh1.setMeshToWorld(X1W);
        mesh2.setMeshToWorld(X2W);
        obbTimer.restart();
        for (int j = 0; j < timingcnt; j++) {
            intersections.clear();
            intersector.intersectMeshMesh(intersections, obbTree1, obbTree2);
        }
        obbTimer.stop();
        aabbTimer.restart();
        for (int j = 0; j < timingcnt; j++) {
            intersections.clear();
            intersector.intersectMeshMesh(intersections, aabbTree1, aabbTree2);
        }
        aabbTimer.stop();
    // oldTimer.restart();
    // for (int j=0; j<timingcnt; j++) {
    // intersections.clear();
    // obbTree1.intersectFully (obbTree2, intersections, ti);
    // }
    // oldTimer.stop();
    }
    int cnt = numcases * timingcnt;
    System.out.println("mesh intersection with OBB: " + obbTimer.result(cnt));
    System.out.println("mesh intersection with AABB: " + aabbTimer.result(cnt));
// System.out.println (
// "mesh intersection with old OBB: " + oldTimer.result(cnt));
}