javafx.scene.transform.Affine#setMyz ( )源码实例Demo

/**
 * Assigns the transformation values of the <i>src</i> {@link Affine} to the
 * <i>dst</i> {@link Affine}.
 *
 * @param dst
 *            The destination {@link Affine}.
 * @param src
 *            The source {@link Affine}.
 * @return The destination {@link Affine} for convenience.
 */
public static Affine setAffine(Affine dst, Affine src) {
	dst.setMxx(src.getMxx());
	dst.setMxy(src.getMxy());
	dst.setMxz(src.getMxz());
	dst.setMyx(src.getMyx());
	dst.setMyy(src.getMyy());
	dst.setMyz(src.getMyz());
	dst.setMzx(src.getMzx());
	dst.setMzy(src.getMzy());
	dst.setMzz(src.getMzz());
	dst.setTx(src.getTx());
	dst.setTy(src.getTy());
	dst.setTz(src.getTz());
	return dst;
}
 

@SuppressWarnings("boxing")
@Override
public void messageArrived(String topic, MqttMessage message) {
	System.out.println("messageArrived(" + topic + ")");
	ByteBuffer buffer = ByteBuffer.wrap(message.getPayload());
	double[] compass = new double[] { buffer.getDouble(), buffer.getDouble(), buffer.getDouble() };
	double[] accel = new double[] { buffer.getDouble(), buffer.getDouble(), buffer.getDouble() };
	double[] gyro = new double[] { buffer.getDouble(), buffer.getDouble(), buffer.getDouble() };
	double[] quat = new double[] { buffer.getDouble(), buffer.getDouble(), buffer.getDouble(), buffer.getDouble() };
	double[] ypr = new double[] { buffer.getDouble(), buffer.getDouble(), buffer.getDouble() };
	double w = quat[QUAT_SCALER];
	double x = quat[QUAT_X];
	double y = quat[QUAT_Y];
	double z = quat[QUAT_Z];
	
	System.out.format("Got IMU data: compass=[%f, %f, %f], accel=[%f, %f, %f], "
			+ "gyro=[%f, %f, %f], quat=[%f, %f, %f, %f], ypr=[%f, %f, %f]%n",
			compass[0], compass[1], compass[2], accel[0], accel[1], accel[2],
			gyro[0], gyro[1], gyro[2], quat[0], quat[1], quat[2], quat[3], ypr[0], ypr[1], ypr[2]);
	
	//Rotate rx = new Rotate(Math.toDegrees(ypr[0]), Rotate.X_AXIS);
	//Rotate ry = new Rotate(Math.toDegrees(ypr[1]), Rotate.Y_AXIS);
	//Rotate rz = new Rotate(Math.toDegrees(ypr[2]), Rotate.Z_AXIS);
	
	double[] idt = { 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1 };
	Affine matrix = new Affine(idt, MatrixType.MT_3D_3x4, 0);
	// http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54
	matrix.setMxx(1 - (2*y*y + 2*z*z));	matrix.setMxy(2*x*y + 2*z*w);		matrix.setMxz(2*x*z - 2*y*w);
	matrix.setMyx(2*x*y - 2*z*w);		matrix.setMyy(1 - (2*x*x + 2*z*z));	matrix.setMyz(2*y*z + 2*x*w);
	matrix.setMzx(2*x*z + 2*y*w);		matrix.setMzy(2*y*z - 2*x*w);		matrix.setMzz(1 - (2*x*x + 2*y*y));
	
	if (! Platform.isFxApplicationThread()) {
		Platform.runLater(() -> {
				testObject.getTransforms().setAll(matrix);
				//testObject.getTransforms().clear();
				//testObject.getTransforms().addAll(rx, ry, rz);
			}
		);
	}
}