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

Example 26 with RotationMatrix3d

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

the class LinearMaterialBase method computeRotation.

protected RotationMatrix3d computeRotation(Matrix3d F, SymmetricMatrix3d P) {
    if (mySVD == null) {
        mySVD = new SVDecomposition3d();
    }
    RotationMatrix3d R = new RotationMatrix3d();
    mySVD.polarDecomposition(R, P, F);
    return R;
}

Example 27 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d 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 28 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d 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 29 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d 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 30 with RotationMatrix3d

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

the class ParticlePlaneConstraint method computeRenderVtxs.

protected void computeRenderVtxs() {
    RotationMatrix3d RPD = new RotationMatrix3d();
    RPD.setZDirection(myNrm);
    myRenderVtxs[0].set(myPlaneSize / 2, myPlaneSize / 2, 0);
    myRenderVtxs[1].set(-myPlaneSize / 2, myPlaneSize / 2, 0);
    myRenderVtxs[2].set(-myPlaneSize / 2, -myPlaneSize / 2, 0);
    myRenderVtxs[3].set(myPlaneSize / 2, -myPlaneSize / 2, 0);
    for (int i = 0; i < myRenderVtxs.length; i++) {
        myRenderVtxs[i].transform(RPD);
        myRenderVtxs[i].add(myCenter);
    }
}