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

Example 1 with Matrix6dBlock

use of maspack.matrix.Matrix6dBlock 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 2 with Matrix6dBlock

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

the class FrameSpring method addPosJacobianWorld.

public void addPosJacobianWorld(SparseNumberedBlockMatrix M, double s) {
    FrameMaterial mat = myMaterial;
    if (mat == null) {
        // just in case implementation allows null material ...
        return;
    }
    Matrix6d JK = new Matrix6d();
    Matrix6d JD = null;
    RotationMatrix3d R1W = new RotationMatrix3d();
    RigidTransform3d XAW = myFrameA.getPose();
    RigidTransform3d XBW;
    if (myFrameB != null) {
        XBW = myFrameB.getPose();
    } else {
        XBW = RigidTransform3d.IDENTITY;
    }
    R1W.mul(XAW.R, myX1A.R);
    Vector3d p1 = new Vector3d();
    Vector3d p2 = new Vector3d();
    Vector3d x21 = new Vector3d();
    p1.transform(XAW.R, myX1A.p);
    p2.transform(XBW.R, myX2B.p);
    Matrix6dBlock blk00 = getBlock(M, myBlk00Num);
    Matrix6dBlock blk11 = getBlock(M, myBlk11Num);
    Matrix6dBlock blk01 = getBlock(M, myBlk01Num);
    Matrix6dBlock blk10 = getBlock(M, myBlk10Num);
    Matrix3d T = myTmpM;
    computeRelativeDisplacements();
    x21.set(myX21.p);
    x21.transform(R1W);
    myFrameA.getVelocity(myVel1);
    if (myFrameB != null) {
        myFrameB.getVelocity(myVel2);
    } else {
        myVel2.setZero();
    }
    if (!mySymmetricJacobian) {
        mat.computeF(myF, myX21, myVel21, myInitialX21);
        // map forces back to World coords
        myF.transform(R1W);
        JD = new Matrix6d();
        mat.computeDFdu(JD, myX21, myVel21, myInitialX21, mySymmetricJacobian);
        JD.transform(R1W);
    }
    mat.computeDFdq(JK, myX21, myVel21, myInitialX21, mySymmetricJacobian);
    JK.transform(R1W);
    JK.scale(-s);
    myVel21.transform(R1W);
    if (blk00 != null) {
        myTmp00.set(JK);
        postMulMoment(myTmp00, p1);
    }
    if (blk11 != null) {
        myTmp11.set(JK);
        postMulMoment(myTmp11, p2);
    }
    if (blk01 != null && blk10 != null) {
        JK.negate();
        myTmp01.set(JK);
        postMulMoment(myTmp01, p2);
        myTmp10.set(JK);
        postMulMoment(myTmp10, p1);
    }
    if (blk00 != null) {
        if (!mySymmetricJacobian) {
            // NEW
            postMulMoment(myTmp00, x21);
            setScaledCrossProd(T, -s, myF.f);
            myTmp00.addSubMatrix03(T);
            setScaledCrossProd(T, -s, myF.m);
            myTmp00.addSubMatrix33(T);
            postMulWrenchCross(myTmp00, JD, s, myVel21);
            setScaledCrossProd(T, s, p1);
            T.crossProduct(myF.f, T);
            myTmp00.addSubMatrix33(T);
            // setScaledCrossProd (T, -s, myFk.m);
            // myTmp00.addSubMatrix33 (T);
            setScaledCrossProd(T, s, p1);
            T.crossProduct(myVel1.w, T);
            postMul03Block(myTmp00, JD, T);
            if (blk10 != null) {
                // NEW
                postMulMoment(myTmp10, x21);
                setScaledCrossProd(T, s, myF.f);
                myTmp10.addSubMatrix03(T);
                setScaledCrossProd(T, s, myF.m);
                myTmp10.addSubMatrix33(T);
                postMulWrenchCross(myTmp10, JD, -s, myVel21);
                setScaledCrossProd(T, -s, p1);
                T.crossProduct(myVel1.w, T);
                postMul03Block(myTmp10, JD, T);
            // setScaledCrossProd (T, s, myFk.m);
            // myTmp10.addSubMatrix33 (T);
            }
        }
        preMulMoment(myTmp00, p1);
        blk00.add(myTmp00);
    }
    if (blk11 != null) {
        if (!mySymmetricJacobian) {
            setScaledCrossProd(T, -s, p2);
            T.crossProduct(myF.f, T);
            myTmp11.addSubMatrix33(T);
            setScaledCrossProd(T, s, p2);
            T.crossProduct(myVel2.w, T);
            postMul03Block(myTmp11, JD, T);
            if (blk01 != null) {
                T.negate();
                postMul03Block(myTmp01, JD, T);
            }
        }
        preMulMoment(myTmp11, p2);
        blk11.add(myTmp11);
    }
    if (blk01 != null && blk10 != null) {
        preMulMoment(myTmp01, p1);
        preMulMoment(myTmp10, p2);
        blk01.add(myTmp01);
        blk10.add(myTmp10);
    }
}

