Examples with FrameMaterial artisynth.core.materials.FrameMaterial used on opensource projects

Example 1 with FrameMaterial

use of artisynth.core.materials.FrameMaterial 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 2 with FrameMaterial

use of artisynth.core.materials.FrameMaterial in project artisynth_core by artisynth.

the class FrameSpring method computeForces.

// Computes the force as seen in frame 1 and places the resukt in myF.
protected void computeForces() {
    FrameMaterial mat = myMaterial;
    if (mat != null) {
        // just in case implementation allows null material ...
        computeRelativeDisplacements();
        mat.computeF(myF, myX21, myVel21, myInitialX21);
    }
}

Example 3 with FrameMaterial

use of artisynth.core.materials.FrameMaterial 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);
    }
}