Java 类javafx.scene.transform.MatrixType 实例源码

/**
 * The same as {link Affine#append(double[] matrix, MatrixType type, int offset)}
 */
@Override
public void append(double[] matrix, MatrixType type, int offset)
{
    switch(type)
    {
        case MT_2D_2x3:
        case MT_2D_3x3:
            append(matrix[offset], matrix[offset + 1], matrix[offset + 2],
                    matrix[offset + 3], matrix[offset + 4], matrix[offset + 5]);
            break;
        case MT_3D_3x4:
        case MT_3D_4x4:
            append(matrix[offset], matrix[offset + 1], matrix[offset + 2], matrix[offset + 3],
                    matrix[offset + 4], matrix[offset + 5], matrix[offset + 6], matrix[offset + 7],
                    matrix[offset + 8], matrix[offset + 9], matrix[offset + 10], matrix[offset + 11]);
            break;
    }
}

@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);
            }
        );
    }
}