Example 3 with Matrix6dBlock

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

the class FrameSpring method addSolveBlocks.

public void addSolveBlocks(SparseNumberedBlockMatrix M) {
    int bi0 = myFrameA.getSolveIndex();
    int bi1 = myFrameB != null ? myFrameB.getSolveIndex() : -1;
    if (bi0 != -1 && bi1 != -1) {
        Matrix6dBlock blk01 = (Matrix6dBlock) M.getBlock(bi0, bi1);
        if (blk01 == null) {
            blk01 = new Matrix6dBlock();
            M.addBlock(bi0, bi1, blk01);
        }
        myBlk01Num = blk01.getBlockNumber();
        Matrix6dBlock blk10 = (Matrix6dBlock) M.getBlock(bi1, bi0);
        if (blk10 == null) {
            blk10 = new Matrix6dBlock();
            M.addBlock(bi1, bi0, blk10);
        }
        myBlk10Num = blk10.getBlockNumber();
    } else {
        // blk01 = null;
        // blk10 = null;
        myBlk01Num = -1;
        myBlk10Num = -1;
    }
    // blk00 = (bi0 != -1 ? (Matrix6dBlock)M.getBlock (bi0, bi0) : null);
    // blk11 = (bi1 != -1 ? (Matrix6dBlock)M.getBlock (bi1, bi1) : null);
    myBlk00Num = (bi0 != -1 ? M.getBlock(bi0, bi0).getBlockNumber() : -1);
    myBlk11Num = (bi1 != -1 ? M.getBlock(bi1, bi1).getBlockNumber() : -1);
}

Example 4 with Matrix6dBlock

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

the class Frame method addTargetJacobian.

public int addTargetJacobian(SparseBlockMatrix J, int bi) {
    if (!isControllable()) {
        throw new IllegalStateException("Target frame is not controllable");
    }
    Matrix6dBlock blk = new Matrix6dBlock();
    blk.setIdentity();
    J.addBlock(bi, getSolveIndex(), blk);
    return bi++;
}

Example 5 with Matrix6dBlock

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

the class FrameSpring method addVelJacobianWorld.

public void addVelJacobianWorld(SparseNumberedBlockMatrix M, double s) {
    FrameMaterial mat = myMaterial;
    if (mat == null) {
        // just in case implementation allows null material ...
        return;
    }
    Matrix6dBlock blk00 = getBlock(M, myBlk00Num);
    Matrix6dBlock blk11 = getBlock(M, myBlk11Num);
    Matrix6dBlock blk01 = getBlock(M, myBlk01Num);
    Matrix6dBlock blk10 = getBlock(M, myBlk10Num);
    Matrix6d D = new Matrix6d();
    RotationMatrix3d R1W = new RotationMatrix3d();
    RigidTransform3d XAW = myFrameA.getPose();
    RigidTransform3d XBW;
    if (myFrameB != null) {
        XBW = myFrameB.getPose();
    } else {
        XBW = RigidTransform3d.IDENTITY;
    }
    R1W.mul(XAW.R, myX1A.R);
    Vector3d p1 = new Vector3d();
    Vector3d p2 = new Vector3d();
    p1.transform(XAW.R, myX1A.p);
    p2.transform(XBW.R, myX2B.p);
    // Matrix3d T = myTmpM;
    computeRelativeDisplacements();
    mat.computeDFdu(D, myX21, myVel21, myInitialX21, mySymmetricJacobian);
    D.transform(R1W);
    D.scale(-s);
    if (blk00 != null) {
        myTmp00.set(D);
        postMulMoment(myTmp00, p1);
        preMulMoment(myTmp00, p1);
        blk00.add(myTmp00);
    }
    if (blk11 != null) {
        myTmp11.set(D);
        postMulMoment(myTmp11, p2);
        preMulMoment(myTmp11, p2);
        blk11.add(myTmp11);
    }
    if (blk01 != null && blk10 != null) {
        D.negate();
        myTmp01.set(D);
        postMulMoment(myTmp01, p2);
        preMulMoment(myTmp01, p1);
        myTmp10.set(D);
        postMulMoment(myTmp10, p1);
        preMulMoment(myTmp10, p2);
        blk01.add(myTmp01);
        blk10.add(myTmp10);
    }
}