lost in 3D space - tilt values (euler?) from rotation matrix (javafx affine) only works partially - javafx

Question

it is a while ago that I asked this question:
javafx - How to apply yaw, pitch and roll deltas (not euler) to a node in respect to the nodes rotation axes instead of the scene rotation axes?
Today I want to ask, how I can get the tilt (fore-back and sideways) relative to the body (not to the room) from the rotation matrix. To make the problem understandable, I took the final code from the fantastic answer of José Pereda and basicly added a method "getEulersFromRotationMatrix". This is working a bit, but at some point freaks out.
Attached find the whole working example. The problem becomes clear with the following click path:
// right after start
tilt fore
tilt left // all right
tilt right
tilt back // all right
// right after start
turn right
turn right
turn right
tilt fore
tilt back // all right
tilt left // bang, tilt values are completely off
While the buttons move the torso as expected, the tilt values (printed out under the buttons) behave wrong at some point.
import javafx.application.Application;
import javafx.application.Platform;
import javafx.event.ActionEvent;
import javafx.event.EventHandler;
import javafx.geometry.Point3D;
import javafx.scene.Group;
import javafx.scene.Parent;
import javafx.scene.PerspectiveCamera;
import javafx.scene.Scene;
import javafx.scene.SceneAntialiasing;
import javafx.scene.SubScene;
import javafx.scene.control.Button;
import javafx.scene.control.Label;
import javafx.scene.layout.HBox;
import javafx.scene.layout.StackPane;
import javafx.scene.layout.VBox;
import javafx.scene.paint.Color;
import javafx.scene.paint.PhongMaterial;
import javafx.scene.shape.Box;
import javafx.scene.transform.Affine;
import javafx.scene.transform.Rotate;
import javafx.stage.Stage;
public class PuppetTestApp extends Application {
private final int width = 800;
private final int height = 500;
private XGroup torsoGroup;
private final double torsoX = 50;
private final double torsoY = 80;
private Label output = new Label();
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("tilt fore"));
b.setOnAction(new TurnAction(torsoGroup.rx, 15) );
buttonBox.getChildren().add(b = new Button("tilt back"));
b.setOnAction(new TurnAction(torsoGroup.rx, -15) );
buttonBox.getChildren().add(b = new Button("tilt left"));
b.setOnAction(new TurnAction(torsoGroup.rz, 15) );
buttonBox.getChildren().add(b = new Button("tilt right"));
b.setOnAction(new TurnAction(torsoGroup.rz, -15) );
buttonBox.getChildren().add(b = new Button("turn left"));
b.setOnAction(new TurnAction(torsoGroup.ry, -28) ); // not 30 degree to avoid any gymbal lock problems
buttonBox.getChildren().add(b = new Button("turn right"));
b.setOnAction(new TurnAction(torsoGroup.ry, 28) ); // not 30 degree to avoid any gymbal lock problems
VBox vbox = new VBox();
vbox.getChildren().add(buttonBox);
vbox.getChildren().add(output);
return vbox;
}
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);
EulerValues euler= getEulersFromRotationMatrix(affine);
output.setText(String.format("tilt fore/back=%3.0f tilt sideways=%3.0f", euler.forward, euler.leftSide));
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();
}
/**
* Shall return the tilt values relative to the body (not relative to the room)
* (Maybe euler angles are not the right term here, but anyway)
*/
private EulerValues getEulersFromRotationMatrix(Affine rot) {
double eulerX; // turn left/right
double eulerY; // tilt fore/back
double eulerZ; // tilt sideways
double r11 = rot.getMxx();
double r12 = rot.getMxy();
double r13 = rot.getMxz();
double r21 = rot.getMyx();
double r31 = rot.getMzx();
double r32 = rot.getMzy();
double r33 = rot.getMzz();
// used instructions from https://www.gregslabaugh.net/publications/euler.pdf
if (r31 != 1.0 && r31 != -1.0) {
eulerX = -Math.asin(r31); // already tried with the 2nd solution as well
double cosX = Math.cos(eulerX);
eulerY = Math.atan2(r32/cosX, r33/cosX);
eulerZ = Math.atan2(r21/cosX, r11/cosX);
}
else {
eulerZ = 0;
if (r31 == -1) {
eulerX = Math.PI / 2;
eulerY = Math.atan2(r12, r13);
}
else {
eulerX = -Math.PI / 2;
eulerY = Math.atan2(-r12, -r13);
}
}
return new EulerValues(
eulerY / Math.PI * 180.0,
eulerZ / Math.PI * 180.0,
-eulerX / Math.PI * 180.0);
}
public class EulerValues {
public double leftTurn;
public double forward;
public double leftSide;
public EulerValues(double forward, double leftSide, double leftTurn) {
this.forward = forward;
this.leftSide = leftSide;
this.leftTurn = leftTurn;
}
}
public static void main(String[] args) {
launch(args);
}
}
PS: This may look like I have close to no progress, but this is only because I try to reduce the question to the possible minimum. If you want to see how this stuff is embedded in my main project, you can watch this little video I just uploaded (but does not add anything to the question): https://www.youtube.com/watch?v=R3t8BIHeo7k

Answer 1

I think I got it by myself now: What I computed was the "default" euler angles, sometimes refered to as z x' z'', where the 1st and 3th rotation is around the same axis. But what I am looking for are the angles that can be applied to the z, y' and x'' achses (in that order) to reach the position presented by the rotation matrix. (and then ignore the z rotation).
Or even better compute the z y' x'' eulers and the z x' y'' eulers and
only use the x' and y' values.
Added:
No, that was wrong. I indeed calculated the Tait-Bryan x y z rotations. So this was not the solution.
Ok, new explanation:
The rotation axes wthat I calculate are room relative rotations (not object relative rotations), and the 2nd rotation is at the vertical axe (which I am not interested in). But because it is "in the middle", it can cancel out the 1st and 3th rotation, and this is what happens.
So the solution should be the change the rotation order, that comes out of my matrix-to-euler algorithm. But how to do this?
I just exchanged all "y" and "z":
r11 = rot.getMxx();
r12 = rot.getMxz();
r13 = rot.getMxy();
r21 = rot.getMzx();
r31 = rot.getMyx();
r32 = rot.getMyz();
r33 = rot.getMyy();
and now it really does what I want. :)