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

Example 6 with Matrix6d

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

the class MooneyRivlinMaterial method main.

public static void main(String[] args) {
    MooneyRivlinMaterial mat = new MooneyRivlinMaterial();
    SolidDeformation def = new SolidDeformation();
    Matrix3d Q = new Matrix3d();
    def.setF(new Matrix3d(1, 3, 5, 2, 1, 4, 6, 1, 2));
    Matrix6d D = new Matrix6d();
    SymmetricMatrix3d sig = new SymmetricMatrix3d();
    mat.setC10(1.2);
    mat.setC01(2.4);
    mat.setC11(3.5);
    mat.setC02(2.6);
    mat.setC20(1.9);
    mat.setBulkModulus(0.0);
    mat.computeStress(sig, def, Q, null);
    // pt.setStress (sig);
    mat.computeTangent(D, sig, def, Q, null);
    System.out.println("sig=\n" + sig.toString("%12.6f"));
    System.out.println("D=\n" + D.toString("%12.6f"));
}

Example 7 with Matrix6d

use of maspack.matrix.Matrix6d 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 8 with Matrix6d

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

the class FrameSpring method copy.

@Override
public ModelComponent copy(int flags, Map<ModelComponent, ModelComponent> copyMap) {
    FrameSpring comp = (FrameSpring) super.copy(flags, copyMap);
    if (myRenderProps != null) {
        comp.setRenderProps(myRenderProps);
    }
    if (myX1A != RigidTransform3d.IDENTITY) {
        comp.myX1A = new RigidTransform3d(myX1A);
    }
    if (myX2B != RigidTransform3d.IDENTITY) {
        comp.myX2B = new RigidTransform3d(myX2B);
    }
    comp.myF = new Wrench();
    comp.myFTmp = new Wrench();
    comp.myVel1 = new Twist();
    comp.myVel2 = new Twist();
    comp.myVel21 = new Twist();
    comp.myTmp00 = new Matrix6d();
    comp.myTmp01 = new Matrix6d();
    comp.myTmp10 = new Matrix6d();
    comp.myTmp11 = new Matrix6d();
    comp.myRenderFrame = new RigidTransform3d();
    comp.myRenderPnt1 = new float[3];
    comp.myRenderPnt2 = new float[3];
    comp.myTmpM = new Matrix3d();
    // comp.myRBA = new RotationMatrix3d();
    comp.myX21 = new RigidTransform3d();
    comp.myMaterial = (FrameMaterial) myMaterial.clone();
    return comp;
}

Example 9 with Matrix6d

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

the class StVenantKirchoffMaterial method main.

public static void main(String[] args) {
    StVenantKirchoffMaterial mat = new StVenantKirchoffMaterial();
    SolidDeformation def = new SolidDeformation();
    Matrix3d Q = new Matrix3d();
    def.setF(new Matrix3d(1, 3, 5, 2, 1, 4, 6, 1, 2));
    Matrix6d D = new Matrix6d();
    SymmetricMatrix3d sig = new SymmetricMatrix3d();
    mat.setYoungsModulus(10);
    mat.computeStress(sig, def, Q, null);
    mat.computeTangent(D, sig, def, Q, null);
    System.out.println("sig=\n" + sig.toString("%12.6f"));
    System.out.println("D=\n" + D.toString("%12.6f"));
}

Example 10 with Matrix6d

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

the class SpatialInertiaTest method test.

