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

Example 51 with Point3d

use of maspack.matrix.Point3d 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 52 with Point3d

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

Example 53 with Point3d

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

the class BVIntersectorTest method testPlaneIntersection.

public void testPlaneIntersection(PolygonalMesh mesh, BVTree bvh, RigidTransform3d XMW) {
    Point3d center = new Point3d();
    double radius = bvh.getRadius();
    bvh.getCenter(center);
    mesh.setMeshToWorld(XMW);
    bvh.setBvhToWorld(XMW);
    Random rand = RandomGenerator.get();
    int numtrials = 100;
    for (int i = 0; i < numtrials; i++) {
        Plane plane = new Plane();
        plane.normal.setRandom();
        plane.offset = 3 * radius * (rand.nextDouble() - 0.5);
        testPlaneIntersection(mesh, bvh, plane);
    }
}

Example 54 with Point3d

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

the class CPD method computeAD.

/**
 * Computes the A matrix used in rigid affine, A = X'*P'*Y
 * as well as the denominator for computing scale:
 * d = trace(Y'*diag(P1)*Y)
 *
 * @param X input points
 * @param mx mean of input
 * @param P probability matrix
 * @param P1 P*ones(N,1)
 * @param Y transforming points
 * @param my mean of transforming
 * @param A output A matrix
 * @param tr trace values, <br>
 *        tr[0] = trace( (X-mx)'*diag(P'1)(X-mx) )<br>
 *        tr[1] = trace( (Y-my)'*diag(P1)(Y-my) )
 * @return denominator for computing scale
 */
private static double computeAD(Point3d[] X, Point3d mx, double[][] P, double[] P1, double[] Pt1, Point3d[] Y, Point3d my, Matrix3d A, double[] tr) {
    int N = X.length;
    int M = Y.length;
    Point3d x = new Point3d();
    Point3d y = new Point3d();
    A.setZero();
    double xPx = 0;
    double yPy = 0;
    y.sub(Y[0], my);
    for (int n = 0; n < N; n++) {
        x.sub(X[n], mx);
        addScaledOuterProduct(A, P[0][n], x, y);
        xPx += Pt1[n] * x.normSquared();
    }
    yPy += P1[0] * y.normSquared();
    for (int m = 1; m < M; m++) {
        y.sub(Y[m], my);
        for (int n = 0; n < N; n++) {
            x.sub(X[n], mx);
            addScaledOuterProduct(A, P[m][n], x, y);
        }
        yPy += P1[m] * y.normSquared();
    }
    tr[0] = xPx;
    tr[1] = yPy;
    return yPy;
}

Example 55 with Point3d

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

the class CPD method rigid.

/**
 * Uses the rigid CPD algorithm to align two meshes
 * @param meshRef reference mesh
 * @param meshReg mesh to register
 * @param w weight, accounting to noise (w=0 --> no noise)
 * @param tol will iterative until objective function changes by less than this
 * @param maxIters maximum number of iterations
 * @param allowScaling whether or not to allow scaling
 * @return the scaled rigid transform for registration
 */
public static ScaledRigidTransform3d rigid(PolygonalMesh meshRef, PolygonalMesh meshReg, double w, double tol, int maxIters, boolean allowScaling) {
    int N = meshRef.numVertices();
    int M = meshReg.numVertices();
    Point3d[] x = new Point3d[N];
    Point3d[] y = new Point3d[M];
    Point3d[] match = new Point3d[M];
    for (int n = 0; n < N; n++) {
        x[n] = meshRef.getVertices().get(n).getWorldPoint();
    }
    for (int m = 0; m < M; m++) {
        y[m] = meshReg.getVertices().get(m).getWorldPoint();
        match[m] = new Point3d();
    }
    return rigid(x, y, w, tol, maxIters, allowScaling, match, null, null);
}