问题
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
来源：https://stackoverflow.com/questions/65115603/lost-in-3d-space-tilt-values-euler-from-rotation-matrix-javafx-affine-onl