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

Example 6 with MatrixBlock

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

the class DynamicAttachment method addSolveBlocks.

public void addSolveBlocks(SparseNumberedBlockMatrix S, boolean[] reduced) {
    DynamicComponent[] masters = getMasters();
    if (masters.length == 0) {
        return;
    }
    if (!S.isVerticallyLinked()) {
        throw new IllegalArgumentException("Matrix G is not vertically linked");
    }
    int bs = getSlaveSolveIndex();
    if (bs == -1) {
        return;
    }
    // rows first
    MatrixBlock blk = S.firstBlockInRow(bs);
    while (blk != null) {
        int bj = blk.getBlockCol();
        for (int i = 0; i < masters.length; i++) {
            if (masters[i].getSolveIndex() != -1) {
                int bm = masters[i].getSolveIndex();
                if (S.getBlock(bm, bj) == null) {
                    MatrixBlock newBlk = MatrixBlockBase.alloc(S.getBlockRowSize(bm), blk.colSize());
                    // S.addBlock (bm, bj, createRowBlock (blk.colSize()));
                    S.addBlock(bm, bj, newBlk);
                // System.out.println ("adding " + bm + " " + bj);
                }
            }
        }
        blk = blk.next();
    }
    reduced[bs] = true;
    // columns next
    blk = S.firstBlockInCol(bs);
    while (blk != null) {
        int bi = blk.getBlockRow();
        if (!reduced[bi]) {
            for (int i = 0; i < masters.length; i++) {
                if (masters[i].getSolveIndex() != -1) {
                    int bm = masters[i].getSolveIndex();
                    if (S.getBlock(bi, bm) == null) {
                        MatrixBlock newBlk = MatrixBlockBase.alloc(blk.rowSize(), S.getBlockColSize(bm));
                        // S.addBlock (bi, bm, createColBlock (blk.rowSize()));
                        S.addBlock(bi, bm, newBlk);
                    // System.out.println ("adding " + bi + " " + bm);
                    }
                }
            }
        }
        blk = blk.down();
    }
}

Example 7 with MatrixBlock

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

the class ContactConstraint method add1DFrictionConstraints.

public int add1DFrictionConstraints(SparseBlockMatrix DT, FrictionInfo[] finfo, double mu, int numf) {
    Vector3d dir = new Vector3d();
    if (myMasters.size() > 0 && computeFrictionDir(dir) > 0 && mu > 0) {
        finfo[numf].mu = mu;
        finfo[numf].contactIdx0 = mySolveIndex;
        finfo[numf].contactIdx1 = -1;
        finfo[numf].flags = FrictionInfo.BILATERAL;
        for (int i = 0; i < myMasters.size(); i++) {
            ContactMaster cm = myMasters.get(i);
            int bi = cm.getSolveIndex();
            if (bi != -1) {
                MatrixBlock blk = cm.get1DFrictionBlock(dir);
                Vector3d tmp = new Vector3d();
                tmp.x = blk.get(0, 0);
                tmp.y = blk.get(1, 0);
                tmp.z = blk.get(2, 0);
                DT.addBlock(bi, numf, blk);
            }
        }
        numf++;
    }
    return numf;
}

Example 8 with MatrixBlock

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

the class ContactConstraint method add2DFrictionConstraints.

public int add2DFrictionConstraints(SparseBlockMatrix DT, FrictionInfo[] finfo, double mu, int numf) {
    if (myMasters.size() > 0) {
        Vector3d dir1 = new Vector3d();
        Vector3d dir2 = new Vector3d();
        computeFrictionDirs(dir1, dir2);
        finfo[numf].mu = mu;
        finfo[numf].contactIdx0 = mySolveIndex;
        finfo[numf].contactIdx1 = -1;
        finfo[numf].flags = 0;
        for (int i = 0; i < myMasters.size(); i++) {
            ContactMaster cm = myMasters.get(i);
            int bi = cm.getSolveIndex();
            if (bi != -1) {
                MatrixBlock blk = cm.get2DFrictionBlock(dir1, dir2);
                DT.addBlock(bi, numf, blk);
            }
        }
        numf++;
    }
    return numf;
}

Example 9 with MatrixBlock

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

the class DynamicAttachment method reduceConstraints.

/**
 * Reduces a sparse column matrix (such as the transpose of a constraint
 * matrix) to account for this attachment. This is done by applying
 * the transform
 * <pre>
 * GT = P GT
 * </pre>
 * where
 * <pre>
 *             T
 *     [ I -Gka  ]
 * P = [         ]
 *     [ 0   0   ]
 * </pre>
 * and Gka is the constraint matrix for this attachment.
 */
public void reduceConstraints(SparseBlockMatrix GT, VectorNd dg) {
    DynamicComponent[] masters = getMasters();
    if (masters.length == 0) {
        return;
    }
    int bs = getSlaveSolveIndex();
    if (bs == -1) {
        return;
    }
    double[] dbuf = null;
    double[] gbuf = null;
    int ssize = 0;
    if (dg != null) {
        gbuf = dg.getBuffer();
        ssize = GT.getBlockRowSize(bs);
        dbuf = getNegatedDerivative(new VectorNd(ssize));
    }
    MatrixBlock blk = GT.firstBlockInRow(bs);
    while (blk != null) {
        int bj = blk.getBlockCol();
        for (int i = 0; i < masters.length; i++) {
            if (masters[i].getSolveIndex() != -1) {
                int bm = masters[i].getSolveIndex();
                MatrixBlock depBlk = GT.getBlock(bm, bj);
                if (depBlk == null) {
                    depBlk = MatrixBlockBase.alloc(GT.getBlockRowSize(bm), blk.colSize());
                    // depBlk = createRowBlock (blk.colSize());
                    GT.addBlock(bm, bj, depBlk);
                }
                mulSubGT(depBlk, blk, i);
                if (gbuf != null && dbuf != null) {
                    int goff = GT.getBlockColOffset(bj);
                    blk.mulTransposeAdd(gbuf, goff, dbuf, 0);
                }
            }
        }
        blk = blk.next();
    }
}

Example 10 with MatrixBlock

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

the class DynamicAttachment method reduceMass.

public void reduceMass(SparseBlockMatrix M, VectorNd f) {
    DynamicComponent[] masters = getMasters();
    if (masters.length == 0) {
        return;
    }
    int bs = getSlaveSolveIndex();
    if (bs == -1) {
        return;
    }
    double[] dbuf = null;
    double[] fbuf = null;
    int soff = -1;
    if (f != null) {
        fbuf = f.getBuffer();
        soff = M.getBlockRowOffset(bs);
        dbuf = getNegatedDerivative(new VectorNd(M.getBlockRowSize(bs)));
    }
    MatrixBlock sblk = M.getBlock(bs, bs);
    if (fbuf != null && dbuf != null) {
        // add coriolis term to existing fictitious force term for the slave
        sblk.mulAdd(fbuf, soff, dbuf, 0);
    }
    // System.out.println ("f1=" + f.toString ("%7.4f"));
    for (int i = 0; i < masters.length; i++) {
        int bm = masters[i].getSolveIndex();
        if (bm != -1) {
            MatrixBlock mblk = M.getBlock(bm, bm);
            addMassToMaster(mblk, sblk, i);
            if (fbuf != null) {
                int moff = M.getBlockRowOffset(bm);
                mulSubGT(fbuf, moff, fbuf, soff, i);
            }
        }
    }
// System.out.println ("f2=" + f.toString ("%7.4f"));
}