javafx - How to apply yaw, pitch and roll deltas (

问题:

Please bear with my long question, I am trying to make it as clear as possible. (As found in another question.)

In the example below, all rotate buttons are a test replacement for the gyro values coming in from a gyroscope sensor. The sensor is fixed to the real world torso, so the buttons are meant to represent rotation deltas to be applied to the virtual torso in respect to the torsos coordinate system, not the scene coordinate system.

All buttons work fine by themselfs if starting from a "zero" rotation. But when I press 3 times yaw, and then roll, then I see that the roll rotation works on the scene axes. But I would like to apply it to the current torso rotation instead.

I have already tried several suggestions to related problems from here, but did not come to a solution.

A side note: I am not sure if the terms yaw, pitch and roll are usually bound to euler angles, so I want to underline that to my understanding the values from the gyro sensor are not euler angles, as they represent rotation deltas relative to the current torso rotation, and not accumulated angles "absolute" to the torso starting point. So if I have used these terms inappropriately please try to understand what I have meant anyway.

(Background info: I have a robot project roboshock.de with a gyroscope sensor connected to the robots torso, and I want to visualize the rotation of the robot on the screen. The rotate buttons in the below example are only a test replacement for the gyro values coming in from the sensor.)

Any help is much appreciated.

import javafx.application.Application;
import javafx.event.ActionEvent;
import javafx.event.EventHandler;
import javafx.scene.Group;
import javafx.scene.Parent;
import javafx.scene.Scene;
import javafx.scene.control.Button;
import javafx.scene.layout.HBox;
import javafx.scene.layout.StackPane;
import javafx.scene.paint.Color;
import javafx.scene.paint.PhongMaterial;
import javafx.scene.shape.Box;
import javafx.scene.transform.Rotate;
import javafx.stage.Stage;

public class PuppetTestApp extends Application {

    int width = 800;
    int height = 500;
    XGroup torsoGroup;
    double torsoX = 50;
    double torsoY = 80;

    public Parent createRobot() {
        Box torso = new Box(torsoX, torsoY, 20);
        torso.setMaterial(new PhongMaterial(Color.RED));
        Box head = new Box(20, 20, 20);
        head.setMaterial(new PhongMaterial(Color.YELLOW.darker()));
        head.setTranslateY(-torsoY / 2 -10);

        torsoGroup = new XGroup();
        torsoGroup.getChildren().addAll(torso, head);
        return torsoGroup;
    }

    public Parent createUI() {
        HBox buttonBox = new HBox();

        Button b;
        buttonBox.getChildren().add(b = new Button("Exit"));
        b.setOnAction( (ActionEvent arg0) -> { System.exit(0); } );

        buttonBox.getChildren().add(b = new Button("pitch up"));
        b.setOnAction(new TurnAction(torsoGroup.rx, -15) );

        buttonBox.getChildren().add(b = new Button("pitch down"));
        b.setOnAction(new TurnAction(torsoGroup.rx, 15) );

        buttonBox.getChildren().add(b = new Button("Yaw left"));
        b.setOnAction(new TurnAction(torsoGroup.ry, -15) );

        buttonBox.getChildren().add(b = new Button("Yaw right"));
        b.setOnAction(new TurnAction(torsoGroup.ry, 15) );

        buttonBox.getChildren().add(b = new Button("Roll right"));
        b.setOnAction(new TurnAction(torsoGroup.rz, -15) );

        buttonBox.getChildren().add(b = new Button("Roll left"));
        b.setOnAction(new TurnAction(torsoGroup.rz, 15) );

        return buttonBox;
    }

    class TurnAction implements EventHandler<ActionEvent> {
        final Rotate rotate;
        double deltaAngle;

        public TurnAction(Rotate rotate, double targetAngle) {
            this.rotate = rotate;
            this.deltaAngle = targetAngle;
        }

        @Override
        public void handle(ActionEvent arg0) {
            addRotate(torsoGroup, rotate, deltaAngle);
        } 
    }

    private void addRotate(XGroup node, Rotate rotate, double angle) {

        // HERE I DO SOMETHING WRONG

        // not working 1:
        //Transform newRotate = new Rotate(angle, rotate.getAxis());
        //node.getTransforms().add(newRotate);

        // not working 2:
        double x = rotate.getAngle();
        rotate.setAngle(x + angle);
    }

    public class XGroup extends Group {

        public Rotate rx = new Rotate(0, Rotate.X_AXIS);
        public Rotate ry = new Rotate(0, Rotate.Y_AXIS);
        public Rotate rz = new Rotate(0, Rotate.Z_AXIS);

        public XGroup() { 
            super(); 
            getTransforms().addAll(rz, ry, rx); 
        }

        public void setRotate(double x, double y, double z) {
            rx.setAngle(x);
            ry.setAngle(y);
            rz.setAngle(z);
        }

        public void setRotateX(double x) { rx.setAngle(x); }
        public void setRotateY(double y) { ry.setAngle(y); }
        public void setRotateZ(double z) { rz.setAngle(z); }
    }

    @Override 
    public void start(Stage stage) throws Exception {
        Parent robot = createRobot();
        Parent ui = createUI();
        StackPane combined = new StackPane();
        combined.getChildren().addAll(ui, robot);
        combined.setStyle("-fx-background-color: linear-gradient(to bottom, cornsilk, midnightblue);");

        Scene scene = new Scene(combined, width, height);
        stage.setScene(scene);
        stage.show();
     }

    public static void main(String[] args) {
        launch(args);
    }
}

回答1:

For starters, there are a few things that you should consider for a JavaFX 3D application:

• depth buffer, and antialiasing
• subScene
• camera

and you don't have any of those.

You need depth buffer enabled, as you can see that the small yellow box seems to be on top of the big box (the light yellow face shouldn't be visible at all):

According to JavaDoc for Scene:

A scene containing 3D shapes or 2D shapes with 3D transforms may use depth buffer support for proper depth sorted rendering

Change:

Scene scene = new Scene(combined, width, height);

to:

Scene scene = new Scene(combined, width, height, true, SceneAntialiasing.BALANCED);

Once you do it, you'll realize that when the box goes to z > 0 is no longer visible, and the buttons on top are not clickable anymore.

It is not good idea to mix 2D and 3D in the same scene. For that you need a SubScene, where you can lay out your 3D content, and leave the 2D in the Scene itself. Also, you can move the depth buffer and antialiasing options to the subScene:

Parent robot = createRobot();
// add subScene
SubScene subScene = new SubScene(robot, width, height, true, SceneAntialiasing.BALANCED);
Parent ui = createUI();
StackPane combined = new StackPane();
combined.getChildren().addAll(ui, subScene);
Scene scene = new Scene(combined, width, height);

Now the problem is the layout, the boxes will show up in the top left corner, not in the center.

You can add a translation to your group:

public XGroup() { 
    super(); 
    getTransforms().addAll(new Translate(width/2, height/2, 0), rz, ry, rx); 
}    

Now you can see the correct depth sorted rendering, and the ui buttons are accessible.

Another option is adding a camera. You can remove the translate transform, and you can also "zoom" to see your boxes bigger:

Parent robot = createRobot();
SubScene subScene = new SubScene(robot, width, height, true, SceneAntialiasing.BALANCED);
PerspectiveCamera camera = new PerspectiveCamera(true);
camera.setNearClip(0.01);
camera.setFarClip(100000);
camera.setTranslateZ(-400);
subScene.setCamera(camera);

Rotations

Now, in terms of rotations, if you want to apply a given rotation over the current state of the boxes, and not related to the "scene" axes (I take you mean the three orthogonal non rotated axis), you have to take into account the previous state before applying a new rotation.

In this blog post about the Rubik's cube, each face (composed of 9 small "cubies") can be rotated again and again and the affected cubies carry on a number of previous rotations. The project can be found here.

In this case, using transforms and Affine prepend was the key to have always updated local orthogonal axes on the 3D body.

I'd suggest this:

private void addRotate(XGroup node, Rotate rotate, double angle) {
    Transform newRotate = new Rotate(angle, rotate.getAxis());
    Affine affine = node.getTransforms().isEmpty() ? new Affine() : new Affine(node.getTransforms().get(0));
    affine.prepend(newRotate);
    node.getTransforms().setAll(affine);
}

Nonetheless, your new rotations are all defined over the scene orthogonal axes.

If you want your local axes instead, you can get them from the affine matrix. If you print the affine at any time, you can get x', y', z' axes from the columns 1, 2 and 3:

Affine [
     0.70710678, 0.50000000,  0.50000000, 0.0
     0.00000000, 0.70710678, -0.70710678, 0.0
    -0.70710678, 0.50000000,  0.50000000, 0.0]

I.e, the x' axis (blue) is {0.7071, 0.0, -0.7071}.

So finally you can define the rotations over the local axis as:

private void addRotate(XGroup node, Rotate rotate, double angle) {

    Affine affine = node.getTransforms().isEmpty() ? new Affine() : new Affine(node.getTransforms().get(0));
    double A11 = affine.getMxx(), A12 = affine.getMxy(), A13 = affine.getMxz(); 
    double A21 = affine.getMyx(), A22 = affine.getMyy(), A23 = affine.getMyz(); 
    double A31 = affine.getMzx(), A32 = affine.getMzy(), A33 = affine.getMzz(); 

    // rotations over local axis  
    Rotate newRotateX = new Rotate(angle, new Point3D(A11, A21, A31));
    Rotate newRotateY = new Rotate(angle, new Point3D(A12, A22, A32));
    Rotate newRotateZ = new Rotate(angle, new Point3D(A13, A23, A33));

    // apply rotation
    affine.prepend(rotate.getAxis() == Rotate.X_AXIS ? newRotateX : 
            rotate.getAxis() == Rotate.Y_AXIS ? newRotateY : newRotateZ);
    node.getTransforms().setAll(affine);
}

I believe this will give you what you were looking for.

This is the whole modified code:

private final int width = 800;
private final int height = 500;
private XGroup torsoGroup;
private final double torsoX = 50;
private final double torsoY = 80;

public Parent createRobot() {
    Box torso = new Box(torsoX, torsoY, 20);
    torso.setMaterial(new PhongMaterial(Color.RED));
    Box head = new Box(20, 20, 20);
    head.setMaterial(new PhongMaterial(Color.YELLOW.darker()));
    head.setTranslateY(-torsoY / 2 -10);

    Box x = new Box(200, 2, 2);
    x.setMaterial(new PhongMaterial(Color.BLUE));
    Box y = new Box(2, 200, 2);
    y.setMaterial(new PhongMaterial(Color.BLUEVIOLET));
    Box z = new Box(2, 2, 200);
    z.setMaterial(new PhongMaterial(Color.BURLYWOOD));

    torsoGroup = new XGroup();
    torsoGroup.getChildren().addAll(torso, head, x, y, z);
    return torsoGroup;
}

public Parent createUI() {
    HBox buttonBox = new HBox();

    Button b;
    buttonBox.getChildren().add(b = new Button("Exit"));
    b.setOnAction( (ActionEvent arg0) -> { Platform.exit(); } );

    buttonBox.getChildren().add(b = new Button("pitch up"));
    b.setOnAction(new TurnAction(torsoGroup.rx, -15) );

    buttonBox.getChildren().add(b = new Button("pitch down"));
    b.setOnAction(new TurnAction(torsoGroup.rx, 15) );

    buttonBox.getChildren().add(b = new Button("Yaw left"));
    b.setOnAction(new TurnAction(torsoGroup.ry, -15) );

    buttonBox.getChildren().add(b = new Button("Yaw right"));
    b.setOnAction(new TurnAction(torsoGroup.ry, 15) );

    buttonBox.getChildren().add(b = new Button("Roll right"));
    b.setOnAction(new TurnAction(torsoGroup.rz, -15) );

    buttonBox.getChildren().add(b = new Button("Roll left"));
    b.setOnAction(new TurnAction(torsoGroup.rz, 15) );

    return buttonBox;
}

class TurnAction implements EventHandler<ActionEvent> {
    final Rotate rotate;
    double deltaAngle;

    public TurnAction(Rotate rotate, double targetAngle) {
        this.rotate = rotate;
        this.deltaAngle = targetAngle;
    }

    @Override
    public void handle(ActionEvent arg0) {
        addRotate(torsoGroup, rotate, deltaAngle);
    } 
}

private void addRotate(XGroup node, Rotate rotate, double angle) {
    Affine affine = node.getTransforms().isEmpty() ? new Affine() : new Affine(node.getTransforms().get(0));
    double A11 = affine.getMxx(), A12 = affine.getMxy(), A13 = affine.getMxz(); 
    double A21 = affine.getMyx(), A22 = affine.getMyy(), A23 = affine.getMyz(); 
    double A31 = affine.getMzx(), A32 = affine.getMzy(), A33 = affine.getMzz(); 

    Rotate newRotateX = new Rotate(angle, new Point3D(A11, A21, A31));
    Rotate newRotateY = new Rotate(angle, new Point3D(A12, A22, A32));
    Rotate newRotateZ = new Rotate(angle, new Point3D(A13, A23, A33));

    affine.prepend(rotate.getAxis() == Rotate.X_AXIS ? newRotateX : 
            rotate.getAxis() == Rotate.Y_AXIS ? newRotateY : newRotateZ);

    node.getTransforms().setAll(affine);
}

public class XGroup extends Group {
    public Rotate rx = new Rotate(0, Rotate.X_AXIS);
    public Rotate ry = new Rotate(0, Rotate.Y_AXIS);
    public Rotate rz = new Rotate(0, Rotate.Z_AXIS);
}

@Override 
public void start(Stage stage) throws Exception {
    Parent robot = createRobot();
    SubScene subScene = new SubScene(robot, width, height, true, SceneAntialiasing.BALANCED);
    PerspectiveCamera camera = new PerspectiveCamera(true);
    camera.setNearClip(0.01);
    camera.setFarClip(100000);
    camera.setTranslateZ(-400);
    subScene.setCamera(camera);

    Parent ui = createUI();
    StackPane combined = new StackPane(ui, subScene);
    combined.setStyle("-fx-background-color: linear-gradient(to bottom, cornsilk, midnightblue);");

    Scene scene = new Scene(combined, width, height);
    stage.setScene(scene);
    stage.show();
}

回答2:

For anybody who is interested: I have put all the final code (including the arduino sketch) on github. There is also a youtube showning the fast and precise response:

https://github.com/tschuett-munich/gyro-to-javafx