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

Example 1 with VectorNd

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

the class IpoptExample method solve.

public void solve() {
    double[] x = new double[n];
    x[0] = 1.0;
    x[1] = 5.0;
    x[2] = 5.0;
    x[3] = 1.0;
    /* allocate space to store the bound multipliers at the solution */
    double[] mult_x_L = new double[n];
    double[] mult_x_U = new double[n];
    double[] obj_value = new double[1];
    solveNLP(x, obj_value, mult_x_L, mult_x_U, n);
    VectorNd vec = new VectorNd(n);
    vec.set(x);
    System.out.println("\n~~~~~~ Solution Found ~~~~~~~");
    System.out.println("x_opt = " + vec.toString("%4.2e"));
    System.out.println("f(x_opt) = " + obj_value[0]);
}

Example 2 with VectorNd

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

the class IpoptExample method Eval_Grad_F.

/**
 * Type defining the callback function for evaluating the gradient of the
 * objective function. Return value should be set to false if there was a
 * problem doing the evaluation.
 */
protected boolean Eval_Grad_F(int n, double[] x, boolean new_x, double[] grad_f) // , UserDataPtr user_data)
{
    // System.out.println("Eval_Grad_F() in Java.");
    if (n != 4)
        return false;
    VectorNd v = new VectorNd(n);
    v.set(x);
    // System.out.println("x = " + v.toString());
    grad_f[0] = x[0] * x[3] + x[3] * (x[0] + x[1] + x[2]);
    grad_f[1] = x[0] * x[3];
    grad_f[2] = x[0] * x[3] + 1;
    grad_f[3] = x[0] * (x[0] + x[1] + x[2]);
    v.set(grad_f);
    // System.out.println("grad_f = " + v.toString());
    return true;
}

Example 3 with VectorNd

use of maspack.matrix.VectorNd 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 VectorNd

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

the class CGSolver method solveTransformed.

public boolean solveTransformed(VectorNd x, SparseMatrixNd A, VectorNd b, double tol, int maxIter, IncompleteCholeskyDecomposition icd) {
    // icd.factor(A);
    // icd.L.setIdentity();
    int n = x.size();
    VectorNd d = new VectorNd(n);
    VectorNd r = new VectorNd(n);
    VectorNd dnew = new VectorNd(n);
    VectorNd rnew = new VectorNd(n);
    VectorNd xnew = new VectorNd(n);
    VectorNd EAEv = new VectorNd(n);
    // x.setZero();
    icd.solveTranspose(EAEv, x);
    A.mul(EAEv, EAEv);
    icd.solve(EAEv, EAEv);
    icd.solve(d, b);
    d.sub(EAEv);
    // d.set(b);
    r.set(d);
    double rnorm2 = r.normSquared();
    double rnorm2tol = Math.sqrt(r.norm()) * tol;
    int i = 0;
    while (i < maxIter) {
        rnorm2 = r.normSquared();
        if (Math.sqrt(rnorm2) < rnorm2tol) {
            System.out.println("error tolerance reached at " + i);
            break;
        }
        icd.solveTranspose(EAEv, d);
        A.mul(EAEv, EAEv);
        icd.solve(EAEv, EAEv);
        // A.mul(tmp, d);
        double alpha = rnorm2 / d.dot(EAEv);
        xnew.scaledAdd(alpha, d, x);
        rnew.scaledAdd(-alpha, EAEv, r);
        double beta = rnew.normSquared() / rnorm2;
        dnew.scaledAdd(beta, d, rnew);
        x.set(xnew);
        r.set(rnew);
        d.set(dnew);
        // System.out.println(rnorm2);
        i++;
    }
    myLastIterationCnt = i;
    myLastResidualSquared = rnorm2;
    icd.solveTranspose(x, x);
    return true;
}

Example 5 with VectorNd

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

the class CGSolverTest method timing.

public void timing() {
    CGSolver solver = new CGSolver();
    FunctionTimer timer = new FunctionTimer();
    for (int nbod = 10; nbod <= 50; nbod += 10) {
        SparseMatrixNd Sc = createBlockDiagonal(nbod);
        VectorNd xc = new VectorNd(Sc.rowSize());
        VectorNd bc = new VectorNd(Sc.rowSize());
        bc.setRandom(-0.5, 0.5, randGen);
        int loopCnt = 10;
        boolean solved = true;
        timer.start();
        for (int i = 0; i < loopCnt; i++) {
            xc.setZero();
            if (!solver.solve(xc, Sc, bc, 1e-8, 10 * xc.size())) {
                solved = false;
                break;
            }
        }
        timer.stop();
        if (!solved) {
            System.out.println("" + nbod + " bodies: No convergence");
        } else {
            System.out.println("" + nbod + " bodies: " + timer.resultMsec(loopCnt));
            System.out.println("         iterations=" + solver.getNumIterations() + " error=" + solver.getRelativeResidual());
        }
    }
}