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

Example 21 with AxisAngle

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

the class AxisAngleField method textToValue.

public Object textToValue(String text, BooleanHolder corrected, StringHolder errMsg) {
    corrected.value = false;
    ReaderTokenizer rtok = new ReaderTokenizer(new StringReader(text));
    VectorNd tmp = new VectorNd(4);
    try {
        for (int i = 0; i < 4; i++) {
            if (rtok.nextToken() != ReaderTokenizer.TT_NUMBER) {
                return illegalValue("Missing or malformed number for element " + i, errMsg);
            }
            tmp.set(i, rtok.nval);
        }
        if (rtok.nextToken() != ReaderTokenizer.TT_EOF) {
            // check if specified degrees or radians
            if (rtok.tokenIsWord()) {
                if (rtok.sval.toLowerCase().equals("r") || rtok.sval.toLowerCase().equals("rad") || rtok.sval.toLowerCase().equals("radians")) {
                    // radians
                    tmp.set(3, Math.toDegrees(tmp.get(3)));
                } else if (rtok.sval.toLowerCase().equals("d") || rtok.sval.toLowerCase().equals("deg") || rtok.sval.toLowerCase().equals("degrees")) {
                // already in degrees
                } else {
                    return illegalValue("Only angle unit specifier allowed after last entry", errMsg);
                }
            } else {
                return illegalValue("Extra characters after last element", errMsg);
            }
        }
        return validValue(new AxisAngle(tmp.get(0), tmp.get(1), tmp.get(2), Math.toRadians(tmp.get(3))), errMsg);
    } catch (Exception e) {
        return illegalValue("Improperly formed 4-tuple", errMsg);
    }
}

Example 22 with AxisAngle

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

the class GLViewer method rotateFixedUp.

/**
 * Rotate the eye coordinate frame about the center point, while maintaining
 * the default up vector.
 *
 * @param xang
 * amount of horizontal rotation (in radians)
 * @param yang
 * amount of vertical rotation (in radians)
 * @see #getUpVector
 */
protected void rotateFixedUp(double xang, double yang) {
    Vector3d reye = new Vector3d();
    reye.sub(getEye(), myViewState.myCenter);
    if (yang != 0) {
        // right-facing vector
        Vector3d xCam = new Vector3d();
        synchronized (viewMatrix) {
            viewMatrix.R.getRow(0, xCam);
        }
        double oldAngle = Math.acos(reye.dot(myViewState.myUp) / reye.norm());
        if (!((yang < 0 && (-yang) > oldAngle) || (yang > 0 && yang > (Math.PI - oldAngle)))) {
            reye.transform(new RotationMatrix3d(new AxisAngle(xCam, yang)));
        }
    }
    if (xang != 0) {
        reye.transform(new RotationMatrix3d(new AxisAngle(myViewState.myUp, xang)));
    }
    Point3d eye = new Point3d();
    eye.add(reye, myViewState.myCenter);
    setEyeToWorld(eye, myViewState.myCenter, myViewState.myUp);
    repaint();
}

Example 23 with AxisAngle

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

the class Transrotator3d method updateRotation.

protected void updateRotation(RotationMatrix3d Rot, boolean constrained) {
    RigidTransform3d Tinc = (RigidTransform3d) myIncrementalTransform;
    RigidTransform3d T = (RigidTransform3d) myTransform;
    RotationMatrix3d R = new RotationMatrix3d();
    R.mulInverseLeft(myRot0, Rot);
    if (constrained) {
        AxisAngle axisAng = new AxisAngle();
        R.getAxisAngle(axisAng);
        double deg = Math.toDegrees(axisAng.angle);
        axisAng.angle = Math.toRadians(5 * Math.round(deg / 5));
        R.setAxisAngle(axisAng);
        myRotPnt.transform(R, myPnt0);
    }
    Tinc.R.mulInverseLeft(T.R, R);
    if (myConstrainer != null) {
        RotationMatrix3d Rnew = new RotationMatrix3d();
        Rnew.mul(myXDraggerToWorld.R, Tinc.R);
        RigidTransform3d Tnew = new RigidTransform3d(myXDraggerToWorld);
        if (myConstrainer.updateRotation(Rnew, myXDraggerToWorld)) {
            Tinc.R.mulInverseLeft(myXDraggerToWorld.R, Rnew);
            myXDraggerToWorld.R.set(Rnew);
            T.R.mul(Tinc.R);
            myRotPnt.transform(T.R, myPnt0);
        } else {
            Tinc.R.setIdentity();
        }
    } else {
        T.R.set(R);
        myXDraggerToWorld.R.mul(Tinc.R);
    }
}

