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

Example 81 with VectorNd

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

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

the class MechSystemSolver method projectRigidBodyPosConstraints.

public void projectRigidBodyPosConstraints(double t) {
    updateStateSizes();
    updateMassMatrix(t);
    VectorNd q = new VectorNd(myActivePosSize);
    StepAdjustment steppingInfo = new StepAdjustment();
    mySys.updateConstraints(t, steppingInfo, /*flags=*/
    MechSystem.COMPUTE_CONTACTS);
    mySys.getActivePosState(q);
    if (computeRigidBodyPosCorrections(q, t)) {
        mySys.setActivePosState(q);
    // mySys.updateConstraints (
    // t, steppingInfo,  /*flags=*/MechSystem.UPDATE_CONTACTS);
    }
}

Example 83 with VectorNd

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

the class MechSystemSolver method rungeKutta4.

protected void rungeKutta4(double t0, double t1, StepAdjustment stepAdjust) {
    double h = t1 - t0;
    double th = (t0 + t1) / 2;
    int velSize = myActiveVelSize;
    int posSize = myActivePosSize;
    myF.setSize(velSize);
    myU.setSize(velSize);
    myUtmp.setSize(velSize);
    myDudt.setSize(velSize);
    myDudtAvg.setSize(velSize);
    myQ.setSize(posSize);
    myQtmp.setSize(posSize);
    myDqdt.setSize(posSize);
    myDqdtAvg.setSize(posSize);
    mySys.updateForces(t0);
    mySys.getActivePosState(myQ);
    mySys.getActiveVelState(myU);
    mySys.getActivePosDerivative(myDqdtAvg, t0);
    updateInverseMassMatrix(t0);
    getActiveVelDerivative(myDudtAvg, myF);
    // k2 term
    myQtmp.scaledAdd(h / 2, myDqdtAvg, myQ);
    // xTmp.set (xVec);
    // mySys.addActivePosImpulse (xTmp, h/2, vVec);
    myUtmp.scaledAdd(h / 2, myDudtAvg, myU);
    mySys.setActivePosState(myQtmp);
    mySys.setActiveVelState(myUtmp);
    updateMassMatrix(-1);
    updateInverseMassMatrix(-1);
    mySys.updateForces(th);
    mySys.getActivePosDerivative(myDqdt, th);
    getActiveVelDerivative(myDudt, myF);
    myDqdtAvg.scaledAdd(2, myDqdt, myDqdtAvg);
    myDudtAvg.scaledAdd(2, myDudt, myDudtAvg);
    // k3 term
    myQtmp.scaledAdd(h / 2, myDqdt, myQ);
    // xTmp.set (xVec);
    // mySys.addActivePosImpulse (xTmp, h/2, vTmp);
    myUtmp.scaledAdd(h / 2, myDudt, myU);
    mySys.setActivePosState(myQtmp);
    mySys.setActiveVelState(myUtmp);
    updateMassMatrix(-1);
    updateInverseMassMatrix(-1);
    mySys.updateForces(th);
    mySys.getActivePosDerivative(myDqdt, th);
    getActiveVelDerivative(myDudt, myF);
    VectorNd dvdtNew = new VectorNd(myDudt);
    dvdtNew.sub(myDudt);
    myDqdtAvg.scaledAdd(2, myDqdt, myDqdtAvg);
    myDudtAvg.scaledAdd(2, myDudt, myDudtAvg);
    // k4 term
    myQtmp.scaledAdd(h, myDqdt, myQ);
    // xTmp.set (xVec);
    // mySys.addActivePosImpulse (xTmp, h, vTmp);
    myUtmp.scaledAdd(h, myDudt, myU);
    mySys.setActivePosState(myQtmp);
    mySys.setActiveVelState(myUtmp);
    updateMassMatrix(-1);
    updateInverseMassMatrix(-1);
    mySys.updateForces(t1);
    mySys.getActivePosDerivative(myDqdt, t1);
    getActiveVelDerivative(myDudt, myF);
    myDqdtAvg.add(myDqdt);
    myDudtAvg.add(myDudt);
    // dxdtAvg.scale (1/6.0);
    // dvdtAvg.scale (1/6.0);
    myUtmp.scaledAdd(h / 6, myDudtAvg, myU);
    myQtmp.scaledAdd(h / 6, myDqdtAvg, myQ);
    // xTmp.set (xVec);
    // mySys.addActivePosImpulse (xTmp, h, vTmp);
    mySys.setActivePosState(myQtmp);
    mySys.setActiveVelState(myUtmp);
    mySys.updateConstraints(t1, null, MechSystem.UPDATE_CONTACTS);
    applyVelCorrection(myUtmp, t0, t1);
    applyPosCorrection(myQtmp, myUtmp, t1, stepAdjust);
}

