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

Example 6 with SparseNumberedBlockMatrix

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

the class MechSystemSolver method KKTFactorAndSolve.

/**
 * Solves a KKT system in which the Jacobian augmented M matrix and
 * and force vectors are given by
 * <pre>
 * M' = M + a0 df/dv + a1 df/dx
 *
 * bf' = bf + (a2 df/dv + a3 df/dx) vel0
 * </pre>
 * It is assumed that a0 and a1 are both non-zero. It is also assumed that
 * the a0 = -alpha h, where h is the step size and alpha indicates the
 * propertion of implicitness for the solve; i.e., for regular backward
 * euler, alpha=1, while for trapezoidal solves, alpha = 0.5;
 *
 * When used to solve for velocities in an implicit integrator, then
 * on input, bf is assumed to be given by
 * <pre>
 * bf = M vel0 + h f
 * </pre>
 * where h is the time step and f is the generalized forces, while
 * on output bf is modified to include the Jacobian terms described
 * above.
 *
 * @param vel returns the computed velocity
 * @param fpar if useFictitousJacobianForces is true, returns fictitious
 * Jacobian forces for parametric components
 * @param bf right side offset
 * @param btmp temporary vector
 * @param vel0 right side velocity
 * @param h interval time step - used to scale constraint offsets and
 * impulses
 * @param a0 left side df/dv coefficient
 * @param a1 left side df/dx coefficient
 * @param a2 right side df/dv coefficient
 * @param a3 right side df/dx coefficient
 */
public void KKTFactorAndSolve(VectorNd vel, VectorNd fpar, VectorNd bf, VectorNd btmp, VectorNd vel0, double h, double a0, double a1, double a2, double a3) {
    // assumes that updateMassMatrix() has been called
    updateStateSizes();
    int velSize = myActiveVelSize;
    boolean analyze = false;
    updateSolveMatrixStructure();
    if (myKKTSolveMatrixVersion != mySolveMatrixVersion) {
        myKKTSolveMatrixVersion = mySolveMatrixVersion;
        analyze = true;
    }
    SparseNumberedBlockMatrix S = mySolveMatrix;
    S.setZero();
    myC.setSize(S.rowSize());
    myC.setZero();
    mySys.addVelJacobian(S, myC, a0);
    // System.out.println ("myC=" + myC);
    if (useFictitousJacobianForces) {
        bf.scaledAdd(-a0, myC);
        if (fpar != null && myParametricVelSize > 0) {
            setSubVector(fpar, myC, velSize, myParametricVelSize);
        }
    }
    if (vel0 != null) {
        double alpha = a2 / a0 - a3 / a1;
        S.mul(btmp, vel0, velSize, velSize);
        bf.scaledAdd(alpha, btmp);
    }
    myC.setZero();
    mySys.addPosJacobian(S, myC, a1);
    if (useFictitousJacobianForces) {
        bf.scaledAdd(-a0, myC);
        if (fpar != null && myParametricVelSize > 0) {
            addSubVector(fpar, myC, velSize, myParametricVelSize);
        }
    }
    if (vel0 != null && a3 != 0) {
        double beta = a3 / a1;
        S.mul(btmp, vel0, velSize, velSize);
        bf.scaledAdd(beta, btmp);
    }
    addActiveMassMatrix(mySys, S);
    if (velSize > 0 && myParametricVelSize > 0) {
        S.mulTranspose(btmp, myUpar, 0, velSize, velSize, myParametricVelSize);
        bf.sub(btmp);
    }
    if (myKKTSolver == null) {
        myKKTSolver = new KKTSolver();
    }
    updateBilateralConstraints();
    if (myKKTGTVersion != myGTVersion) {
        analyze = true;
        myKKTGTVersion = myGTVersion;
    }
    if (myGsize > 0 && myParametricVelSize > 0) {
        myGT.mulTranspose(myBg, myUpar, 0, myGsize, velSize, myParametricVelSize);
        // move to rhs
        myBg.negate();
    } else {
        myBg.setZero();
    }
    // a0 is assumed to be negative, which moves myGdot over to the rhs
    // myBg.scaledAdd (a0, myGdot);
    // -a0);
    setBilateralOffsets(h, -a0);
    updateUnilateralConstraints();
    if (myNsize > 0 && myParametricVelSize > 0) {
        myNT.mulTranspose(myBn, myUpar, 0, myNsize, velSize, myParametricVelSize);
        // move to rhs
        myBn.negate();
    } else {
        myBn.setZero();
    }
    // a0 is assumed to be negative, which moves myNdot over to the rhs
    // -a0);
    setUnilateralOffsets(h, -a0);
    // get these in case we are doing hybrid solves and they are needed to
    // help with a warm start
    mySys.getBilateralImpulses(myLam);
    mySys.getUnilateralImpulses(myThe);
    if (!solveModePrinted) {
        String msg = (myHybridSolveP ? "hybrid solves" : "direct solves");
        if (mySys.getSolveMatrixType() == Matrix.INDEFINITE) {
            msg += ", unsymmetric matrix";
        } else {
            msg += ", symmetric matrix";
        }
        System.out.println(msg);
        solveModePrinted = true;
    }
    if (crsWriter == null && crsFileName != null) {
        try {
            crsWriter = ArtisynthIO.newIndentingPrintWriter(crsFileName);
        } catch (Exception e) {
            crsFileName = null;
        }
    }
    if (velSize != 0) {
        if (vel0 != null) {
            // set vel to vel0 in case the solver needs a warm start
            vel.set(vel0);
        }
        if (analyze) {
            myKKTSolver.analyze(S, velSize, myGT, myRg, mySys.getSolveMatrixType());
        }
        if (myHybridSolveP && !analyze && myNT.colSize() == 0) {
            if (profileKKTSolveTime) {
                timerStart();
            }
            myKKTSolver.factorAndSolve(S, velSize, myGT, myRg, vel, myLam, bf, myBg, myHybridSolveTol);
            if (profileKKTSolveTime) {
                timerStop("KKTsolve(hybrid)");
            }
        } else {
            if (profileKKTSolveTime) {
                timerStart();
            }
            myKKTSolver.factor(S, velSize, myGT, myRg, myNT, myRn);
            myKKTSolver.solve(vel, myLam, myThe, bf, myBg, myBn);
            if (profileKKTSolveTime) {
                timerStop("KKTsolve");
            }
        }
        if (computeKKTResidual) {
            double res = myKKTSolver.residual(S, velSize, myGT, myRg, myNT, myRn, vel, myLam, myThe, bf, myBg, myBn);
            System.out.println("vel residual (" + velSize + "," + myGT.colSize() + "," + myNT.colSize() + "): " + res);
        }
        if (crsWriter != null) {
            String msg = "# KKTsolve M=" + velSize + " G=" + myGT.colSize() + " N=" + myNT.colSize() + (analyze ? " ANALYZE" : "");
            System.out.println(msg);
            try {
                crsWriter.println(msg);
                myKKTSolver.printLinearProblem(crsWriter, bf, myBg, "%g", crsOmitDiag);
            } catch (Exception e) {
                e.printStackTrace();
                crsWriter = null;
                crsFileName = null;
            }
        }
    }
    mySys.setBilateralImpulses(myLam, h);
    mySys.setUnilateralImpulses(myThe, h);
    if (myUpdateForcesAtStepEnd) {
        if (myGsize > 0) {
            myGT.mulAdd(myFcon, myLam, velSize, myGsize);
        }
        if (myNsize > 0) {
            myNT.mulAdd(myFcon, myThe, velSize, myNsize);
        }
    }
    if (myLogWriter != null) {
        try {
            NumberFormat fmt = new NumberFormat("%g");
            myLogWriter.println("M(" + velSize + "x" + velSize + ")=[");
            S.write(myLogWriter, fmt, Matrix.WriteFormat.SYMMETRIC_CRS, velSize, velSize);
            myLogWriter.println("];");
            myLogWriter.println("GT(" + velSize + "x" + myGT.colSize() + ")=[");
            myGT.write(myLogWriter, fmt, Matrix.WriteFormat.CRS, velSize, myGT.colSize());
            myLogWriter.println("];");
            myLogWriter.println("NT(" + velSize + "x" + myNT.colSize() + ")=[");
            myNT.write(myLogWriter, fmt, Matrix.WriteFormat.CRS, velSize, myNT.colSize());
            myLogWriter.println("];");
            myLogWriter.println("bf=[");
            bf.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("myBg=[");
            myBg.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("myBn=[");
            myBn.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("vel=[");
            vel.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("myLam=[");
            myLam.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("myThe=[");
            myThe.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("");
            myLogWriter.flush();
            System.out.println("logging");
        } catch (IOException e) {
            e.printStackTrace();
            myLogWriter = null;
        }
    }
}