Example 24 with AxisAngle

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

the class TextTest method build.

@Override
public void build(String[] args) throws IOException {
    super.build(args);
    final HashMap<String, Font> fontMap = new HashMap<>();
    for (Font font : GraphicsEnvironment.getLocalGraphicsEnvironment().getAllFonts()) {
        fontMap.put(font.getName(), font);
    }
    TextComponent3d text = new TextComponent3d("hello world");
    text.setFont(new Font(Font.SANS_SERIF, 0, 54));
    text.setText("Hello world! And goodnight to all.");
    text.setPosition(new Point3d(0.4f, 0.3f, 0.3f));
    text.setTextSize(0.25);
    text.setTextColor(Color.WHITE);
    text.setFollowEye(false);
    RenderProps.setFaceStyle(text, FaceStyle.FRONT_AND_BACK);
    addRenderable(text);
    text = new TextComponent3d("alphabet");
    text.setFont(new Font(Font.SANS_SERIF, 0, 54));
    text.setText("ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz");
    text.setPosition(new Point3d(-2f, 0f, 0f));
    text.setTextSize(0.25);
    text.setTextColor(Color.ORANGE);
    text.setFollowEye(false);
    RenderProps.setFaceStyle(text, FaceStyle.FRONT_AND_BACK);
    addRenderable(text);
    Font comic = fontMap.get("Comic Sans MS");
    if (comic == null) {
        comic = new Font(Font.MONOSPACED, Font.BOLD, 32);
    } else {
        comic = comic.deriveFont(Font.BOLD, 32);
    }
    text = new TextComponent3d("mikey");
    text.setFont(new Font(Font.SANS_SERIF, 0, 54));
    text.setText("Cowabunga");
    text.setPosition(new Point3d(0, 0.5f, 0f));
    text.setTextSize(0.25);
    text.setTextColor(Color.CYAN);
    text.setHorizontalAlignment(HorizontalAlignment.CENTRE);
    text.setFollowEye(false);
    RenderProps.setFaceStyle(text, FaceStyle.FRONT_AND_BACK);
    RenderProps.setShading(text, Shading.NONE);
    addRenderable(text);
    text = new TextComponent3d("flip");
    text.setFont(new Font(Font.SERIF, 0, 64));
    text.setText("Flip me!");
    text.setPosition(new Point3d(0, -0.44f, 0f));
    text.setOrientation(new AxisAngle(0, 1, 0, Math.toRadians(30)));
    text.setTextSize(0.3);
    text.setTextColor(Color.MAGENTA);
    text.setHorizontalAlignment(HorizontalAlignment.CENTRE);
    text.setFollowEye(false);
    RenderProps.setFaceStyle(text, FaceStyle.FRONT);
    addRenderable(text);
    text = new TextComponent3d("flip2");
    text.setFont(new Font(Font.SERIF, 0, 64));
    text.setText("Flip me!");
    text.setPosition(new Point3d(0, -0.44f, 0f));
    text.setOrientation(new AxisAngle(0, 1, 0, Math.toRadians(210)));
    text.setTextSize(0.3);
    text.setTextColor(Color.MAGENTA);
    text.setHorizontalAlignment(HorizontalAlignment.CENTRE);
    text.setFollowEye(false);
    RenderProps.setFaceStyle(text, FaceStyle.FRONT);
    addRenderable(text);
    PrintWriter writer = new PrintWriter(ArtisynthPath.getTempDir().getAbsolutePath() + "/texttest.txt");
    write(writer, new NumberFormat("%g"), null);
    writer.close();
    ArrayDeque<ScanToken> tokens = new ArrayDeque<>();
    ReaderTokenizer rtok = new ReaderTokenizer(new FileReader(ArtisynthPath.getTempDir().getAbsolutePath() + "/texttest.txt"));
    scan(rtok, tokens);
    rtok.close();
}