public void test() throws Exception {
    SpatialInertia M1 = new SpatialInertia();
    SpatialInertia M2 = new SpatialInertia();
    SpatialInertia M3 = new SpatialInertia();
    double mass;
    Point3d com = new Point3d();
    SymmetricMatrix3d J = new SymmetricMatrix3d();
    Matrix3d F = new Matrix3d();
    for (int i = 0; i < 100; i++) {
        M1.setRandom(-0.5, 0.5, randGen);
        String s = M1.toString();
        M2.scan(new ReaderTokenizer(new StringReader(s)));
        checkEqual("toString/scan MASS_INERTIA check", M2, M1, EPS);
        s = M1.toString("%g", SpatialInertia.MATRIX_STRING);
        M2.scan(new ReaderTokenizer(new StringReader(s)));
        checkEqual("toString/scan MATRIX_STRING check", M2, M1, EPS);
        MatrixNd MA = new MatrixNd(0, 0);
        MatrixNd MB = new MatrixNd(0, 0);
        MA.set(M1);
        M2.set(MA);
        mass = randGen.nextDouble();
        com.setRandom(-0.5, 0.5, randGen);
        F.setRandom(-0.5, 0.5, randGen);
        J.mulTransposeLeft(F);
        M1.setMass(mass);
        M1.setCenterOfMass(com);
        M1.setRotationalInertia(J);
        checkComponents("set component check", M1, mass, com, J);
        M2.set(M1);
        checkComponents("set check", M2, mass, com, J);
        M1.set(mass, J, com);
        checkComponents("set check", M1, mass, com, J);
        M1.set(mass, J);
        com.setZero();
        checkComponents("set check", M1, mass, com, J);
        M1.setRandom();
        MA.set(M1);
        MB.set(M2);
        // check addition
        M2.add(M1);
        MB.add(MA);
        M3.set(MB);
        checkEqual("add check", M2, M3, EPS);
        // check subtraction
        MB.sub(MA);
        M2.sub(M1);
        M3.set(MB);
        checkEqual("sub check", M2, M3, EPS);
        // check scaling
        MB.scale(3);
        M2.scale(3);
        M3.set(MB);
        checkEqual("scale check", M2, M3, EPS);
        MatrixNd invM2 = new MatrixNd(0, 0);
        MatrixNd invMB = new MatrixNd(0, 0);
        // check inversion
        MB.set(M2);
        M2.getInverse(invM2);
        invMB.invert(MB);
        double eps = invM2.frobeniusNorm() * EPS;
        checkEqual("inverse check", invM2, invMB, eps);
        Matrix6d invM6 = new Matrix6d();
        SpatialInertia.invert(invM6, M2);
        checkEqual("inverse check", invM6, invMB, eps);
        CholeskyDecomposition chol = new CholeskyDecomposition();
        chol.factor(MB);
        MatrixNd U = new MatrixNd(6, 6);
        MatrixNd L = new MatrixNd(6, 6);
        chol.get(L);
        U.transpose(L);
        MatrixNd invU = new MatrixNd(6, 6);
        MatrixNd invL = new MatrixNd(6, 6);
        invU.invert(U);
        invL.transpose(invU);
        VectorNd vec = new VectorNd(6);
        VectorNd res = new VectorNd(6);
        Twist tw1 = new Twist();
        Twist twr = new Twist();
        Twist twrCheck = new Twist();
        Wrench wr1 = new Wrench();
        Wrench wrr = new Wrench();
        Wrench wrrCheck = new Wrench();
        // check multiply by vector (twist)
        tw1.setRandom();
        M2.mul(wrr, tw1);
        vec.set(tw1);
        MB.mul(res, vec);
        wrrCheck.set(res);
        checkEqual("mul check", wrr, wrrCheck, EPS);
        // check inverse multiply by vector (wrench)
        wr1.setRandom();
        M2.mulInverse(twr, wr1);
        vec.set(wr1);
        invMB.mul(res, vec);
        twrCheck.set(res);
        checkEqual("inverse mul check", twr, twrCheck, eps);
        // check inverse multiply by vector (twist, twist)
        wr1.setRandom();
        twr.set(wr1);
        M2.mulInverse(twr, twr);
        vec.set(wr1);
        invMB.mul(res, vec);
        twrCheck.set(res);
        checkEqual("inverse mul check", twr, twrCheck, eps);
        // check right factor multiply by vector
        tw1.setRandom();
        M2.mulRightFactor(twr, tw1);
        vec.set(tw1);
        U.mul(res, vec);
        twrCheck.set(res);
        checkEqual("mul right factor", twr, twrCheck, EPS);
        M2.mulRightFactor(tw1, tw1);
        checkEqual("mul right factor (twr == tw1)", tw1, twrCheck, EPS);
        // check right factor inverse multiply by vector
        tw1.setRandom();
        M2.mulRightFactorInverse(twr, tw1);
        vec.set(tw1);
        invU.mul(res, vec);
        twrCheck.set(res);
        checkEqual("mul right factor inverse", twr, twrCheck, eps);
        M2.mulRightFactorInverse(tw1, tw1);
        checkEqual("mul right factor inverse (twr == tw1)", tw1, twrCheck, eps);
        // check left factor multiply by vector
        wr1.setRandom();
        M2.mulLeftFactor(wrr, wr1);
        vec.set(wr1);
        L.mul(res, vec);
        wrrCheck.set(res);
        checkEqual("mul left factor", wrr, wrrCheck, EPS);
        M2.mulLeftFactor(wr1, wr1);
        checkEqual("mul left factor (wrr == wr1)", wr1, wrrCheck, EPS);
        // check left factor inverse multiply by vector
        wr1.setRandom();
        M2.mulLeftFactorInverse(wrr, wr1);
        vec.set(wr1);
        invL.mul(res, vec);
        wrrCheck.set(res);
        checkEqual("mul left factor inverse", wrr, wrrCheck, eps);
        M2.mulLeftFactorInverse(wr1, wr1);
        checkEqual("mul left factor inverse (wrr == wr1)", wr1, wrrCheck, eps);
        // check addition of point mass
        Matrix6d M6 = new Matrix6d();
        SpatialInertia MCheck = new SpatialInertia();
        MCheck.set(M1);
        M6.set(M1);
        Point3d com1 = new Point3d();
        double mass1 = randGen.nextDouble();
        com1.setRandom(-0.5, 0.5, randGen);
        Point3d com2 = new Point3d();
        double mass2 = randGen.nextDouble();
        com2.setRandom(-0.5, 0.5, randGen);
        M3.setPointMass(mass1, com1);
        MCheck.add(M3);
        M3.setPointMass(mass2, com2);
        MCheck.add(M3);
        M1.addPointMass(mass1, com1);
        M1.addPointMass(mass2, com2);
        checkEqual("point mass addition", M1, MCheck, EPS);
        SpatialInertia.addPointMass(M6, mass1, com1);
        SpatialInertia.addPointMass(M6, mass2, com2);
        Matrix6d MM = new Matrix6d();
        MM.sub(MCheck, M6);
        checkEqual("point mass addition", M6, MCheck, EPS);
        // check rotation transforms
        RotationMatrix3d R = new RotationMatrix3d();
        SpatialInertia MSave = new SpatialInertia(M1);
        R.setRandom();
        M1.getRotated(M6, R);
        MCheck.set(M1);
        MCheck.transform(R);
        checkEqual("getRotated", M6, MCheck, EPS);
        MCheck.inverseTransform(R);
        checkEqual("inverseTransform", MCheck, MSave, EPS);
        R.transpose();
        M1.set(M6);
        M1.getRotated(M6, R);
        // should be back to original
        checkEqual("getRotated (back)", M6, MSave, EPS);
    }
// // test creation of inertia from a mesh
// double density = 20;
// double wx = 3.0;
// double wy = 1.0;
// double wz = 2.0;
// SpatialInertia boxInertia =
// SpatialInertia.createBoxInertia (20 * wx * wy * wz, wx, wy, wz);
// PolygonalMesh boxMesh = MeshFactory.createQuadBox (wx, wy, wz);
// SpatialInertia meshInertia =
// SpatialInertia.createClosedVolumeInertia (boxMesh, density);
// 
// checkMeshInertia (boxMesh, density, boxInertia);
// 
// SpatialInertia innerBoxInertia =
// SpatialInertia.createBoxInertia (1.0 * density, 1, 1, 1);
// boxInertia.setBox (20 * wx * wy * wz, wx, wy, wz);
// boxInertia.sub (innerBoxInertia);
// boxMesh = buildMeshWithHole (wx, wy, wz);
// 
// checkMeshInertia (boxMesh, density, boxInertia);
// 
// boxMesh.triangulate();
// checkMeshInertia (boxMesh, density, boxInertia);
}