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

Example 1 with Matrix3x1

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