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

Example 1 with SparseNumberedBlockMatrix

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

the class MechSystemSolver method updateUnilateralConstraints.

protected void updateUnilateralConstraints() {
    // assumes that updateStateSizes() has been called
    myNT = new SparseNumberedBlockMatrix();
    mySys.getUnilateralConstraints(myNT, myNdot);
    myNsize = myNT.colSize();
    ensureNInfoCapacity(myNsize);
    myRn.setSize(myNsize);
    myBn.setSize(myNsize);
    myThe.setSize(myNsize);
}

Example 2 with SparseNumberedBlockMatrix

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

the class MechSystemSolver method createActiveStiffnessMatrix.

public SparseBlockMatrix createActiveStiffnessMatrix(double h) {
    updateStateSizes();
    if (mySolveMatrixVersion != mySys.getStructureVersion()) {
        mySolveMatrixVersion = mySys.getStructureVersion();
        mySolveMatrix = new SparseNumberedBlockMatrix();
        mySys.buildSolveMatrix(mySolveMatrix);
    }
    mySolveMatrix.setZero();
    mySys.addPosJacobian(mySolveMatrix, null, h);
    int nactive = mySys.numActiveComponents();
    return mySolveMatrix.createSubMatrix(nactive, nactive);
}

Example 3 with SparseNumberedBlockMatrix

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

the class MechSystemSolver method computeStiffnessPosCorrection.

protected void computeStiffnessPosCorrection(VectorNd vel, int velSize) {
    boolean analyze = false;
    updateSolveMatrixStructure();
    if (myKKTSolveMatrixVersion != mySolveMatrixVersion) {
        myKKTSolveMatrixVersion = mySolveMatrixVersion;
        analyze = true;
    }
    SparseNumberedBlockMatrix S = mySolveMatrix;
    S.setZero();
    mySys.addVelJacobian(S, null, -1);
    mySys.addPosJacobian(S, null, -1);
    addActiveMassMatrix(mySys, S);
    if (myKKTSolver == null) {
        myKKTSolver = new KKTSolver();
        analyze = true;
    }
    if (myKKTGTVersion != myGTVersion) {
        analyze = true;
        myKKTGTVersion = myGTVersion;
    }
    if (analyze) {
        myKKTSolver.analyze(S, velSize, myGT, myRg, mySys.getSolveMatrixType());
    }
    if (myHybridSolveP && !analyze && myNT.colSize() == 0) {
        myKKTSolver.factorAndSolve(S, velSize, myGT, myRg, vel, myLam, myBf, myBg, myHybridSolveTol);
    } else {
        myKKTSolver.factor(S, velSize, myGT, myRg, myNT, myRn);
        myKKTSolver.solve(vel, myLam, myThe, myBf, myBg, myBn);
    }
    if (computeKKTResidual) {
        double res = myKKTSolver.residual(S, velSize, myGT, myRg, myNT, myRn, vel, myLam, myThe, myBf, myBg, myBn);
        System.out.println("stiffness pos cor residual (" + velSize + "," + myGT.colSize() + "," + myNT.colSize() + "): " + res);
    }
}

Example 4 with SparseNumberedBlockMatrix

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

the class MechSystemSolver method updateSolveMatrixStructure.

protected void updateSolveMatrixStructure() {
    // assumes that updateStateSizes() has been called
    if (mySolveMatrix == null || mySys.getStructureVersion() != mySolveMatrixVersion) {
        mySolveMatrixVersion = mySys.getStructureVersion();
        mySolveMatrix = new SparseNumberedBlockMatrix();
        mySys.buildSolveMatrix(mySolveMatrix);
    }
}

Example 5 with SparseNumberedBlockMatrix

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

the class MechSystemSolver method KKTStaticFactorAndSolve.

/**
 * Solves a static KKT system of the form
 * <pre>{@code
 * -df/dx*Delta(x) -G^T*lambda - N^T*theta = f
 * G*Delta(x) + g = 0, N*Delta(x) + n >= 0
 * }</pre>
 *
 * @param u returned displacement Delta(x)
 * @param bf right-hand side net force
 * @param beta scale factor for any additional forces such as fictitious forces
 * @param btmp temporary vector
 */