Example 84 with VectorNd

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

the class MechSystemSolver method fullBackwardEuler.

public void fullBackwardEuler(double t0, double t1, StepAdjustment stepAdjust) {
    if (myMatrixSolver == MatrixSolver.None) {
        throw new UnsupportedOperationException("MatrixSolver cannot be 'None' for this integrator");
    }
    double h = t1 - t0;
    int velSize = myActiveVelSize;
    int posSize = myActivePosSize;
    VectorNd xVec0 = new VectorNd(posSize);
    myB.setSize(velSize);
    myUtmp.setSize(velSize);
    myF.setSize(velSize);
    myU.setSize(velSize);
    myQ.setSize(posSize);
    // dxdtVec.setSize (posSize);
    myFparC.setSize(myParametricVelSize);
    mySys.updateConstraints(t1, null, MechSystem.UPDATE_CONTACTS);
    mySys.updateForces(t1);
    // b = M v
    mySys.getActiveVelState(myU);
    mulActiveInertias(myB, myU);
    mySys.getActiveForces(myF);
    myF.add(myMassForces);
    myB.scaledAdd(h, myF, myB);
    KKTFactorAndSolve(myUtmp, myFparC, myB, /*tmp=*/
    myF, myU, h);
    mySys.setActiveVelState(myUtmp);
    mySys.getActivePosState(xVec0);
    // mySys.getActivePosDerivative (dxdtVec, t1, 0);
    // xVec.scaledAdd (h, dxdtVec, xVec0);
    myQ.set(xVec0);
    mySys.addActivePosImpulse(myQ, h, myUtmp);
    mySys.setActivePosState(myQ);
    mySys.updateForces(t1);
    double fres = computeForceResidual(t0, t1, /*tmp=*/
    myF, velSize);
    double FRES_TOL = 1e-8;
    int MAX_ITER = 5;
    int iter = 0;
    if (fres > FRES_TOL) {
        VectorNd velk = new VectorNd(velSize);
        while (fres > FRES_TOL && iter < MAX_ITER) {
            velk.set(myUtmp);
            // no need to update mass matrix
            mulActiveInertias(myB, myU);
            mySys.getActiveForces(myF);
            // XXX mass forces need to be updated?
            myF.add(myMassForces);
            myB.scaledAdd(h, myF, myB);
            KKTFactorAndSolve(myUtmp, myFparC, myB, /*tmp=*/
            myF, velk, h, -h, -h * h, -h, -h * h);
            mySys.setActiveVelState(myUtmp);
            // mySys.getActivePosDerivative (dxdtVec, t1, 0);
            // xVec.scaledAdd (h, dxdtVec, xVec0);
            myQ.set(xVec0);
            mySys.addActivePosImpulse(myQ, h, myUtmp);
            mySys.setActivePosState(myQ);
            mySys.updateForces(t1);
            fres = computeForceResidual(t0, t1, /*tmp=*/
            myF, velSize);
            iter++;
        // System.out.println ("vel=" + myUtmp.toString ("%10.4f"));
        // System.out.printf ("fres[%d]=%g\n", iter, fres);
        }
    }
    // System.out.println ("iter=" + iter);
    // computeImplicitParametricForces (myUtmp, myFparC);
    mySys.updateConstraints(t1, null, MechSystem.UPDATE_CONTACTS);
    projectFrictionConstraints(myUtmp, t1);
    mySys.setActiveVelState(myUtmp);
    applyPosCorrection(myQ, myUtmp, t1, stepAdjust);
}

Example 85 with VectorNd

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

the class MechSystemSolver method updateMassMatrix.

public void updateMassMatrix(double t) {
    // assumes that updateStateSizes() has been called
    int version = mySys.getStructureVersion();
    if (version != myMassVersion) {
        myMass = new SparseNumberedBlockMatrix();
        myMassForces = new VectorNd(myMass.rowSize());
        myMassConstantP = mySys.buildMassMatrix(myMass);
        mySys.getMassMatrix(myMass, myMassForces, t);
        myMassVersion = version;
        myMassTime = t;
    } else if (t == -1 || myMassTime != t) {
        mySys.getMassMatrix(myMass, myMassForces, t);
        myMassTime = t;
    }
}