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

Example 71 with RigidTransform3d

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

the class RevoluteJoint method getTheta.

public double getTheta() {
    // RigidTransform3d XAW = myBodyA.getPose();
    // RigidTransform3d XBW =
    // myBodyB != null ? myBodyB.getPose() : RigidTransform3d.IDENTITY;
    // 
    // // initialize TGD to TCD; it will get projected to TGD within
    // // myCoupling.getTheta();
    // RigidTransform3d TCA = new RigidTransform3d();
    // RigidTransform3d TGD = new RigidTransform3d();
    // getCurrentTCA (TCA);
    // getCurrentTDB (TGD);
    // TGD.mulInverseBoth (TGD, XBW);
    // TGD.mul (XAW);
    // TGD.mul (TCA);
    // initialize TGD to TCD; it will get projected to TGD within
    // myCoupling.getTheta();
    RigidTransform3d TGD = new RigidTransform3d();
    getCurrentTCD(TGD);
    double theta = Math.toDegrees(((RevoluteCoupling) myCoupling).getTheta(TGD));
    return theta;
}

Example 72 with RigidTransform3d

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

the class SegmentedPlanarConnector method doset.

// private void makePlanesFromSegments (
// RigidTransform3d XPB, ArrayList<Point3d> segPoints)
// {
// myPlanes = new ArrayList<Plane>();
// // now create the planes
// Vector3d u = new Vector3d();
// Vector3d nrm = new Vector3d();
// Vector3d y = new Vector3d (XPB.R.m01, XPB.R.m11, XPB.R.m21);
// for (int i=1; i<segPoints.size(); i++)
// { u.sub (segPoints.get(i), segPoints.get(i-1));
// nrm.cross (u, y);
// nrm.normalize();
// myPlanes.add (new Plane (nrm, nrm.dot(segPoints.get(i))));
// }
// }
// private void makeSegmentPoints (
// RigidTransform3d XPB, double[] segs)
// {
// myPoints = new ArrayList<Point3d>();
// // make private copy of the points
// for (int i=0; i<segs.length; i+=2)
// { Point3d pnt = new Point3d(segs[i], 0.0, segs[i+1]);
// pnt.transform (XPB);
// myPoints.add (pnt);
// }
// }
private void doset(RigidBody bodyA, Vector3d pCA, RigidBody bodyB, RigidTransform3d XDB, double[] segs) {
    RigidTransform3d TCA = new RigidTransform3d();
    TCA.p.set(pCA);
    setBodies(bodyA, TCA, bodyB, XDB);
    mySegPlaneCoupling.setSegments(segs);
}

Example 73 with RigidTransform3d

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

the class SegmentedPlanarConnector method render.

public void render(Renderer renderer, int flags) {
    Vector3d nrm = new Vector3d(0, 0, 1);
    RigidTransform3d TDW = getCurrentTDW();
    RenderProps props = myRenderProps;
    Shading savedShading = renderer.setPropsShading(props);
    renderer.setFaceColoring(props, isSelected());
    renderer.setFaceStyle(props.getFaceStyle());
    ArrayList<Plane> planes = mySegPlaneCoupling.getPlanes();
    for (int i = 0; i < planes.size(); i++) {
        Plane plane = planes.get(i);
        nrm.set(plane.getNormal());
        computeRenderVtxs(i, TDW);
        renderer.beginDraw(DrawMode.TRIANGLE_STRIP);
        if (myRenderNormalReversedP) {
            renderer.setNormal(-nrm.x, -nrm.y, -nrm.z);
        } else {
            renderer.setNormal(nrm.x, nrm.y, nrm.z);
        }
        renderer.addVertex(myRenderVtxs[3]);
        renderer.addVertex(myRenderVtxs[0]);
        renderer.addVertex(myRenderVtxs[2]);
        renderer.addVertex(myRenderVtxs[1]);
        renderer.endDraw();
    }
    renderer.setShading(savedShading);
    renderer.setFaceStyle(FaceStyle.FRONT);
    renderer.drawPoint(myRenderProps, myRenderCoords, isSelected());
}

Example 74 with RigidTransform3d

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

the class PlanarConnector method set.

/**
 * Sets this PlanarConnectorX to connect a rigid body with the world frame.
 * The contact point is fixed in the body frame, while the plane is fixed in
 * the world frame.
 *
 * @param bodyA
 * rigid body
 * @param pCA
 * location of contact point relative to body
 * @param XPW
 * plane coordinate frame with respect to the world. The plane normal is
 * given by the z axis of this frame, and the plane's origin is given by
 * XPB.p
 */
public void set(RigidBody bodyA, Vector3d pCA, RigidTransform3d XPW) {
    RigidTransform3d TCA = new RigidTransform3d();
    TCA.p.set(pCA);
    setBodies(bodyA, TCA, null, XPW);
}

Example 75 with RigidTransform3d

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

the class CutPlaneProbe method getPlane.

/**
 * Gets the plane along which the display lies
 */
public void getPlane(Plane plane) {
    RigidTransform3d X = XGridToWorld;
    plane.set(-X.R.m02, -X.R.m12, -X.R.m22, -X.R.m02 * X.p.x - X.R.m12 * X.p.y - X.R.m22 * X.p.z);
}