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

Example 31 with MatrixNd

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

the class MLSShapeFunction method eval.

public double eval(MFreeNode3d node, MFreeNode3d[] nodes, Point3d pnt) {
    MatrixNd _mInv = new MatrixNd(nBasis, nBasis);
    computeMInv(_mInv, pnt, nodes);
    return eval(node, nodes, pnt, _mInv);
}

Example 32 with MatrixNd

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

the class MLSShapeFunction method computeDDM.

public void computeDDM(MatrixNd DM, int di, int dj, Point3d pnt, MFreeNode3d[] nodeList) {
    MatrixNd cc = new MatrixNd(nBasis, nBasis);
    DM.setSize(nBasis, nBasis);
    DM.setZero();
    int[] derivatives = new int[3];
    derivatives[di] = 1;
    derivatives[dj] = derivatives[dj] + 1;
    for (MFreeNode3d node : nodeList) {
        Point3d pos = node.getRestPosition();
        computePCorrelation(cc, pos.x, pos.y, pos.z);
        DM.scaledAdd(node.getWeightFunction().evalDerivative(pnt, derivatives), cc);
    }
}

Example 33 with MatrixNd

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

the class GMLSShapeFunction method computeM.

@Override
public void computeM(MatrixNd M, Point3d pnt, MFreeNode3d[] nodeList) {
    MatrixNd cc = new MatrixNd(nBasis, nBasis);
    M.setSize(nBasis, nBasis);
    M.setZero();
    for (MFreeNode3d node : nodeList) {
        Point3d pos = node.getRestPosition();
        double w = node.getWeight(pnt);
        for (int i = 0; i <= myOrder; i++) {
            for (int j = 0; j <= myOrder - i; j++) {
                for (int k = 0; k <= myOrder - i - j; k++) {
                    // account for symmetries
                    double d = dcoeff(i, j, k);
                    computeDPCorrelation(cc, pos.x, pos.y, pos.z, i, j, k);
                    M.scaledAdd(d * w, cc);
                }
            }
        }
    }
}

Example 34 with MatrixNd

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

the class CPD method coherent.

/**
 * Uses the coherent CPD algorithm to align a set of points
 * @param X reference input points
 * @param Y points to register
 * @param lambda weight factor for regularization term (> 0)
 * @param beta2 coherence factor, beta^2 (> 0)
 * @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 TY transformed points
 * @param sigma2Holder initial variance estimate
 */
public static Point3d[] coherent(Point3d[] X, Point3d[] Y, double lambda, double beta2, double w, double tol, int maxIters, Point3d[] TY, double[] sigma2Holder) {
    int M = Y.length;
    int N = X.length;
    if (TY == null) {
        TY = new Point3d[Y.length];
        for (int i = 0; i < TY.length; i++) {
            TY[i] = new Point3d();
        }
    }
    transformPoints(Y, TY);
    double sigma2;
    if (sigma2Holder == null || sigma2Holder[0] < 0) {
        sigma2 = computeVariance(X, TY, null, 1.0 / M);
    } else {
        sigma2 = sigma2Holder[0];
    }
    MatrixNd G = new MatrixNd(M, M);
    computeG(beta2, Y, G);
    LUDecomposition lu = new LUDecomposition(M);
    MatrixNd A = new MatrixNd(M, M);
    MatrixNd B = new MatrixNd(M, 3);
    MatrixNd PX = new MatrixNd(M, 3);
    MatrixNd W = new MatrixNd(M, 3);
    double[][] P = new double[M][N];
    double[] P1 = new double[M];
    double[] Pt1 = new double[N];
    double Np;
    double err = Double.MAX_VALUE;
    int iters = 0;
    double sigma2prev;
    // iterative part of algorithm
    while ((iters < maxIters) && (err > tol)) {
        // E-step
        Np = computeP(X, TY, sigma2, w, P, P1, Pt1);
        // M-step
        // set up AW = B, solve for W
        A.set(G);
        for (int i = 0; i < M; i++) {
            A.add(i, i, lambda * sigma2 / P1[i]);
        }
        computeCoherentRHS(P, P1, X, Y, PX, B);
        // solve
        // XXX may want to hook into Pardiso, set prev W as initial guess
        lu.factor(A);
        boolean nonSingular = lu.solve(W, B);
        if (!nonSingular) {
            System.out.println("CPD.coherent(...): Warning... matrix non-singular");
        }
        // update transformed points
        transformPoints(Y, G, W, TY);
        if (verbose) {
            System.out.println(TY[0]);
        }
        sigma2prev = sigma2;
        // update variance estimate
        double xPx = 0;
        double trPXTY = 0;
        double trTYPTY = 0;
        for (int m = 0; m < M; m++) {
            trPXTY += PX.get(m, 0) * TY[m].x + PX.get(m, 1) * TY[m].y + PX.get(m, 2) * TY[m].z;
            trTYPTY += P1[m] * TY[m].normSquared();
        }
        for (int n = 0; n < N; n++) {
            xPx += Pt1[n] * X[n].normSquared();
        }
        sigma2 = (xPx - 2 * trPXTY + trTYPTY) / (3 * Np);
        err = Math.abs(sigma2 - sigma2prev);
        iters++;
    }
    if (verbose) {
        System.out.println("Registration complete in " + iters + " iterations");
    }
    if (sigma2Holder != null) {
        sigma2Holder[0] = sigma2;
    }
    return TY;
}

Example 35 with MatrixNd

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

the class SphericalJointForceBound method globalToFrame.

/**
 * Computes the bound vectors into the joint's frame of reference.
 */
protected void globalToFrame() {
    NFrame = new MatrixNd(bounds.size(), 3);
    RigidTransform3d R = frame.getPose();
    for (int i = 0; i < bounds.size(); i++) {
        // Make a copy of vector at bounds(i)
        Vector3d vFrame = new Vector3d(bounds.get(i));
        // Transform it into the frame's coordinates
        vFrame.inverseTransform(R);
        // Add it to NFrame
        NFrame.setRow(i, vFrame);
    }
}