Example 7 with SparseNumberedBlockMatrix

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

the class MechSystemSolver method updateFrictionConstraints.

protected void updateFrictionConstraints() {
    // assumes that updateStateSizes() has been called
    myDT = new SparseNumberedBlockMatrix();
    int fmax = mySys.maxFrictionConstraintSets();
    ensureFrictionCapacity(fmax);
    mySys.getFrictionConstraints(myDT, myFrictionInfo);
    int sizeD = myDT.colSize();
    myBd.setSize(sizeD);
    myPhi.setSize(sizeD);
    // compute friction offsets
    if (sizeD > 0 && myParametricVelSize > 0) {
        myDT.mulTranspose(myBd, myUpar, 0, sizeD, myActiveVelSize, myParametricVelSize);
        // move to rhs
        myBd.negate();
    } else {
        myBd.setZero();
    }
}

Example 8 with SparseNumberedBlockMatrix

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

the class MechSystemSolver method updateInverseMassMatrix.

protected boolean updateInverseMassMatrix(double t) {
    // assumes that updateMassMatrix has been called.
    boolean structureChanged = false;
    int version = mySys.getStructureVersion();
    if (version != myInverseMassVersion) {
        myInverseMass = new SparseNumberedBlockMatrix();
        mySys.buildMassMatrix(myInverseMass);
        mySys.getInverseMassMatrix(myInverseMass, myMass);
        myInverseMassVersion = version;
        myInverseMassTime = t;
        structureChanged = true;
    } else if (t == -1 || myInverseMassTime != t) {
        mySys.getInverseMassMatrix(myInverseMass, myMass);
        myInverseMassTime = t;
    }
    return structureChanged;
}

Example 9 with SparseNumberedBlockMatrix

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

Example 10 with SparseNumberedBlockMatrix

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

the class MechSystemSolver method updateBilateralConstraints.

protected void updateBilateralConstraints() {
    // assumes that updateStateSizes() has been called
    int[] oldStructure = null;
    if (myGT != null) {
        oldStructure = myGT.getBlockStructure();
    }
    myGT = new SparseNumberedBlockMatrix();
    mySys.getBilateralConstraints(myGT, myGdot);
    if (oldStructure == null || !myGT.blockStructureEquals(oldStructure)) {
        myGTVersion++;
    }
    myGsize = myGT.colSize();
    ensureGInfoCapacity(myGsize);
    myRg.setSize(myGsize);
    myBg.setSize(myGsize);
    myLam.setSize(myGsize);
}