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

Example 1 with Matrix6x3Block

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

the class Frame method computeWorldPointForceJacobian.

/**
 * Computes the force Jacobian, in world coordinates, for a point attached
 * to this frame. If the velocity state size (vsize) of this frame is 6,
 * then GT should be an instance of Matrix6x3Block. Otherwise, it should be
 * an instance of MatrixNdBlock with a size of vsize x 3.
 *
 * For the Matrix6x3Block case, the Jacobian is a 6x3 matrix with
 * the following form:
 * <pre>
 * [     I     ]
 * [           ]
 * [ [ R loc ] ]
 * </pre>
 * where I is the 3x3 identity matrix, R is the frame orientation matrix,
 * and [ x ] denotes the 3x3 skew-symmetric cross product matrix.
 *
 * @param GT returns the force Jacobian
 * @param loc
 * position of the point, in body coordinates
 */
public void computeWorldPointForceJacobian(MatrixBlock GT, Point3d loc) {
    Matrix6x3Block blk;
    try {
        blk = (Matrix6x3Block) GT;
    } catch (ClassCastException e) {
        throw new IllegalArgumentException("GT is not an instance of Matrix6x3Block, is " + GT.getClass());
    }
    RotationMatrix3d R = getPose().R;
    blk.setZero();
    blk.m00 = 1;
    blk.m11 = 1;
    blk.m22 = 1;
    double lxb = loc.x;
    double lyb = loc.y;
    double lzb = loc.z;
    double lxw = R.m00 * lxb + R.m01 * lyb + R.m02 * lzb;
    double lyw = R.m10 * lxb + R.m11 * lyb + R.m12 * lzb;
    double lzw = R.m20 * lxb + R.m21 * lyb + R.m22 * lzb;
    blk.m40 = lzw;
    blk.m50 = -lyw;
    blk.m51 = lxw;
    blk.m31 = -lzw;
    blk.m32 = lyw;
    blk.m42 = -lxw;
}

Example 2 with Matrix6x3Block

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

the class Frame method computeLocalPointForceJacobian.

/**
 * Computes a force Jacobian for a point attached to this frame, and then
 * optionally rotates its columns using a rotation matrix R. If the
 * velocity state size (vsize) of this frame is 6, then G should be an
 * instance of Matrix6x3Block. Otherwise, it should be an instance of
 * MatrixNdBlock with a size of vsize x 3.
 *
 * If the rotation matrix R is <code>null</code>, then the Jacobian
 * takes the following form for the 6x3 matrix case:
 * <pre>
 * [     I       ]
 * [             ]
 * [ [ RF loc ]  ]
 * </pre>
 * where RF is the frame orientation matrix, loc is the location of the
 * point in frame coordinates, and [ x ] denotes the 3x3 skew-symmetric
 * cross product matrix.
 *
 * If the rotation matrix R is not <code>null</code>, then the
 * 6x3 case takes the following form:
 * <pre>
 * [     R         ]
 * [               ]
 * [ [ RF loc ]  R ]
 * </pre>
 *
 * @param GT returns the force Jacobian
 * @param loc location of the point, relative to the frame
 * @param R optional rotation transform
 */
public void computeLocalPointForceJacobian(MatrixBlock GT, Vector3d loc, RotationMatrix3d R) {
    Matrix6x3Block blk;
    try {
        blk = (Matrix6x3Block) GT;
    } catch (ClassCastException e) {
        throw new IllegalArgumentException("GT is not an instance of Matrix6x3Block, is " + GT.getClass());
    }
    Vector3d locw = new Vector3d();
    locw.transform(getPose().R, loc);
    double x = locw.x;
    double y = locw.y;
    double z = locw.z;
    if (R == null) {
        blk.m00 = 1;
        blk.m01 = 0;
        blk.m02 = 0;
        blk.m10 = 0;
        blk.m11 = 1;
        blk.m12 = 0;
        blk.m20 = 0;
        blk.m21 = 0;
        blk.m22 = 1;
        blk.m30 = 0;
        blk.m31 = -z;
        blk.m32 = y;
        blk.m40 = z;
        blk.m41 = 0;
        blk.m42 = -x;
        blk.m50 = -y;
        blk.m51 = x;
        blk.m52 = 0;
    } else {
        blk.m00 = R.m00;
        blk.m01 = R.m01;
        blk.m02 = R.m02;
        blk.m10 = R.m10;
        blk.m11 = R.m11;
        blk.m12 = R.m12;
        blk.m20 = R.m20;
        blk.m21 = R.m21;
        blk.m22 = R.m22;
        blk.m30 = y * R.m20 - z * R.m10;
        blk.m31 = y * R.m21 - z * R.m11;
        blk.m32 = y * R.m22 - z * R.m12;
        blk.m40 = z * R.m00 - x * R.m20;
        blk.m41 = z * R.m01 - x * R.m21;
        blk.m42 = z * R.m02 - x * R.m22;
        blk.m50 = x * R.m10 - y * R.m00;
        blk.m51 = x * R.m11 - y * R.m01;
        blk.m52 = x * R.m12 - y * R.m02;
    }
}

Example 3 with Matrix6x3Block

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

use of maspack.matrix.Matrix6x3Block 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());
                }
            }
        }
    }
}