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

Example 86 with RigidTransform3d

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

the class FemCollision method positionBall.

void positionBall(FemModel3d ball, Vector3d startPos) {
    if (ball == null)
        return;
    Vector3d avg = new Vector3d();
    for (FemNode3d n : ball.getNodes()) avg.add(n.getPosition());
    avg.scale(1 / (double) ball.getNodes().size());
    Vector3d pos = new Vector3d(startPos);
    pos.sub(avg);
    ball.transformGeometry(new RigidTransform3d(pos, new AxisAngle()));
    for (FemNode3d n : ball.getNodes()) {
        n.setVelocity(0, 0, 0);
    }
}

Example 87 with RigidTransform3d

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

the class DicomLoader method addPlane.

void addPlane() {
    if (viewer == null) {
        return;
    }
    DicomImage image = viewer.getImage();
    Point3d pmin = new Point3d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
    Point3d pmax = new Point3d(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY);
    viewer.updateBounds(pmin, pmax);
    // center transform
    RigidTransform3d trans = new RigidTransform3d();
    trans.p.interpolate(pmin, 0.5, pmax);
    Vector2d size = new Vector2d(pmax.x - pmin.x, pmax.y - pmin.y);
    DicomPlaneViewer dpv = new DicomPlaneViewer("plane_" + viewerPlanes.size(), image, trans, size);
    addRenderable(dpv);
    viewerPlanes.add(dpv);
}

Example 88 with RigidTransform3d

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

the class ArticulatedFem method build.

