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

Example 1 with MatrixNd

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

the class MatlabInterfaceTest method testGetMatrix.

public void testGetMatrix() throws IOException {
    String name = "X";
    // String str = name + " = [3, 4; 9, 6; 5, 2];";
    // double[][] mat = new double[3][2];
    // String str = name + " = gallery(5);";
    // double[][] mat = new double[5][5];
    String str = name + " = [gallery(3); gallery(3)*pi];";
    double[][] mat = new double[6][3];
    System.out.println("Send cmd: " + str);
    matlab.evalString(str);
    matlab.getMatrix("X", mat);
    MatrixNd M = new MatrixNd(mat);
    System.out.println("M = \n" + M.toString());
}

Example 2 with MatrixNd

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

the class MatlabInterfaceTest method testSetMatrix.

public void testSetMatrix() throws IOException {
    double[][] mat = new double[][] { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 } };
    MatrixNd M = new MatrixNd(mat);
    System.out.println("Java: M = \n" + M.toString());
    matlab.putMatrix("M", mat);
    matlab.evalString("X = M*2");
    // Get output
    System.out.println(matlab.getOutputString(500));
}

Example 3 with MatrixNd

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

the class LemkeContactSolver method checkPivotComputations.

private void checkPivotComputations(ContactVariable drive) {
    MatrixNd Mv = new MatrixNd(numActiveVars, numActiveVars + 1);
    MatrixNd MvCheck = new MatrixNd(numActiveVars, numActiveVars + 1);
    // first, compute Mv using the normal routines of this class
    double[] mvcol = new double[numActiveVars];
    int asize = 0;
    for (int i = 0; i < numActiveVars; i++) {
        int vari = activeToVar[i];
        if (zVars[vari].isBasic) {
            computeMvCol(mvcol, wVars[vari]);
            System.out.println(zVars[vari].getName());
            asize++;
        } else {
            computeMvCol(mvcol, zVars[vari]);
        }
        Mv.setColumn(i, mvcol);
    }
    if (z0Var.isBasic) {
        computeMvCol(mvcol, wzVar);
        Mv.setColumn(numActiveVars, mvcol);
        asize++;
    } else {
        computeMvCol(mvcol, z0Var);
        Mv.setColumn(numActiveVars, mvcol);
    }
    if (asize == 0) {
        return;
    }
    // Next, compute Mv using a general method
    int bsize = numActiveVars - asize;
    MatrixNd Maa = new MatrixNd(asize, asize);
    MatrixNd Mba = new MatrixNd(bsize, asize);
    MatrixNd Mab = new MatrixNd(asize, bsize + 1);
    MatrixNd Mbb = new MatrixNd(bsize, bsize + 1);
    MatrixNd Mvaa = new MatrixNd(asize, asize);
    MatrixNd Mvba = new MatrixNd(bsize, asize);
    MatrixNd Mvab = new MatrixNd(asize, bsize + 1);
    MatrixNd Mvbb = new MatrixNd(bsize, bsize + 1);
    // System.out.println ("asize=" + asize + " bsize=" + bsize);
    int[] zaIndices = new int[asize];
    int[] waIndices = new int[asize];
    int[] zbIndices = new int[bsize + 1];
    int[] wbIndices = new int[bsize];
    int iza = 0;
    int iwa = 0;
    int izb = 0;
    int iwb = 0;
    int kwzi = -1;
    for (int j = 0; j < numActiveVars; j++) {
        int idx = activeToVar[j];
        if (zVars[idx].isBasic) {
            zaIndices[iza++] = j;
        } else {
            zbIndices[izb++] = j;
        }
    }
    if (z0Var.isBasic) {
        zaIndices[iza++] = numActiveVars;
    } else {
        zbIndices[izb++] = numActiveVars;
    }
    for (int i = 0; i < numActiveVars; i++) {
        int vari = activeToVar[i];
        if (!wVars[vari].isBasic) {
            if (wVars[vari] == wzVar) {
                kwzi = iwa;
            }
            waIndices[iwa++] = i;
        } else {
            wbIndices[iwb++] = i;
        }
    }
    MatrixNd M = new MatrixNd(numActiveVars, numActiveVars + 1);
    for (int i = 0; i < numActiveVars; i++) {
        for (int j = 0; j < numActiveVars; j++) {
            M.set(i, j, Melem(activeToVar[i], activeToVar[j]));
        }
        M.set(i, numActiveVars, Melem(activeToVar[i], numActiveVars));
    }
    M.getSubMatrix(waIndices, zaIndices, Maa);
    M.getSubMatrix(waIndices, zbIndices, Mab);
    M.getSubMatrix(wbIndices, zaIndices, Mba);
    M.getSubMatrix(wbIndices, zbIndices, Mbb);
    System.out.println("Maa=\n" + Maa.toString("%9.6f"));
    Mvaa.invert(Maa);
    Mvab.mul(Mvaa, Mab);
    Mvab.negate();
    if (bsize != 0) {
        Mvba.mul(Mba, Mvaa);
        Mvbb.mul(Mba, Mvab);
        Mvbb.add(Mbb);
    }
    // compute q values
    iwa = 0;
    iwb = 0;
    VectorNd qa = new VectorNd(asize);
    VectorNd qb = new VectorNd(bsize);
    VectorNd qva = new VectorNd(asize);
    VectorNd qvb = new VectorNd(bsize);
    double[] qvcol = new double[numActiveVars];
    for (int i = 0; i < numVars; i++) {
        if (wVars[i].type != GAMMA) {
            qvcol[i] = wVars[i].nd.dot(wapplyAdjusted) - bq[i];
        } else {
            qvcol[i] = -bq[i];
        }
    }
    for (iwa = 0; iwa < asize; iwa++) {
        qa.set(iwa, qvcol[waIndices[iwa]]);
    }
    for (iwb = 0; iwb < bsize; iwb++) {
        qb.set(iwb, qvcol[wbIndices[iwb]]);
    }
    Mvaa.mul(qva, qa);
    qva.negate();
    Mba.mul(qvb, qva);
    qvb.add(qb);
    if (wzVar != null) {
        zaIndices[asize - 1] = wzVar.idx;
        waIndices[kwzi] = numActiveVars;
    }
    MvCheck.setSubMatrix(zaIndices, waIndices, Mvaa);
    MvCheck.setSubMatrix(zaIndices, zbIndices, Mvab);
    MvCheck.setSubMatrix(wbIndices, waIndices, Mvba);
    MvCheck.setSubMatrix(wbIndices, zbIndices, Mvbb);
    VectorNd qvCheck = new VectorNd(numActiveVars);
    for (iza = 0; iza < asize; iza++) {
        qvCheck.set(zaIndices[iza], qva.get(iza));
    }
    for (iwb = 0; iwb < bsize; iwb++) {
        qvCheck.set(wbIndices[iwb], qvb.get(iwb));
    }
    double mtol = MvCheck.infinityNorm() * 1e-12;
    double qtol = MvCheck.infinityNorm() * 1e-12;
    VectorNd qvVec = new VectorNd(numActiveVars);
    qvVec.set(qv);
    VectorNd qInit = new VectorNd(numActiveVars);
    qInit.set(qvcol);
    if (!Mv.epsilonEquals(MvCheck, mtol) | !qvVec.epsilonEquals(qvCheck, qtol)) {
        System.out.println("Error, pivot check failed, see mvcheck.m for matrices");
        printBasis(drive);
        try {
            PrintStream fps = new PrintStream(new BufferedOutputStream(new FileOutputStream("mvcheck.m")));
            fps.println("M=[\n" + M.toString("%9.6f") + "];");
            fps.println("Mv=[\n" + Mv.toString("%9.6f") + "];");
            fps.println("MvCheck=[\n" + MvCheck.toString("%9.6f") + "];");
            fps.println("Maa=[\n" + Maa.toString("%18.12f") + "];");
            fps.println("Mab=[\n" + Mab.toString("%9.6f") + "];");
            fps.println("Mba=[\n" + Mba.toString("%9.6f") + "];");
            fps.println("Mbb=[\n" + Mbb.toString("%9.6f") + "];");
            fps.println("q=[" + qInit.toString("%9.6f") + "];");
            fps.println("qa=[" + qa.toString("%9.6f") + "];");
            fps.println("qb=[" + qb.toString("%9.6f") + "];");
            fps.println("qv=[" + qvVec.toString("%9.6f") + "];");
            fps.println("qvCheck=[" + qvCheck.toString("%9.6f") + "];");
            fps.close();
        } catch (Exception e) {
            e.printStackTrace();
        }
        MvCheck.sub(Mv);
        System.out.println("M error: " + MvCheck.infinityNorm());
        qvCheck.sub(qvVec);
        System.out.println("q error: " + qvCheck.infinityNorm());
    }
}