Example 25 with AxisAngle

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

the class RigidBodyCollision method build.

public void build(String[] args) throws IOException {
    try {
        mechmod = new MechModel();
        mechmod.setMaxStepSize(0.005);
        boxes = new ArrayList<RigidBody>();
        table = new RigidBody("table");
        table.setDynamic(false);
        table.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        AffineTransform3d trans = new AffineTransform3d();
        trans.setIdentity();
        trans.applyScaling(4, 2, 0.5);
        table.transformGeometry(trans);
        table.setPose(new RigidTransform3d(new Vector3d(0, 0, 0.8077474533228615), new AxisAngle()));
        table.setInertia(SpatialInertia.createBoxInertia(1, 1, 1, 1));
        mechmod.addRigidBody(table);
        boxes.add(table);
        if (wireFrame) {
            setWireFrame(table);
        }
        // middle box in pile
        box0 = new RigidBody("box0");
        box0.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.5, 0.5, 0.5);
        box0.transformGeometry(trans);
        box0.setInertia(SpatialInertia.createBoxInertia(4, 1, 1, 1));
        addBox(box0, Color.GREEN);
        if (wireFrame) {
            setWireFrame(box0);
        }
        // long thin box, bottom of pile
        box1 = new RigidBody("box1");
        box1.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.6, 0.1, 1.9);
        box1.transformGeometry(trans);
        box1.setInertia(SpatialInertia.createBoxInertia(1, 1, 0.1, 4));
        addBox(box1, Color.YELLOW);
        if (wireFrame) {
            setWireFrame(box1);
        }
        // left hand box falling on unsupported end of
        box2 = new RigidBody("box2");
        // box1
        box2.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.5, 0.5, 0.5);
        box2.transformGeometry(trans);
        box2.setInertia(SpatialInertia.createBoxInertia(20, 1, 1, 1));
        addBox(box2, Color.BLUE);
        if (wireFrame) {
            setWireFrame(box2);
        }
        // top box in pile
        box3 = new RigidBody("box3");
        box3.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.4, 0.4, 0.4);
        box3.transformGeometry(trans);
        box3.setInertia(SpatialInertia.createBoxInertia(0.5, 0.5, 0.5, 4));
        addBox(box3, Color.CYAN);
        // box3.getMesh().name = "box3";
        if (wireFrame) {
            setWireFrame(box3);
        }
        // solo box off to the right.
        box4 = new RigidBody("box4");
        box4.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.6, 0.6, 0.3);
        box4.transformGeometry(trans);
        box4.setInertia(SpatialInertia.createBoxInertia(0.5, 0.5, 0.5, 4));
        box4.setPose(new RigidTransform3d(new Vector3d(1, 0.0, 5), new AxisAngle(0, 0, 0, 0)));
        addBox(box4, Color.RED);
        // box4.getMesh().name = "box4";
        if (wireFrame) {
            setWireFrame(box4);
        }
        mechmod.setDefaultCollisionBehavior(true, 0.05);
        reset();
        addModel(mechmod);
        ControlPanel panel = new ControlPanel();
        panel.addWidget(mechmod, "integrator");
        panel.addWidget(mechmod, "maxStepSize");
        addControlPanel(panel);
        Main.getMain().arrangeControlPanels(this);
        CollisionManager cm = mechmod.getCollisionManager();
        RenderProps.setVisible(cm, true);
        RenderProps.setLineWidth(cm, 3);
        RenderProps.setLineColor(cm, Color.RED);
        cm.setDrawContactNormals(true);
        // addBreakPoint (0.74);
        for (int i = 1; i <= 10; i++) {
            addWayPoint(0.1 * i);
        }
    // setWaypointChecking (true);
    } catch (IOException e) {
        throw e;
    }
}