public void build(String[] args) {
    int nlinks = 3;
    int nelemsx = 6;
    int nelemsz = 2;
    double femLength = 0.6;
    double femHeight = 0.2;
    double boxLength = 0.1;
    double boxHeight = 0.3;
    double linkLength = femLength + 2 * boxLength;
    myMechMod = new MechModel("mech");
    RigidTransform3d X = new RigidTransform3d();
    RigidBody lastBox = null;
    double linkCenter;
    RigidBody leftAnchorBox = makeBox();
    linkCenter = linkLength * (-nlinks / 2.0 + 0.5);
    X.p.set(linkCenter - (boxLength + femLength) / 2, 0, boxHeight);
    leftAnchorBox.setPose(X);
    leftAnchorBox.setDynamic(false);
    // myMechMod.addRigidBody (leftAnchorBox);
    RigidBody rightAnchorBox = makeBox();
    linkCenter = linkLength * (-nlinks / 2.0 + (nlinks - 1) + 0.5);
    X.p.set(linkCenter + (boxLength + femLength) / 2, 0, boxHeight);
    rightAnchorBox.setPose(X);
    rightAnchorBox.setDynamic(false);
    for (int i = 0; i < nlinks; i++) {
        linkCenter = linkLength * (-nlinks / 2.0 + i + 0.5);
        LinkedList<FemNode3d> leftNodes = new LinkedList<FemNode3d>();
        LinkedList<FemNode3d> rightNodes = new LinkedList<FemNode3d>();
        FemModel3d femMod = FemFactory.createTetGrid(null, femLength, femHeight, femHeight, nelemsx, nelemsz, nelemsz);
        femMod.setDensity(myDensity);
        femMod.setLinearMaterial(200000, 0.4, true);
        femMod.setGravity(0, 0, -9.8);
        double eps = 0.000001;
        for (FemNode3d n : femMod.getNodes()) {
            double x = n.getPosition().x;
            if (x <= -femLength / 2 + eps) {
                leftNodes.add(n);
            } else if (x >= femLength / 2 - eps) {
                rightNodes.add(n);
            }
        }
        femMod.setStiffnessDamping(0.002);
        RenderProps.setLineWidth(femMod.getElements(), 2);
        RenderProps.setLineColor(femMod.getElements(), Color.BLUE);
        RenderProps.setPointStyle(femMod.getNodes(), Renderer.PointStyle.SPHERE);
        RenderProps.setPointRadius(femMod.getNodes(), 0.005);
        femMod.setSurfaceRendering(SurfaceRender.Shaded);
        RenderProps.setFaceColor(femMod, new Color(0f, 0.7f, 0.7f));
        RenderProps.setLineColor(femMod.getElements(), new Color(0f, 0.2f, 0.2f));
        RenderProps.setLineWidth(femMod.getElements(), 3);
        X.p.set(linkCenter, 0, 0);
        femMod.transformGeometry(X);
        myMechMod.addModel(femMod);
        RigidBody leftBox = makeBox();
        X.p.set(linkCenter - (boxLength + femLength) / 2, 0, 0);
        leftBox.setPose(X);
        myMechMod.addRigidBody(leftBox);
        RigidBody rightBox = makeBox();
        X.p.set(linkCenter + (boxLength + femLength) / 2, 0, 0);
        rightBox.setPose(X);
        myMechMod.addRigidBody(rightBox);
        for (FemNode3d n : leftNodes) {
            myMechMod.attachPoint(n, leftBox);
        }
        for (FemNode3d n : rightNodes) {
            myMechMod.attachPoint(n, rightBox);
        }
        RigidTransform3d TCA = new RigidTransform3d();
        RigidTransform3d TCB = new RigidTransform3d();
        RevoluteJoint joint;
        TCA.p.set(-boxLength / 2, 0, boxHeight / 2);
        TCA.R.mulAxisAngle(1, 0, 0, Math.PI / 2);
        if (lastBox == null) {
            TCB.mul(leftBox.getPose(), TCA);
            // TCB.mulInverseLeft (leftAnchorBox.getPose(), TCB);
            joint = new RevoluteJoint(leftBox, TCA, TCB);
        } else {
            TCB.p.set(boxLength / 2, 0, boxHeight / 2);
            TCB.R.mulAxisAngle(1, 0, 0, Math.PI / 2);
            joint = new RevoluteJoint(leftBox, TCA, lastBox, TCB);
        }
        RenderProps.setLineRadius(joint, 0.01);
        RenderProps.setLineColor(joint, new Color(0.15f, 0.15f, 1f));
        joint.setAxisLength(0.5);
        myMechMod.addBodyConnector(joint);
        if (addLastJoint && i == nlinks - 1) {
            TCA.p.set(boxLength / 2, 0, boxHeight / 2);
            TCB.mul(rightBox.getPose(), TCA);
            // TCB.mulInverseLeft (rightAnchorBox.getPose(), TCB);
            joint = new RevoluteJoint(rightBox, TCA, TCB);
            RenderProps.setLineRadius(joint, 0.01);
            RenderProps.setLineColor(joint, new Color(0.15f, 0.15f, 1f));
            joint.setAxisLength(0.5);
            myMechMod.addBodyConnector(joint);
        }
        lastBox = rightBox;
    }
    if (!addLastJoint) {
        lastBox.setDynamic(false);
    }
    myMechMod.setIntegrator(Integrator.BackwardEuler);
    addModel(myMechMod);
    addControlPanel(myMechMod);
}

Example 89 with RigidTransform3d

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

the class HydrostatDemo method addContactBlock.

public void addContactBlock(MechModel mech) {
    RigidBody block = new RigidBody();
    block.setName("block");
    block.setMesh(MeshFactory.createQuadBox(100, 100, 20), null);
    block.setInertia(SpatialInertia.createBoxInertia(1, 100, 100, 20));
    block.setPose(new RigidTransform3d(new Vector3d(-50, 0, 40), new RotationMatrix3d()));
    mech.addRigidBody(block);
}

Example 90 with RigidTransform3d

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

the class GridPlane method setOrientation.

/**
 * Sets the orientation of the display
 */
public void setOrientation(AxisAngle axisAng) {
    RigidTransform3d X = new RigidTransform3d(XGridToWorld);
    X.R.setAxisAngle(axisAng);
    setGridToWorld(X);
}