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

Example 1 with LUDecomposition

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

the class CRSolverTest method test.

public void test() {
    // use a small matrix size for now
    int size = 20;
    double eps = 1e-9;
    CRSolver solver = new CRSolver();
    LUDecomposition LU = new LUDecomposition();
    MatrixNd M = new MatrixNd(size, size);
    VectorNd x = new VectorNd(size);
    VectorNd b = new VectorNd(size);
    VectorNd xcheck = new VectorNd(size);
    // identity matrix to try as trivial pre-conditioner
    MatrixNd I = new MatrixNd(size, size);
    I.setIdentity();
    // inverse matrix to try as pre-conditioner
    MatrixNd Minv = new MatrixNd(size, size);
    for (int i = 0; i < 10; i++) {
        int numIter;
        M.setRandom(-0.5, 0.5, randGen);
        // make sure matrix is SPD
        M.mulTranspose(M);
        b.setRandom(-0.5, 0.5, randGen);
        x.setZero();
        if (!solver.solve(x, M, b, eps, size * size)) {
            throw new TestException("No convergence: error=" + solver.getRelativeResidual());
        }
        numIter = solver.getNumIterations();
        LU.factor(M);
        LU.solve(xcheck, b);
        if (!xcheck.epsilonEquals(x, x.norm() * eps)) {
            throw new TestException("Solver gave wrong answer. Expected\n" + xcheck.toString("%8.3f") + "\nGot\n" + x.toString("%8.3f"));
        }
        // System.out.println ("condEst=" + LU.conditionEstimate (M));
        SparseMatrixNd Sc = createBlockDiagonal(10);
        VectorNd bc = new VectorNd(Sc.colSize());
        VectorNd xc = new VectorNd(Sc.colSize());
        bc.setRandom(-0.5, 0.5, randGen);
        if (!solver.solve(xc, Sc, bc, eps, 10 * xc.size())) {
            throw new TestException("No convergence, constraint problem: error=" + solver.getRelativeResidual());
        }
    // x.setZero();
    // if(!solver.solve(x, M, b, eps, size * size, I))
    // {
    // throw new TestException("No convergence, identity preconditioner:
    // error=" + solver.getRelativeResidual());
    // }
    // 
    // if(numIter != solver.getNumIterations())
    // {
    // throw new TestException("Different iteration count with identity
    // preconditioner: " + solver.getNumIterations() + " vs. " + numIter);
    // }
    // 
    // LU.inverse(Minv);
    // x.setZero();
    // if(!solver.solve(x, M, b, eps, size * size, Minv))
    // {
    // throw new TestException("No convergence, inverse preconditioner:
    // error=" + solver.getRelativeResidual());
    // }
    // 
    // if(solver.getNumIterations() > 2)
    // {
    // throw new TestException("Num iterations > 2 with inverse
    // preconditioner");
    // }
    // System.out.println ("num iter=" + solver.getNumIterations());
    // try
    // { PrintWriter pw =
    // new PrintWriter (new FileWriter ("mat.m"));
    // pw.println ("M=[\n" + Sc.toString ("%12.9f") + "]");
    // pw.println ("b=[\n" + bc.toString ("%12.9f") + "]");
    // pw.println ("x=[\n" + xc.toString ("%12.9f") + "]");
    // pw.close();
    // }
    // catch (Exception e)
    // {
    // }
    }
}

Example 2 with LUDecomposition

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

the class MFreeElement3d method getNaturalCoordinates.

/**
 * Given point p, get its natural coordinates with respect to this element.
 * Returns true if the algorithm converges, false if a maximum number of
 * iterations is reached. Uses a modified Newton's method to solve the
 * equations. The <code>coords</code> argument that returned the coordinates is
 * used, on input, to supply an initial guess of the coordinates.
 * Zero is generally a safe guess.
 *
 * @param coords
 * Outputs the natural coordinates, and supplies (on input) an initial
 * guess as to their position.
 * @param pnt
 * A given point (in world coords)
 * @param maxIters
 * Maximum number of Newton iterations
 * @param N
 * Resulting shape functionvalues
 * @return the number of iterations required for convergence, or
 * -1 if the calculation did not converge.
 */
