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

Example 1 with Matrix3x6Block

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

the class FemModel3d method getMassMatrixValues.

@Override
public void getMassMatrixValues(SparseBlockMatrix M, VectorNd f, double t) {
    int bk;
    for (int k = 0; k < myNodes.size(); k++) {
        FemNode3d n = myNodes.get(k);
        if ((bk = n.getSolveIndex()) != -1) {
            n.getEffectiveMass(M.getBlock(bk, bk), t);
            n.getEffectiveMassForces(f, t, M.getBlockRowOffset(bk));
        }
    }
    if (myFrameRelativeP) {
        // angular velocity in body coords
        Vector3d wbod = new Vector3d();
        RotationMatrix3d R = myFrame.getPose().R;
        double[] fbuf = f.getBuffer();
        wbod.inverseTransform(R, myFrame.getVelocity().w);
        Vector3d w = myFrame.getVelocity().w;
        // update inertia in Frame
        double mass = 0;
        Point3d com = new Point3d();
        SymmetricMatrix3d J = new SymmetricMatrix3d();
        // extra fictitious forces for spatial inertia due to nodal velocity
        Vector3d fv = new Vector3d();
        Vector3d fw = new Vector3d();
        Vector3d fn = new Vector3d();
        Vector3d tmp = new Vector3d();
        // local node position rotated to world
        Point3d c = new Point3d();
        // local node velocity rotated to world
        Vector3d v = new Vector3d();
        int bf = myFrame.getSolveIndex();
        for (int k = 0; k < myNodes.size(); k++) {
            FemNode3d n = myNodes.get(k);
            if ((bk = n.getSolveIndex()) != -1) {
                c.transform(R, n.getLocalPosition());
                v.transform(R, n.getLocalVelocity());
                double m = n.getEffectiveMass();
                // System.out.println ("m " + k + " " + m);
                com.scaledAdd(m, c);
                mass += m;
                SpatialInertia.addPointRotationalInertia(J, m, c);
                if (useFrameRelativeCouplingMasses) {
                    Matrix3x6Block blk = (Matrix3x6Block) M.getBlock(bk, bf);
                    Matrix6x3Block blkT = (Matrix6x3Block) M.getBlock(bf, bk);
                    setCouplingMass(blk, m, R, c);
                    blkT.transpose(blk);
                    // compute fictitious forces for nodes, and accumulate nodal
                    // velocity fictitious force terms for spatial inertia
                    // tmp = 2*m (w X v)
                    tmp.cross(w, v);
                    tmp.scale(2 * m);
                    // fv += 2*m (w X v)
                    fv.add(tmp);
                    // fw += 2*m (c X w X v)
                    fw.crossAdd(c, tmp, fw);
                    // fn = 2*m (w X v)
                    fn.set(tmp);
                    // tmp = m*w X c
                    tmp.cross(w, c);
                    tmp.scale(m);
                    // fn += m (w X w X c)
                    fn.crossAdd(w, tmp, fn);
                    fn.inverseTransform(R);
                    // set fictitious force terms for node
                    int idx = M.getBlockRowOffset(bk);
                    fbuf[idx++] = -fn.x;
                    fbuf[idx++] = -fn.y;
                    fbuf[idx++] = -fn.z;
                }
            }
        }
        com.scale(1 / mass);
        SpatialInertia.addPointRotationalInertia(J, -mass, com);
        SpatialInertia S = new SpatialInertia();
        S.set(mass, J, com);
        // Frame keeps inertia in local coords
        S.inverseTransform(R);
        myFrame.setInertia(S);
        myFrame.getMass(M.getBlock(bf, bf), t);
        myFrame.getEffectiveMassForces(f, t, M.getBlockRowOffset(bf));
        if (useFrameRelativeCouplingMasses) {
            int idx = M.getBlockRowOffset(bf);
            fbuf[idx++] -= fv.x;
            fbuf[idx++] -= fv.y;
            fbuf[idx++] -= fv.z;
            fbuf[idx++] -= fw.x;
            fbuf[idx++] -= fw.y;
            fbuf[idx++] -= fw.z;
        }
        if (frameMassFraction != 0) {
            scaleFrameNodeMasses(M, f, 1.0 - frameMassFraction);
        }
    }
}

Example 2 with Matrix3x6Block

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

the class FemModel3d method addGeneralMassBlocks.

public void addGeneralMassBlocks(SparseBlockMatrix M) {
    if (myFrameRelativeP && useFrameRelativeCouplingMasses) {
        int bi = myFrame.getSolveIndex();
        if (bi != -1) {
            for (int i = 0; i < myNodes.size(); i++) {
                int bj = myNodes.get(i).getSolveIndex();
                if (bj != -1) {
                    M.addBlock(bi, bj, new Matrix6x3Block());
                    M.addBlock(bj, bi, new Matrix3x6Block());
                }
            }
        }
    }
}