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

Example 61 with RigidTransform3d

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

the class RigidBodyAgent method createPreviewBody.

private void createPreviewBody() {
    resetScaling();
    body = new RigidBody();
    setProperties(body, getPrototypeComponent(myComponentType));
    setProperties(myPrototype, myPrototype);
    body.setMesh(myGeometryPanel.getMesh(), myGeometryPanel.getMeshFileName(), myGeometryPanel.getMeshTransform());
    RigidTransform3d X = new RigidTransform3d();
    X.p.set(positionField.getVectorValue());
    X.R.setAxisAngle(orientationField.getAxisAngleValue());
    body.setPose(X);
    RenderProps props = body.createRenderProps();
    props.setFaceStyle(Renderer.FaceStyle.NONE);
    props.setDrawEdges(true);
    props.setLineColor(Color.LIGHT_GRAY);
    body.setRenderProps(props);
    myMain.getWorkspace().getViewerManager().addRenderable(body);
    rotator = new Transrotator3d();
    GLViewer viewer = myMain.getMain().getViewer();
    rotator.setDraggerToWorld(X);
    rotator.setSize(viewer.distancePerPixel(viewer.getCenter()) * viewer.getScreenWidth() / 6);
    rotator.addListener(new RigidBodyDraggerListener());
    myMain.getWorkspace().getViewerManager().addDragger(rotator);
    myGeometryPanel.setAttachedBody(body);
    myMain.rerender();
}

Example 62 with RigidTransform3d

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

the class ForceTarget method prerender.

@Override
public void prerender(RenderList list) {
    super.prerender(list);
    if (myConnector instanceof PlanarConnector) {
        RigidTransform3d TDW = myConnector.getCurrentTDW();
        startvec = TDW.p;
        endvec.transform(TDW, Vector3d.Z_UNIT);
        endvec.scale(arrowSize / endvec.norm());
        endvec.scale(myTargetLambda.get(0));
        endvec.add(startvec);
        set(start, startvec);
        set(end, endvec);
    } else if (myConnector instanceof SphericalJointBase) {
        startvec = myConnector.getCurrentTCW().p;
        endvec.x = myTargetLambda.get(0) * arrowSize;
        endvec.y = myTargetLambda.get(1) * arrowSize;
        endvec.z = myTargetLambda.get(2) * arrowSize;
        endvec.add(startvec);
        set(start, startvec);
        set(end, endvec);
    }
}

Example 63 with RigidTransform3d

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

the class JointBase method prerender.

public void prerender(RenderList list) {
    RigidTransform3d TDW = getCurrentTDW();
    myRenderFrameD.set(TDW);
    RigidTransform3d TCW = getCurrentTCW();
    myRenderFrameC.set(TCW);
}

Example 64 with RigidTransform3d

use of maspack.matrix.RigidTransform3d 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 65 with RigidTransform3d

use of maspack.matrix.RigidTransform3d 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;
}