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

Example 1 with MatrixNdBlock

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

the class MechSystemBase method getAttachmentConstraints.

/**
 * Returns the transpose of the attachment constraint matrix.  This is used
 * only for debugging.
 *
 * @return transposed attachment constraint matrix
 */
public SparseBlockMatrix getAttachmentConstraints() {
    updateDynamicComponentLists();
    // figure out the column sizes
    ArrayList<DynamicAttachment> attachments = getOrderedAttachments();
    int[] colSizes = new int[attachments.size()];
    for (int k = 0; k < attachments.size(); k++) {
        DynamicComponent slave = attachments.get(k).getSlave();
        int bj = attachments.size() - 1 - k;
        colSizes[bj] = myDynamicSizes[slave.getSolveIndex()];
    }
    SparseBlockMatrix GT = new SparseBlockMatrix(myDynamicSizes, colSizes);
    for (int k = 0; k < attachments.size(); k++) {
        int bj = attachments.size() - 1 - k;
        DynamicAttachment a = attachments.get(k);
        int ssize = colSizes[bj];
        DynamicComponent[] masters = a.getMasters();
        DynamicComponent slave = a.getSlave();
        MatrixNdBlock sblk = new MatrixNdBlock(ssize, ssize);
        sblk.setIdentity();
        GT.addBlock(slave.getSolveIndex(), bj, sblk);
        for (int i = 0; i < masters.length; i++) {
            int bi = masters[i].getSolveIndex();
            int msize = myDynamicSizes[bi];
            MatrixNdBlock mblk = new MatrixNdBlock(msize, ssize);
            a.mulSubGT(mblk, sblk, i);
            mblk.negate();
            GT.addBlock(bi, bj, mblk);
        }
    }
    return GT;
}