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

Example 56 with RigidTransform3d

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

the class FemModel3d method attachFrame.

public void attachFrame(RigidTransform3d TRW) {
    if (TRW == null) {
        myFrameConstraint = null;
        myFrame.setPose(RigidTransform3d.IDENTITY);
        myFrame.setVelocity(Twist.ZERO);
    } else {
        RigidTransform3d TX = new RigidTransform3d(TRW);
        Point3d pos = new Point3d(TRW.p);
        FemElement3d elem = findContainingElement(pos);
        if (elem == null) {
            Point3d newLoc = new Point3d();
            elem = findNearestSurfaceElement(newLoc, pos);
            TX.p.set(newLoc);
        }
        myFrame.setPose(TX);
        myFrameConstraint = new FrameFem3dConstraint(myFrame, elem);
    }
}

Example 57 with RigidTransform3d

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

the class FemModel3d method createFrameAttachment.

/**
 * {@inheritDoc}
 */
public FrameFem3dAttachment createFrameAttachment(Frame frame, RigidTransform3d TFW) {
    if (frame == null && TFW == null) {
        throw new IllegalArgumentException("frame and TFW cannot both be null");
    }
    if (frame != null && frame.isAttached()) {
        throw new IllegalArgumentException("frame is already attached");
    }
    Point3d loc = new Point3d();
    Point3d pos = new Point3d(TFW != null ? TFW.p : frame.getPose().p);
    FemElement3d elem = findNearestElement(loc, pos);
    if (!loc.equals(pos)) {
        TFW = new RigidTransform3d(TFW);
        TFW.p.set(loc);
    }
    FrameFem3dAttachment ffa = new FrameFem3dAttachment(frame);
    ffa.setFromElement(TFW, elem);
    if (frame != null) {
        if (DynamicAttachment.containsLoop(ffa, frame, null)) {
            throw new IllegalArgumentException("attachment contains loop");
        }
    }
    return ffa;
}

Example 58 with RigidTransform3d

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

the class FemModel3d method createFrameAttachment.

/**
 * Returns a FrameAttachment that attaches a <code>frame</code> to this
 * component. Once attached the frame will follow the body around.  The
 * initial pose of the frame is specified by <code>TFW</code>, which gives
 * its position and orientation in world coordinates. If <code>TFW</code> is
 * <code>null</code>, then the current pose of the frame is used. If
 * <code>frame</code> is <code>null</code>, then a virtual attachment is
 * created at the initial pose specified by
 * <code>TFW</code>. <code>frame</code> and <code>TFW</code> cannot both be
 * <code>null</code>.
 *
 * @param frame frame to be attached
 * @param TFW transform from (initial) frame coordinates to world
 * coordinates
 * @param project if true and if the frame is outside the FEM, then
 * the frame gets projected to the nearest point on the FEM
 * @return attachment attaching <code>frame</code> to this component
 */
public FrameFem3dAttachment createFrameAttachment(Frame frame, RigidTransform3d TFW, boolean project) {
    if (frame == null && TFW == null) {
        throw new IllegalArgumentException("frame and TFW cannot both be null");
    }
    if (frame != null && frame.isAttached()) {
        throw new IllegalArgumentException("frame is already attached");
    }
    Point3d loc = new Point3d();
    Point3d pos = new Point3d(TFW != null ? TFW.p : frame.getPose().p);
    FemElement3d elem = findNearestElement(loc, pos);
    if (project && !loc.equals(pos)) {
        TFW = new RigidTransform3d(TFW);
        TFW.p.set(loc);
    }
    FrameFem3dAttachment ffa = new FrameFem3dAttachment(frame);
    ffa.setFromElement(TFW, elem);
    if (frame != null) {
        if (DynamicAttachment.containsLoop(ffa, frame, null)) {
            throw new IllegalArgumentException("attachment contains loop");
        }
    }
    return ffa;
}

Example 59 with RigidTransform3d

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

the class SkinMeshBody method scanFrameInfo.

protected void scanFrameInfo(ReaderTokenizer rtok, Deque<ScanToken> tokens) throws IOException {
    myFrameInfo.clear();
    rtok.scanToken('[');
    while (ScanWriteUtils.scanAndStoreReference(rtok, tokens)) {
        RigidTransform3d basePose = new RigidTransform3d();
        basePose.scan(rtok);
        // frame will be filled in by postscan ...
        FrameInfo finfo = new FrameInfo();
        finfo.myBasePose.set(basePose);
        finfo.setIndex(myFrameInfo.size());
        myFrameInfo.add(finfo);
    }
    if (rtok.ttype != ']') {
        throw new IOException("Expected ']', got " + rtok);
    }
}

Example 60 with RigidTransform3d

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

the class RigidBodyAgent method updateDisplayGeometry.

private void updateDisplayGeometry() {
    if (body != null) {
        RigidTransform3d X = new RigidTransform3d();
        X.p.set(positionField.getVectorValue());
        X.R.setAxisAngle(orientationField.getAxisAngleValue());
        body.setPose(X);
        rotator.setDraggerToWorld(X);
        myMain.rerender();
    }
}