Example 4 with MatrixNd

use of maspack.matrix.MatrixNd 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 5 with MatrixNd

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

the class FemModel3d method computePressuresAndRinv.

protected void computePressuresAndRinv(FemElement3d e, IncompressibleMaterial imat, double scale) {
    int npvals = e.numPressureVals();
    myRinv.setSize(npvals, npvals);
    myPressures.setSize(npvals);
    double[] pbuf = myPressures.getBuffer();
    double restVol = e.getRestVolume();
    if (npvals > 1) {
        myPressures.setZero();
        IntegrationPoint3d[] ipnts = e.getIntegrationPoints();
        IntegrationData3d[] idata = e.getIntegrationData();
        if (imat.getBulkPotential() != BulkPotential.QUADRATIC) {
            myRinv.setZero();
        }
        for (int k = 0; k < ipnts.length; k++) {
            IntegrationPoint3d pt = ipnts[k];
            pt.computeJacobian(e.getNodes());
            double detJ0 = idata[k].getDetJ0();
            double detJ = pt.getJ().determinant() / detJ0;
            double dV = detJ0 * pt.getWeight();
            double[] H = pt.getPressureWeights().getBuffer();
            for (int i = 0; i < npvals; i++) {
                pbuf[i] += H[i] * imat.getEffectivePressure(detJ) * dV;
            }
            if (imat.getBulkPotential() != BulkPotential.QUADRATIC) {
                double mod = imat.getEffectiveModulus(detJ);
                for (int i = 0; i < npvals; i++) {
                    for (int j = 0; j < npvals; j++) {
                        myRinv.add(i, j, H[i] * H[j] * mod * dV);
                    }
                }
            }
        }
        Matrix W = e.getPressureWeightMatrix();
        W.mul(myPressures, myPressures);
        myPressures.scale(1 / restVol);
        if (imat.getBulkPotential() == BulkPotential.QUADRATIC) {
            myRinv.set(W);
            myRinv.scale(scale * imat.getBulkModulus() / restVol);
        } else {
            // optimize later
            MatrixNd Wtmp = new MatrixNd(W);
            Wtmp.scale(scale / restVol);
            myRinv.mul(Wtmp);
            myRinv.mul(Wtmp, myRinv);
        }
    } else {
        double Jpartial = e.myVolumes[0] / e.myRestVolumes[0];
        pbuf[0] = (imat.getEffectivePressure(Jpartial) + 0 * e.myLagrangePressures[0]);
        myRinv.set(0, 0, scale * imat.getEffectiveModulus(Jpartial) / restVol);
    }
}