public int getNaturalCoordinates(Point3d coords, Point3d pnt, int maxIters, VectorNd N) {
    Vector3d res = new Point3d();
    int i;
    double tol = ((MFreeNode3d) myNodes[0]).getInfluenceRadius() * 1e-12;
    if (tol <= 0) {
        tol = 1e-12;
    }
    if (N == null) {
        N = new VectorNd(numNodes());
    } else {
        N.setSize(numNodes());
    }
    // if (!coordsAreInside(coords)) {
    // return -1;
    // }
    myShapeFunction.update(coords, (MFreeNode3d[]) myNodes);
    computeNaturalCoordsResidual(res, coords, pnt, N);
    double prn = res.norm();
    // System.out.println ("res=" + prn);
    if (prn < tol) {
        // already have the right answer
        return 0;
    }
    LUDecomposition LUD = new LUDecomposition();
    Vector3d prevCoords = new Vector3d();
    Vector3d dNds = new Vector3d();
    Matrix3d dxds = new Matrix3d();
    Vector3d del = new Point3d();
    /*
       * solve using Newton's method.
       */
    for (i = 0; i < maxIters; i++) {
        // compute the Jacobian dx/ds for the current guess
        dxds.setZero();
        for (int k = 0; k < numNodes(); k++) {
            myShapeFunction.evalDerivative(k, dNds);
            dxds.addOuterProduct(myNodes[k].getPosition(), dNds);
        }
        LUD.factor(dxds);
        double cond = LUD.conditionEstimate(dxds);
        if (cond > 1e10)
            System.err.println("Warning: condition number for solving natural coordinates is " + cond);
        // solve Jacobian to obtain an update for the coords
        LUD.solve(del, res);
        prevCoords.set(coords);
        coords.sub(del);
        // if (!coordsAreInside(coords)) {
        // return -1;
        // }
        myShapeFunction.update(coords, (MFreeNode3d[]) myNodes);
        computeNaturalCoordsResidual(res, coords, pnt, N);
        double rn = res.norm();
        // If the residual norm is within tolerance, we have converged.
        if (rn < tol) {
            // System.out.println ("2 res=" + rn);
            return i + 1;
        }
        if (rn > prn) {
            // it may be that "coords + del" is a worse solution.  Let's make
            // sure we go the correct way binary search suitable alpha in [0 1]
            double eps = 1e-12;
            // and keep cutting the step size in half until the residual starts
            // dropping again
            double alpha = 0.5;
            while (alpha > eps && rn > prn) {
                coords.scaledAdd(-alpha, del, prevCoords);
                if (!coordsAreInside(coords)) {
                    return -1;
                }
                myShapeFunction.update(coords, (MFreeNode3d[]) myNodes);
                computeNaturalCoordsResidual(res, coords, pnt, N);
                rn = res.norm();
                alpha *= 0.5;
            // System.out.println ("  alpha=" + alpha + " rn=" + rn);
            }
            // System.out.println (" alpha=" + alpha + " rn=" + rn + " prn=" + prn);
            if (alpha < eps) {
                // failed
                return -1;
            }
        }
        prn = rn;
    }
    // failed
    return -1;
}

Example 3 with LUDecomposition

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

the class CGSolverTest method test.

public void test() {
    // use a small matrix size for now
    int size = 20;
    double eps = 1e-9;
    CGSolver solver = new CGSolver();
    LUDecomposition LU = new LUDecomposition();
    MatrixNd M = new MatrixNd(size, size);
    VectorNd x = new VectorNd(size);
    VectorNd b = new VectorNd(size);
    VectorNd xcheck = new VectorNd(size);
    // identity matrix to try as trivial pre-conditioner
    MatrixNd I = new MatrixNd(size, size);
    I.setIdentity();
    // inverse matrix to try as pre-conditioner
    MatrixNd Minv = new MatrixNd(size, size);
    for (int i = 0; i < 10; i++) {
        int numIter;
        M.setRandom(-0.5, 0.5, randGen);
        // make sure matrix is SPD
        M.mulTranspose(M);
        b.setRandom(-0.5, 0.5, randGen);
        x.setZero();
        if (!solver.solve(x, M, b, eps, size * size)) {
            throw new TestException("No convergence: error=" + solver.getRelativeResidual());
        }
        numIter = solver.getNumIterations();
        LU.factor(M);
        LU.solve(xcheck, b);
        if (!xcheck.epsilonEquals(x, x.norm() * eps)) {
            throw new TestException("Solver gave wrong answer. Expected\n" + xcheck.toString("%8.3f") + "\nGot\n" + x.toString("%8.3f"));
        }
        // System.out.println ("condEst=" + LU.conditionEstimate (M));
        SparseMatrixNd Sc = createBlockDiagonal(10);
        VectorNd bc = new VectorNd(Sc.colSize());
        VectorNd xc = new VectorNd(Sc.colSize());
        bc.setRandom(-0.5, 0.5, randGen);
        if (!solver.solve(xc, Sc, bc, eps, 10 * xc.size())) {
            throw new TestException("No convergence, constraint problem: error=" + solver.getRelativeResidual());
        }
        x.setZero();
        if (!solver.solve(x, M, b, eps, size * size, I)) {
            throw new TestException("No convergence, identity preconditioner: error=" + solver.getRelativeResidual());
        }
        if (numIter != solver.getNumIterations()) {
            throw new TestException("Different iteration count with identity preconditioner: " + solver.getNumIterations() + " vs. " + numIter);
        }
        LU.inverse(Minv);
        x.setZero();
        if (!solver.solve(x, M, b, eps, size * size, Minv)) {
            throw new TestException("No convergence, inverse preconditioner: error=" + solver.getRelativeResidual());
        }
        if (solver.getNumIterations() > 2) {
            throw new TestException("Num iterations > 2 with inverse preconditioner");
        }
    // System.out.println ("num iter=" + solver.getNumIterations());
    // try
    // { PrintWriter pw =
    // new PrintWriter (new FileWriter ("mat.m"));
    // pw.println ("M=[\n" + Sc.toString ("%12.9f") + "]");
    // pw.println ("b=[\n" + bc.toString ("%12.9f") + "]");
    // pw.println ("x=[\n" + xc.toString ("%12.9f") + "]");
    // pw.close();
    // }
    // catch (Exception e)
    // {
    // }
    }
}

Example 4 with LUDecomposition

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