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

Example 1 with MatrixBlock

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

the class FemModel3d method createStiffnessMatrix.

// builds a Stiffness matrix, where entries are ordered by node numbers
public SparseBlockMatrix createStiffnessMatrix() {
    if (!myStressesValidP || !myStiffnessesValidP) {
        updateStressAndStiffness();
    }
    SparseBlockMatrix M = new SparseBlockMatrix();
    int nnodes = numNodes();
    int[] sizes = new int[nnodes];
    for (int i = 0; i < nnodes; ++i) {
        sizes[i] = 3;
    }
    M.addRows(sizes, sizes.length);
    M.addCols(sizes, sizes.length);
    M.setVerticallyLinked(true);
    int idx = 0;
    for (FemNode3d node : getNodes()) {
        node.setIndex(idx++);
    }
    // create solve blocks
    for (FemNode3d node : getNodes()) {
        MatrixBlock blk = node.createSolveBlock();
        M.addBlock(node.getIndex(), node.getIndex(), blk);
    }
    for (int i = 0; i < myNodes.size(); i++) {
        FemNode3d node = myNodes.get(i);
        for (FemNodeNeighbor nbr : getNodeNeighbors(node)) {
            FemNode3d other = nbr.getNode();
            Matrix3x3Block blk = null;
            if (other != node) {
                blk = new Matrix3x3Block();
                M.addBlock(node.getIndex(), nbr.getNode().getIndex(), blk);
            } else {
                blk = (Matrix3x3Block) M.getBlock(node.getIndex(), node.getIndex());
            }
            nbr.addPosJacobian(blk, -1.0);
        }
    }
    return M;
}

Example 2 with MatrixBlock

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

the class FemModel3d method mulInverseMass.

@Override
public void mulInverseMass(SparseBlockMatrix M, VectorNd a, VectorNd f) {
    double[] abuf = a.getBuffer();
    double[] fbuf = f.getBuffer();
    int asize = a.size();
    if (M.getAlignedBlockRow(asize) == -1) {
        throw new IllegalArgumentException("size of 'a' not block aligned with 'M'");
    }
    if (f.size() < asize) {
        throw new IllegalArgumentException("size of 'f' is less than the size of 'a'");
    }
    int bf = myFrame.getSolveIndex();
    int bk;
    if (myFrameRelativeP && bf < asize) {
        int fidx = M.getBlockRowOffset(bf);
        Wrench wtmp = new Wrench();
        double[] tmp6 = new double[6];
        for (int i = 0; i < myNodes.size(); i++) {
            FemNode3d n = myNodes.get(i);
            if ((bk = n.getSolveIndex()) != -1) {
                int idx = M.getBlockRowOffset(bk);
                if (idx < asize) {
                    MatrixBlock Mfk = M.getBlock(bf, bk);
                    zero6Vector(tmp6);
                    Mfk.mulAdd(tmp6, 0, fbuf, idx);
                    Matrix3x3Block Mkk = (Matrix3x3Block) M.getBlock(bk, bk);
                    // XXX do we want to use n.mulInverseEffectiveMass?
                    scaledAdd(wtmp, -1 / Mkk.m00, tmp6, 0);
                }
            }
        }
        scaledAdd(wtmp, 1, fbuf, fidx);
        // XXX do we need to use getBlock (bf, bf)???
        Matrix6dBlock Mff = (Matrix6dBlock) M.getBlock(bf, bf);
        SpatialInertia S = new SpatialInertia();
        S.set(Mff);
        Twist ttmp = new Twist();
        S.mulInverse(ttmp, wtmp);
        setFromTwist(abuf, fidx, ttmp);
        double[] tmp3 = new double[3];
        for (int i = 0; i < myNodes.size(); i++) {
            FemNode3d n = myNodes.get(i);
            if ((bk = n.getSolveIndex()) != -1) {
                int idx = M.getBlockRowOffset(bk);
                if (idx < asize) {
                    MatrixBlock Mkf = M.getBlock(bk, bf);
                    Matrix3x3Block Mkk = (Matrix3x3Block) M.getBlock(bk, bk);
                    double minv = 1 / Mkk.m00;
                    zero3Vector(tmp3);
                    Mkf.mulAdd(tmp3, 0, abuf, fidx);
                    abuf[idx++] = minv * (fbuf[idx] - tmp3[0]);
                    abuf[idx++] = minv * (fbuf[idx] - tmp3[1]);
                    abuf[idx++] = minv * (fbuf[idx] - tmp3[2]);
                }
            }
        }
    } else {
        for (int i = 0; i < myNodes.size(); i++) {
            FemNode3d n = myNodes.get(i);
            if ((bk = n.getSolveIndex()) != -1) {
                int idx = M.getBlockRowOffset(bk);
                if (idx < asize) {
                    n.mulInverseEffectiveMass(M.getBlock(bk, bk), abuf, fbuf, idx);
                }
            }
        }
    }
}