public void KKTStaticFactorAndSolve(VectorNd u, VectorNd bf, double beta, VectorNd btmp) {
    updateStateSizes();
    int velSize = myActiveVelSize;
    boolean analyze = false;
    updateSolveMatrixStructure();
    if (myStaticKKTVersion != mySolveMatrixVersion) {
        myStaticKKTVersion = mySolveMatrixVersion;
        analyze = true;
    }
    SparseNumberedBlockMatrix S = mySolveMatrix;
    S.setZero();
    // add tikhonov regularization factor
    if (myStaticTikhonov > 0) {
        // identity
        for (int i = 0; i < S.numBlockRows(); ++i) {
            MatrixBlock bi = S.getBlock(i, i);
            for (int j = 0; j < bi.rowSize(); ++j) {
                bi.set(j, j, myStaticTikhonov);
            }
        }
    }
    myC.setZero();
    mySys.addPosJacobian(S, myC, -1);
    if (useFictitousJacobianForces && beta != 0) {
        bf.scaledAdd(beta, myC);
    }
    if (myStaticSolver == null) {
        myStaticSolver = new KKTSolver();
    }
    updateBilateralConstraints();
    if (myKKTGTVersion != myGTVersion) {
        analyze = true;
        myKKTGTVersion = myGTVersion;
    }
    // bilateral offsets
    // setBilateralOffsets (h, -a0); // -a0);
    // myVel.setSize (velSize);
    getBilateralDeviation(myBg);
    myRg.setZero();
    updateUnilateralConstraints();
    getUnilateralDeviation(myBn);
    // if (myStaticTikhonov < 0) {
    // double fn2 = S.frobeniusNormSquared();
    // if (myGsize > 0) {
    // fn2 += myGT.frobeniusNormSquared();
    // }
    // if (myNsize > 0) {
    // fn2 += myNT.frobeniusNormSquared();
    // }
    // double eps = Math.sqrt(0.1*Math.sqrt(fn2/velSize));
    // // add scaled identity
    // for (int i=0; i<S.numBlockRows(); ++i) {
    // MatrixBlock bi = S.getBlock(i, i);
    // for (int j=0; j<bi.rowSize(); ++j) {
    // bi.set(j,j, bi.get(j, j)+eps);
    // }
    // }
    // System.out.println("Tikhonov: " + eps);
    // }
    // 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) {
        u.setZero();
        if (analyze) {
            myStaticSolver.analyze(S, velSize, myGT, myRg, mySys.getSolveMatrixType());
        }
        if (myHybridSolveP && !analyze && myNT.colSize() == 0) {
            if (profileKKTSolveTime) {
                timerStart();
            }
            myStaticSolver.factorAndSolve(S, velSize, myGT, myRg, u, myLam, bf, myBg, myHybridSolveTol);
            if (profileKKTSolveTime) {
                timerStop("KKTsolve(hybrid)");
            }
        } else {
            if (profileKKTSolveTime) {
                timerStart();
            }
            myStaticSolver.factor(S, velSize, myGT, myRg, myNT, myRn);
            // int nperturbed = myStaticSolver.getNumNonZerosInFactors();
            myStaticSolver.solve(u, myLam, myThe, bf, myBg, myBn);
            if (profileKKTSolveTime) {
                timerStop("KKTsolve");
            }
        }
        if (computeKKTResidual) {
            double res = myStaticSolver.residual(S, velSize, myGT, myRg, myNT, myRn, u, 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);
                myStaticSolver.printLinearProblem(crsWriter, bf, myBg, "%g", crsOmitDiag);
            } catch (Exception e) {
                e.printStackTrace();
                crsWriter = null;
                crsFileName = null;
            }
        }
    }
    mySys.setBilateralImpulses(myLam, 1);
    mySys.setUnilateralImpulses(myThe, 1);
    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.Dense, velSize, velSize);
            myLogWriter.println("];");
            myLogWriter.println("GT(" + velSize + "x" + myGT.colSize() + ")=[");
            myGT.write(myLogWriter, fmt, Matrix.WriteFormat.Dense, velSize, myGT.colSize());
            myLogWriter.println("];");
            myLogWriter.println("NT(" + velSize + "x" + myNT.colSize() + ")=[");
            myNT.write(myLogWriter, fmt, Matrix.WriteFormat.Dense, 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("u=[");
            u.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;
        }
    }
}