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

Example 1 with Matrix3x3Block

use of maspack.matrix.Matrix3x3Block 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 Matrix3x3Block

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

the class FemModel3d method addNodeNeighborBlock.

protected void addNodeNeighborBlock(SparseNumberedBlockMatrix S, FemNodeNeighbor nbr, int bi) {
    int bj = nbr.myNode.getSolveIndex();
    Matrix3x3Block blk = null;
    int blkNum = -1;
    if (bj != -1) {
        blk = (Matrix3x3Block) S.getBlock(bi, bj);
        if (blk == null) {
            blk = new Matrix3x3Block();
            S.addBlock(bi, bj, blk);
        }
        blkNum = blk.getBlockNumber();
    }
    // nbr.setBlock (blk);
    nbr.setBlockNumber(blkNum);
}

Example 3 with Matrix3x3Block

use of maspack.matrix.Matrix3x3Block 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 4 with Matrix3x3Block

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

the class PointFem3dAttachment method addMassToMaster.

public void addMassToMaster(MatrixBlock mblk, MatrixBlock sblk, int idx) {
    if (idx >= myNodes.length) {
        throw new IllegalArgumentException("Master idx=" + idx + " must be in the range [0," + (myNodes.length - 1) + "]");
    }
    if (!(mblk instanceof Matrix3x3Block)) {
        throw new IllegalArgumentException("Master blk not instance of Matrix6dBlock");
    }
    if (!(sblk instanceof Matrix3x3Block)) {
        throw new IllegalArgumentException("Master blk not instance of Matrix3x3Block");
    }
    double ci = myCoords.get(idx);
    // This is ci instead of ci*ci because mass is lumped ...
    double m = ci * ((Matrix3x3Block) sblk).m00;
    Matrix3x3Block dblk = (Matrix3x3Block) mblk;
    dblk.m00 += m;
    dblk.m11 += m;
    dblk.m22 += m;
}

Example 5 with Matrix3x3Block

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

the class Point method addToSolveBlockDiagonal.

public void addToSolveBlockDiagonal(SparseNumberedBlockMatrix S, double d) {
    if (mySolveBlockNum != -1) {
        Matrix3x3Block blk = (Matrix3x3Block) S.getBlockByNumber(mySolveBlockNum);
        blk.m00 += d;
        blk.m11 += d;
        blk.m22 += d;
    }
}