Example 3 with MatrixBlock

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

the class MechSystemSolver method projectSingleFrictionConstraint.

private double projectSingleFrictionConstraint(VectorNd vel, SparseBlockMatrix DT, int bj, double phiMax, double doff, boolean ignoreRigidBodies) {
    int nactive = mySys.numActiveComponents();
    int nparam = mySys.numParametricComponents();
    int nlimit = nactive;
    if (MechSystemBase.myParametricsInSystemMatrix) {
        nlimit += nparam;
    }
    Vector3d d = new Vector3d();
    Vector3d r = new Vector3d();
    double[] vbuf = vel.getBuffer();
    double[] pbuf = myUpar.getBuffer();
    double[] vtbuf = new double[1];
    double phi;
    double dmd = 0;
    for (MatrixBlock blk = DT.firstBlockInCol(bj); blk != null; blk = blk.down()) {
        int bi = blk.getBlockRow();
        if (bi < nactive) {
            blk.mulTransposeAdd(vtbuf, 0, vbuf, DT.getBlockRowOffset(bi));
            if (blk instanceof Matrix3x1) {
                ((Matrix3x1) blk).get(d);
                // XXX assumes that corresponding block is Matrix3d
                Matrix3dBase Minv = (Matrix3dBase) myInverseMass.getBlock(bi, bi);
                Minv.mul(r, d);
                dmd += r.dot(d);
            } else if (!ignoreRigidBodies) {
            // XXX implement
            }
        } else if (MechSystemBase.myParametricsInSystemMatrix && bi < nactive + nparam) {
            int poff = DT.getBlockRowOffset(bi) - myActiveVelSize;
            blk.mulTransposeAdd(vtbuf, 0, pbuf, poff);
        }
    }
    double vt = vtbuf[0] - doff;
    // prevent division by zero
    if (dmd == 0) {
        dmd = 1e-16;
    }
    // System.out.println (" vtMag=" + vt + " dmd=" + dmd + " phiMax="+phiMax);
    if (vt > dmd * phiMax) {
        phi = -phiMax;
    } else if (vt < -dmd * phiMax) {
        phi = phiMax;
    } else {
        phi = -vt / dmd;
    }
    VectorNd dvec = new VectorNd();
    for (MatrixBlock blk = DT.firstBlockInCol(bj); blk != null; blk = blk.down()) {
        int bi = blk.getBlockRow();
        if (bi >= nactive) {
            break;
        }
        if (blk.rowSize() == 3) {
            dvec.setSize(3);
            blk.getColumn(0, dvec);
            MatrixBlock Minv = myInverseMass.getBlock(bi, bi);
            dvec.scale(phi);
            Minv.mulAdd(vbuf, DT.getBlockRowOffset(bi), dvec.getBuffer(), 0);
        } else if (!ignoreRigidBodies) {
        // XXX implement
        }
    }
    return phi;
}

Example 4 with MatrixBlock

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

Example 5 with MatrixBlock

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

the class ContactConstraint method addConstraintBlocks.

public void addConstraintBlocks(SparseBlockMatrix GT, int bj) {
    for (int k = 0; k < myMasters.size(); k++) {
        ContactMaster cm = myMasters.get(k);
        int bi = cm.getSolveIndex();
        if (bi != -1) {
            MatrixBlock blk = cm.getBlock(myNormal);
            GT.addBlock(bi, bj, blk);
        }
    }
}