bundles/core/org.eclipse.apogy.core.ui/src/org/eclipse/apogy/core/ui/composites/ApogySystemPoseComposite.java - gerrit/apogy/apogy - Git at Google

 /*******************************************************************************
  * Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency
  * All rights reserved. This program and the accompanying materials
  * are made available under the terms of the Eclipse Public License v1.0
  * which accompanies this distribution, and is available at
  * http://www.eclipse.org/legal/epl-v10.html
  *
  * Contributors:
  *     Pierre Allard,
  *     Regent L'Archeveque - initial API and implementation
  *
  * SPDX-License-Identifier: EPL-1.0
  *******************************************************************************/
 package org.eclipse.apogy.core.ui.composites;

 import java.text.DecimalFormat;

 import javax.vecmath.Point3d;

 import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
 import org.eclipse.apogy.common.math.ApogyCommonMathFactory;
 import org.eclipse.apogy.common.math.Matrix4x4;
 import org.eclipse.apogy.common.math.Tuple3d;
 import org.eclipse.apogy.common.math.ui.composites.Tuple3dComposite;
 import org.eclipse.apogy.core.ApogyCorePackage;
 import org.eclipse.apogy.core.ApogySystemApiAdapter;
 import org.eclipse.apogy.core.PoseProvider;
 import org.eclipse.apogy.core.invocator.ApogyCoreInvocatorFacade;
 import org.eclipse.apogy.core.invocator.VariableFeatureReference;
 import org.eclipse.apogy.core.invocator.VariablesList;
 import org.eclipse.apogy.core.invocator.ui.composites.VariableFeatureReferenceComposite;
 import org.eclipse.emf.common.notify.Adapter;
 import org.eclipse.emf.common.notify.Notification;
 import org.eclipse.emf.common.notify.impl.AdapterImpl;
 import org.eclipse.emf.ecore.util.EcoreUtil;
 import org.eclipse.jface.dialogs.Dialog;
 import org.eclipse.jface.window.Window;
 import org.eclipse.swt.SWT;
 import org.eclipse.swt.events.DisposeEvent;
 import org.eclipse.swt.events.DisposeListener;
 import org.eclipse.swt.events.KeyEvent;
 import org.eclipse.swt.events.KeyListener;
 import org.eclipse.swt.events.SelectionEvent;
 import org.eclipse.swt.events.SelectionListener;
 import org.eclipse.swt.graphics.Color;
 import org.eclipse.swt.graphics.Point;
 import org.eclipse.swt.layout.GridData;
 import org.eclipse.swt.layout.GridLayout;
 import org.eclipse.swt.widgets.Button;
 import org.eclipse.swt.widgets.Composite;
 import org.eclipse.swt.widgets.Control;
 import org.eclipse.swt.widgets.Display;
 import org.eclipse.swt.widgets.Label;
 import org.eclipse.swt.widgets.Shell;
 import org.eclipse.swt.widgets.Text;

 public class ApogySystemPoseComposite extends ApogySystemApiAdapterBasedComposite {
 	public static final String DEGREE_STRING = "\u00b0";
 	public static final int ATTITUDE_INDICATOR_WIDTH = 200;
 	public static final int ATTITUDE_INDICATOR_HEIGHT = 200;

 	private Adapter poseAdapter;

 	private VariablesList variables;
 	// private VariableFeatureReference variableFeatureReference;

 	private Tuple3d previousPosition = null;
 	private final Tuple3d position = ApogyCommonMathFactory.eINSTANCE.createTuple3d();
 	private final Tuple3d orientation = ApogyCommonMathFactory.eINSTANCE.createTuple3d();

 	private double minimumDeltaPosition = 0.05;
 	private double tripDistance = 0.0;

 	private final DecimalFormat distanceFormat = new DecimalFormat("0.00");

 	private Text txtVariableFeatureReference;
 	private Text txtTripDistance;
 	private Button btnTripReset;
 	private Button btnTripSet;

 	private Tuple3dComposite translationComposite;
 	private Tuple3dComposite rotationComposite;

 	public ApogySystemPoseComposite(Composite parent, int style) {
 		this(parent, style, null, null, 0.0);
 	}

 	public ApogySystemPoseComposite(Composite parent, int style, VariablesList variables,
 			VariableFeatureReference variableFeatureReference, double tripDistance) {
 		super(parent, style);

 		this.tripDistance = tripDistance;
 		this.variables = variables;

 		GridLayout layout = new GridLayout(4, false);
 		setLayout(layout);

 		Label lblVariableFeatureReference = new Label(this, SWT.NONE);
 		lblVariableFeatureReference.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
 		lblVariableFeatureReference.setText("Variable Feature : ");

 		this.txtVariableFeatureReference = new Text(this, SWT.BORDER);
 		GridData gd_txtVariableFeatureReference = new GridData(SWT.FILL, SWT.CENTER, false, false, 2, 1);
 		gd_txtVariableFeatureReference.widthHint = 300;
 		this.txtVariableFeatureReference.setLayoutData(gd_txtVariableFeatureReference);

 		Button btnSelectVariableFeatureReference = new Button(this, SWT.PUSH);
 		btnSelectVariableFeatureReference.setText("Select...");
 		btnSelectVariableFeatureReference.addSelectionListener(new SelectionListener() {

 			@Override
 			public void widgetSelected(SelectionEvent e) {
 				final Shell shell = Display.getCurrent().getActiveShell();
 				Dialog dialog = new Dialog(shell) {
 					VariableFeatureReferenceComposite vfrComposite = null;

 					@Override
 					protected Control createDialogArea(Composite parent) {
 						Composite area = (Composite) super.createDialogArea(parent);

 						this.vfrComposite = new VariableFeatureReferenceComposite(area, SWT.NONE);
 						this.vfrComposite.setLayoutData(new GridData(SWT.FILL, SWT.CENTER, false, false, 3, 1));
 						this.vfrComposite.set(ApogySystemPoseComposite.this.variables,
 								ApogySystemPoseComposite.this.getVariableFeatureReference());

 						return area;
 					}

 					@Override
 					public boolean close() {
 						if (this.vfrComposite != null && !this.vfrComposite.isDisposed()) {
 							this.vfrComposite.set(null, null);
 							this.vfrComposite.dispose();
 						}

 						return super.close();
 					}
 				};

 				int result = dialog.open();
 				if (result == Window.OK) {
 					// Update displayed VFR.
 					updateDisplayedVariableFeatureReference();
 				}
 			}

 			@Override
 			public void widgetDefaultSelected(SelectionEvent e) {
 			}
 		});

 		// Trip Distance.
 		Label lblPOdometer = new Label(this, SWT.NONE);
 		lblPOdometer.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
 		lblPOdometer.setText("Trip Odometer (m):");

 		this.txtTripDistance = new Text(this, SWT.BORDER | SWT.SINGLE);
 		this.txtTripDistance.setText(this.distanceFormat.format(this.tripDistance));
 		this.txtTripDistance.setEnabled(false);
 		GridData gd_lblTripdistancelabel = new GridData(SWT.LEFT, SWT.CENTER, false, false, 1, 1);
 		gd_lblTripdistancelabel.widthHint = 150;
 		gd_lblTripdistancelabel.minimumWidth = 150;
 		this.txtTripDistance.setLayoutData(gd_lblTripdistancelabel);

 		this.btnTripReset = new Button(this, SWT.NONE);
 		this.btnTripReset.setLayoutData(new GridData(SWT.CENTER, SWT.CENTER, false, false, 1, 1));
 		this.btnTripReset.setText("Reset");
 		this.btnTripReset.setEnabled(false);
 		this.btnTripReset.addSelectionListener(new SelectionListener() {
 			@Override
 			public void widgetSelected(SelectionEvent e) {
 				setTripDistance(0.0);
 				// txtTripDistance.setText(distanceFormat.format(0.0));
 			}

 			@Override
 			public void widgetDefaultSelected(SelectionEvent e) {
 			}
 		});

 		this.btnTripSet = new Button(this, SWT.NONE);
 		this.btnTripSet.setLayoutData(new GridData(SWT.CENTER, SWT.CENTER, false, false, 1, 1));
 		this.btnTripSet.setText("Set To Value");
 		this.btnTripSet.setEnabled(false);
 		this.btnTripSet.addSelectionListener(new SelectionListener() {
 			@Override
 			public void widgetSelected(SelectionEvent e) {
 				OdometrySetValueDialog dialog = new OdometrySetValueDialog(getShell());
 				if (dialog.open() == Window.OK) {
 					setTripDistance(dialog.tripDistanceValue);
 				}
 			}

 			@Override
 			public void widgetDefaultSelected(SelectionEvent e) {
 			}
 		});

 		// Translation
 		Label lblPosition = new Label(this, SWT.NONE);
 		lblPosition.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
 		lblPosition.setAlignment(SWT.RIGHT);
 		lblPosition.setText("Position (m):");

 		this.translationComposite = new Tuple3dComposite(this, SWT.NONE, "0.00");
 		this.translationComposite.setEnabled(false);
 		GridData gd_translationComposite = new GridData(SWT.LEFT, SWT.CENTER, false, false, 3, 1);
 		gd_translationComposite.minimumWidth = 300;
 		gd_translationComposite.widthHint = 300;
 		this.translationComposite.setLayoutData(gd_translationComposite);
 		this.translationComposite.setTuple3d(this.position);
 		this.translationComposite.setEnableEditing(false);

 		// Rotation
 		Label lblOrientation = new Label(this, SWT.NONE);
 		lblOrientation.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
 		lblOrientation.setAlignment(SWT.RIGHT);
 		lblOrientation.setText("Orientation (" + DEGREE_STRING + "):");

 		this.rotationComposite = new Tuple3dComposite(this, SWT.NONE, "0.0");
 		this.rotationComposite.setEnabled(false);
 		GridData gd_rotationComposite = new GridData(SWT.LEFT, SWT.CENTER, false, false, 3, 1);
 		gd_rotationComposite.minimumWidth = 300;
 		gd_rotationComposite.widthHint = 300;
 		this.rotationComposite.setLayoutData(gd_rotationComposite);
 		this.rotationComposite.setTuple3d(this.orientation);
 		this.rotationComposite.setEnableEditing(false);
 		setVariableFeatureReference(variableFeatureReference);

 		updateDisplayedVariableFeatureReference();

 		addDisposeListener(new DisposeListener() {

 			@Override
 			public void widgetDisposed(DisposeEvent e) {
 				// Unregister adapter from apogySystemApiAdapter if required.
 				if (getApogySystemApiAdapter() != null) {
 					getApogySystemApiAdapter().eAdapters().remove(getPoseAdapter());
 				}

 				// Clear the Tuple3D composites.
 				if (!ApogySystemPoseComposite.this.translationComposite.isDisposed())
 					ApogySystemPoseComposite.this.translationComposite.setTuple3d(null);
 				if (!ApogySystemPoseComposite.this.rotationComposite.isDisposed())
 					ApogySystemPoseComposite.this.rotationComposite.setTuple3d(null);
 			}
 		});
 	}

 	public double getTripDistance() {
 		return this.tripDistance;
 	}

 	public void setTripDistance(double tripDistance) {
 		this.tripDistance = tripDistance;

 		// Do this on UI Thread.
 		getDisplay().asyncExec(new Runnable() {
 			@Override
 			public void run() {
 				// Update distance displays
 				if (ApogySystemPoseComposite.this.txtTripDistance != null
 						&& !ApogySystemPoseComposite.this.txtTripDistance.isDisposed()) {
 					ApogySystemPoseComposite.this.txtTripDistance
 							.setText(ApogySystemPoseComposite.this.distanceFormat.format(tripDistance));
 				}
 			}
 		});
 	}

 	@Override
 	protected void apogySystemApiAdapterChanged(final ApogySystemApiAdapter oldApogySystemApiAdapter,
 			final ApogySystemApiAdapter newApogySystemApiAdapter) {
 		// Do this on UI Thread.
 		getDisplay().asyncExec(new Runnable() {
 			@Override
 			public void run() {
 				updateApogySystemApiAdapter(oldApogySystemApiAdapter, newApogySystemApiAdapter);
 			}
 		});
 	}

 	public void setVariableList(final VariablesList variables) {
 		this.variables = variables;
 	}

 	@Override
 	public void setVariableFeatureReference(VariableFeatureReference newVariableFeatureReference) {
 		super.setVariableFeatureReference(newVariableFeatureReference);

 		getDisplay().asyncExec(new Runnable() {
 			@Override
 			public void run() {
 				updateDisplayedVariableFeatureReference();
 			}
 		});
 	}

 	/**
 	 * Returns the minimum distance between two position update that should be
 	 * considered significant enough to update the trip odometer distance.
 	 *
 	 * @return minimum distance, in meters.
 	 */
 	public double getMinimumDeltaPosition() {
 		return this.minimumDeltaPosition;
 	}

 	/**
 	 * Sets the minimum distance between two position update that should be
 	 * considered significant enough to update the trip odometer distance.
 	 *
 	 * @param minimumDeltaPosition The minimum distance, in meters.
 	 */
 	public void setMinimumDeltaPosition(double minimumDeltaPosition) {
 		this.minimumDeltaPosition = minimumDeltaPosition;
 	}

 	protected void updateApogySystemApiAdapter(ApogySystemApiAdapter oldApogySystemApiAdapter,
 			ApogySystemApiAdapter newApogySystemApiAdapter) {
 		// Unregister adapter from previous apogySystemApiAdapter is required.
 		if (oldApogySystemApiAdapter != null) {
 			oldApogySystemApiAdapter.eAdapters().remove(getPoseAdapter());
 		}

 		if (newApogySystemApiAdapter != null) {
 			// Updates pose.
 			updatePose(newApogySystemApiAdapter.getPoseTransform());

 			// Register adapter to new apogySystemApiAdapter.
 			newApogySystemApiAdapter.eAdapters().add(getPoseAdapter());
 		}

 		// Enables / Disable displays.
 		setDisplaysEnabled(newApogySystemApiAdapter != null);
 	}

 	protected void setDisplaysEnabled(boolean enabled) {
 		if (this.translationComposite != null && !this.translationComposite.isDisposed()) {
 			this.translationComposite.setEnabled(enabled);
 		}

 		if (this.rotationComposite != null && !this.rotationComposite.isDisposed()) {
 			this.rotationComposite.setEnabled(enabled);
 		}

 		if (this.txtTripDistance != null && !this.txtTripDistance.isDisposed()) {
 			this.txtTripDistance.setEnabled(enabled);
 		}

 		if (this.btnTripReset != null && !this.btnTripReset.isDisposed()) {
 			this.btnTripReset.setEnabled(enabled);
 		}

 		if (this.btnTripSet != null && !this.btnTripSet.isDisposed()) {
 			this.btnTripSet.setEnabled(enabled);
 		}
 	}

 	protected void updateDisplayedVariableFeatureReference() {
 		if (this.txtVariableFeatureReference != null && !this.txtVariableFeatureReference.isDisposed()) {
 			String text = ApogyCoreInvocatorFacade.INSTANCE
 					.getVariableFeatureReferenceString(this.getVariableFeatureReference());
 			this.txtVariableFeatureReference.setText(text);
 		}
 	}

 	protected void updatePose(Matrix4x4 newPose) {
 		if (newPose != null) {
 			// Updates Position.
 			Tuple3d pos = ApogyCommonMathFacade.INSTANCE.extractPosition(newPose);
 			this.position.setX(pos.getX());
 			this.position.setY(pos.getY());
 			this.position.setZ(pos.getZ());

 			// Updates Orientation.
 			Tuple3d ori = ApogyCommonMathFacade.INSTANCE.extractOrientation(newPose);
 			this.orientation.setX(Math.toDegrees(ori.getX()));
 			this.orientation.setY(Math.toDegrees(ori.getY()));
 			this.orientation.setZ(Math.toDegrees(ori.getZ()));

 			// Update distance.
 			if (this.previousPosition != null) {
 				Point3d p0 = new Point3d(this.previousPosition.asTuple3d());
 				Point3d p1 = new Point3d(pos.asTuple3d());

 				double distanceDelta = p0.distance(p1);

 				// Checks if the displacement is significant enough.
 				if (distanceDelta >= this.minimumDeltaPosition) {
 					this.tripDistance += distanceDelta;

 					this.previousPosition = EcoreUtil.copy(this.position);
 				}
 			} else {
 				this.previousPosition = EcoreUtil.copy(this.position);
 			}

 			// Update distance displays
 			if (this.txtTripDistance != null && !this.txtTripDistance.isDisposed()) {
 				this.txtTripDistance.setText(this.distanceFormat.format(this.tripDistance));
 			}
 		}
 	}

 	protected Adapter getPoseAdapter() {
 		if (this.poseAdapter == null) {
 			this.poseAdapter = new AdapterImpl() {
 				@Override
 				public void notifyChanged(Notification msg) {
 					if (msg.getNotifier() instanceof PoseProvider) {
 						int featureID = msg.getFeatureID(PoseProvider.class);
 						switch (featureID) {
 						case ApogyCorePackage.POSE_PROVIDER__POSE_TRANSFORM:
 							if (msg.getNewValue() instanceof Matrix4x4) {
 								final Matrix4x4 newPose = (Matrix4x4) msg.getNewValue();

 								// Do this on UI Thread.
 								if (!ApogySystemPoseComposite.this.isDisposed()) {
 									getDisplay().asyncExec(new Runnable() {
 										@Override
 										public void run() {
 											updatePose(newPose);
 										}
 									});
 								}
 							}
 							break;

 						default:
 							break;
 						}
 					}
 				}
 			};
 		}

 		return this.poseAdapter;
 	}

 	private class OdometrySetValueDialog extends Dialog {
 		public double tripDistanceValue = 0.0;

 		public OdometrySetValueDialog(Shell parentShell) {
 			super(parentShell);
 		}

 		@Override
 		protected Control createDialogArea(Composite parent) {
 			Composite container = (Composite) super.createDialogArea(parent);
 			container.setLayout(new GridLayout(2, false));

 			Label label = new Label(container, SWT.NONE);
 			label.setText("Trip distance (m) :");

 			Text text = new Text(container, SWT.BORDER);
 			text.setText("0.0");
 			GridData gd = new GridData(SWT.LEFT, SWT.CENTER, false, false);
 			gd.minimumWidth = 250;
 			gd.widthHint = 250;
 			text.setLayoutData(gd);

 			text.addKeyListener(new KeyListener() {
 				Color color = new Color(getDisplay(), text.getBackground().getRed(), text.getBackground().getGreen(),
 						text.getBackground().getBlue());

 				@Override
 				public void keyReleased(KeyEvent e) {
 					try {
 						OdometrySetValueDialog.this.tripDistanceValue = Double.parseDouble(text.getText());
 						text.setBackground(this.color);
 					} catch (Exception ex) {
 						text.setBackground(new Color(getDisplay(), 255, 0, 0));
 					}
 				}

 				@Override
 				public void keyPressed(KeyEvent e) {
 					try {
 						OdometrySetValueDialog.this.tripDistanceValue = Double.parseDouble(text.getText());
 						text.setBackground(this.color);
 					} catch (Exception ex) {
 						text.setBackground(new Color(getDisplay(), 255, 0, 0));
 					}
 				}
 			});

 			return container;
 		}

 		// overriding this methods allows you to set the
 		// title of the custom dialog
 		@Override
 		protected void configureShell(Shell newShell) {
 			super.configureShell(newShell);
 			newShell.setText("Set current trip distance value");
 		}

 		@Override
 		protected Point getInitialSize() {
 			return new Point(450, 300);
 		}

 	}
 }
	/*******************************************************************************
	* Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency
	* All rights reserved. This program and the accompanying materials
	* are made available under the terms of the Eclipse Public License v1.0
	* which accompanies this distribution, and is available at
	* http://www.eclipse.org/legal/epl-v10.html
	*
	* Contributors:
	* Pierre Allard,
	* Regent L'Archeveque - initial API and implementation
	*
	* SPDX-License-Identifier: EPL-1.0
	*******************************************************************************/
	package org.eclipse.apogy.core.ui.composites;

	import java.text.DecimalFormat;

	import javax.vecmath.Point3d;

	import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
	import org.eclipse.apogy.common.math.ApogyCommonMathFactory;
	import org.eclipse.apogy.common.math.Matrix4x4;
	import org.eclipse.apogy.common.math.Tuple3d;
	import org.eclipse.apogy.common.math.ui.composites.Tuple3dComposite;
	import org.eclipse.apogy.core.ApogyCorePackage;
	import org.eclipse.apogy.core.ApogySystemApiAdapter;
	import org.eclipse.apogy.core.PoseProvider;
	import org.eclipse.apogy.core.invocator.ApogyCoreInvocatorFacade;
	import org.eclipse.apogy.core.invocator.VariableFeatureReference;
	import org.eclipse.apogy.core.invocator.VariablesList;
	import org.eclipse.apogy.core.invocator.ui.composites.VariableFeatureReferenceComposite;
	import org.eclipse.emf.common.notify.Adapter;
	import org.eclipse.emf.common.notify.Notification;
	import org.eclipse.emf.common.notify.impl.AdapterImpl;
	import org.eclipse.emf.ecore.util.EcoreUtil;
	import org.eclipse.jface.dialogs.Dialog;
	import org.eclipse.jface.window.Window;
	import org.eclipse.swt.SWT;
	import org.eclipse.swt.events.DisposeEvent;
	import org.eclipse.swt.events.DisposeListener;
	import org.eclipse.swt.events.KeyEvent;
	import org.eclipse.swt.events.KeyListener;
	import org.eclipse.swt.events.SelectionEvent;
	import org.eclipse.swt.events.SelectionListener;
	import org.eclipse.swt.graphics.Color;
	import org.eclipse.swt.graphics.Point;
	import org.eclipse.swt.layout.GridData;
	import org.eclipse.swt.layout.GridLayout;
	import org.eclipse.swt.widgets.Button;
	import org.eclipse.swt.widgets.Composite;
	import org.eclipse.swt.widgets.Control;
	import org.eclipse.swt.widgets.Display;
	import org.eclipse.swt.widgets.Label;
	import org.eclipse.swt.widgets.Shell;
	import org.eclipse.swt.widgets.Text;

	public class ApogySystemPoseComposite extends ApogySystemApiAdapterBasedComposite {
	public static final String DEGREE_STRING = "\u00b0";
	public static final int ATTITUDE_INDICATOR_WIDTH = 200;
	public static final int ATTITUDE_INDICATOR_HEIGHT = 200;

	private Adapter poseAdapter;

	private VariablesList variables;
	// private VariableFeatureReference variableFeatureReference;

	private Tuple3d previousPosition = null;
	private final Tuple3d position = ApogyCommonMathFactory.eINSTANCE.createTuple3d();
	private final Tuple3d orientation = ApogyCommonMathFactory.eINSTANCE.createTuple3d();

	private double minimumDeltaPosition = 0.05;
	private double tripDistance = 0.0;

	private final DecimalFormat distanceFormat = new DecimalFormat("0.00");

	private Text txtVariableFeatureReference;
	private Text txtTripDistance;
	private Button btnTripReset;
	private Button btnTripSet;

	private Tuple3dComposite translationComposite;
	private Tuple3dComposite rotationComposite;

	public ApogySystemPoseComposite(Composite parent, int style) {
	this(parent, style, null, null, 0.0);
	}

	public ApogySystemPoseComposite(Composite parent, int style, VariablesList variables,
	VariableFeatureReference variableFeatureReference, double tripDistance) {
	super(parent, style);

	this.tripDistance = tripDistance;
	this.variables = variables;

	GridLayout layout = new GridLayout(4, false);
	setLayout(layout);

	Label lblVariableFeatureReference = new Label(this, SWT.NONE);
	lblVariableFeatureReference.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
	lblVariableFeatureReference.setText("Variable Feature : ");

	this.txtVariableFeatureReference = new Text(this, SWT.BORDER);
	GridData gd_txtVariableFeatureReference = new GridData(SWT.FILL, SWT.CENTER, false, false, 2, 1);
	gd_txtVariableFeatureReference.widthHint = 300;
	this.txtVariableFeatureReference.setLayoutData(gd_txtVariableFeatureReference);

	Button btnSelectVariableFeatureReference = new Button(this, SWT.PUSH);
	btnSelectVariableFeatureReference.setText("Select...");
	btnSelectVariableFeatureReference.addSelectionListener(new SelectionListener() {

	@Override
	public void widgetSelected(SelectionEvent e) {
	final Shell shell = Display.getCurrent().getActiveShell();
	Dialog dialog = new Dialog(shell) {
	VariableFeatureReferenceComposite vfrComposite = null;

	@Override
	protected Control createDialogArea(Composite parent) {
	Composite area = (Composite) super.createDialogArea(parent);

	this.vfrComposite = new VariableFeatureReferenceComposite(area, SWT.NONE);
	this.vfrComposite.setLayoutData(new GridData(SWT.FILL, SWT.CENTER, false, false, 3, 1));
	this.vfrComposite.set(ApogySystemPoseComposite.this.variables,
	ApogySystemPoseComposite.this.getVariableFeatureReference());

	return area;
	}

	@Override
	public boolean close() {
	if (this.vfrComposite != null && !this.vfrComposite.isDisposed()) {
	this.vfrComposite.set(null, null);
	this.vfrComposite.dispose();
	}

	return super.close();
	}
	};

	int result = dialog.open();
	if (result == Window.OK) {
	// Update displayed VFR.
	updateDisplayedVariableFeatureReference();
	}
	}

	@Override
	public void widgetDefaultSelected(SelectionEvent e) {
	}
	});

	// Trip Distance.
	Label lblPOdometer = new Label(this, SWT.NONE);
	lblPOdometer.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
	lblPOdometer.setText("Trip Odometer (m):");

	this.txtTripDistance = new Text(this, SWT.BORDER \| SWT.SINGLE);
	this.txtTripDistance.setText(this.distanceFormat.format(this.tripDistance));
	this.txtTripDistance.setEnabled(false);
	GridData gd_lblTripdistancelabel = new GridData(SWT.LEFT, SWT.CENTER, false, false, 1, 1);
	gd_lblTripdistancelabel.widthHint = 150;
	gd_lblTripdistancelabel.minimumWidth = 150;
	this.txtTripDistance.setLayoutData(gd_lblTripdistancelabel);

	this.btnTripReset = new Button(this, SWT.NONE);
	this.btnTripReset.setLayoutData(new GridData(SWT.CENTER, SWT.CENTER, false, false, 1, 1));
	this.btnTripReset.setText("Reset");
	this.btnTripReset.setEnabled(false);
	this.btnTripReset.addSelectionListener(new SelectionListener() {
	@Override
	public void widgetSelected(SelectionEvent e) {
	setTripDistance(0.0);
	// txtTripDistance.setText(distanceFormat.format(0.0));
	}

	@Override
	public void widgetDefaultSelected(SelectionEvent e) {
	}
	});

	this.btnTripSet = new Button(this, SWT.NONE);
	this.btnTripSet.setLayoutData(new GridData(SWT.CENTER, SWT.CENTER, false, false, 1, 1));
	this.btnTripSet.setText("Set To Value");
	this.btnTripSet.setEnabled(false);
	this.btnTripSet.addSelectionListener(new SelectionListener() {
	@Override
	public void widgetSelected(SelectionEvent e) {
	OdometrySetValueDialog dialog = new OdometrySetValueDialog(getShell());
	if (dialog.open() == Window.OK) {
	setTripDistance(dialog.tripDistanceValue);
	}
	}

	@Override
	public void widgetDefaultSelected(SelectionEvent e) {
	}
	});

	// Translation
	Label lblPosition = new Label(this, SWT.NONE);
	lblPosition.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
	lblPosition.setAlignment(SWT.RIGHT);
	lblPosition.setText("Position (m):");

	this.translationComposite = new Tuple3dComposite(this, SWT.NONE, "0.00");
	this.translationComposite.setEnabled(false);
	GridData gd_translationComposite = new GridData(SWT.LEFT, SWT.CENTER, false, false, 3, 1);
	gd_translationComposite.minimumWidth = 300;
	gd_translationComposite.widthHint = 300;
	this.translationComposite.setLayoutData(gd_translationComposite);
	this.translationComposite.setTuple3d(this.position);
	this.translationComposite.setEnableEditing(false);

	// Rotation
	Label lblOrientation = new Label(this, SWT.NONE);
	lblOrientation.setLayoutData(new GridData(SWT.RIGHT, SWT.CENTER, false, false, 1, 1));
	lblOrientation.setAlignment(SWT.RIGHT);
	lblOrientation.setText("Orientation (" + DEGREE_STRING + "):");

	this.rotationComposite = new Tuple3dComposite(this, SWT.NONE, "0.0");
	this.rotationComposite.setEnabled(false);
	GridData gd_rotationComposite = new GridData(SWT.LEFT, SWT.CENTER, false, false, 3, 1);
	gd_rotationComposite.minimumWidth = 300;
	gd_rotationComposite.widthHint = 300;
	this.rotationComposite.setLayoutData(gd_rotationComposite);
	this.rotationComposite.setTuple3d(this.orientation);
	this.rotationComposite.setEnableEditing(false);
	setVariableFeatureReference(variableFeatureReference);

	updateDisplayedVariableFeatureReference();

	addDisposeListener(new DisposeListener() {

	@Override
	public void widgetDisposed(DisposeEvent e) {
	// Unregister adapter from apogySystemApiAdapter if required.
	if (getApogySystemApiAdapter() != null) {
	getApogySystemApiAdapter().eAdapters().remove(getPoseAdapter());
	}

	// Clear the Tuple3D composites.
	if (!ApogySystemPoseComposite.this.translationComposite.isDisposed())
	ApogySystemPoseComposite.this.translationComposite.setTuple3d(null);
	if (!ApogySystemPoseComposite.this.rotationComposite.isDisposed())
	ApogySystemPoseComposite.this.rotationComposite.setTuple3d(null);
	}
	});
	}

	public double getTripDistance() {
	return this.tripDistance;
	}

	public void setTripDistance(double tripDistance) {
	this.tripDistance = tripDistance;

	// Do this on UI Thread.
	getDisplay().asyncExec(new Runnable() {
	@Override
	public void run() {
	// Update distance displays
	if (ApogySystemPoseComposite.this.txtTripDistance != null
	&& !ApogySystemPoseComposite.this.txtTripDistance.isDisposed()) {
	ApogySystemPoseComposite.this.txtTripDistance
	.setText(ApogySystemPoseComposite.this.distanceFormat.format(tripDistance));
	}
	}
	});
	}

	@Override
	protected void apogySystemApiAdapterChanged(final ApogySystemApiAdapter oldApogySystemApiAdapter,
	final ApogySystemApiAdapter newApogySystemApiAdapter) {
	// Do this on UI Thread.
	getDisplay().asyncExec(new Runnable() {
	@Override
	public void run() {
	updateApogySystemApiAdapter(oldApogySystemApiAdapter, newApogySystemApiAdapter);
	}
	});
	}

	public void setVariableList(final VariablesList variables) {
	this.variables = variables;
	}

	@Override
	public void setVariableFeatureReference(VariableFeatureReference newVariableFeatureReference) {
	super.setVariableFeatureReference(newVariableFeatureReference);

	getDisplay().asyncExec(new Runnable() {
	@Override
	public void run() {
	updateDisplayedVariableFeatureReference();
	}
	});
	}

	/**
	* Returns the minimum distance between two position update that should be
	* considered significant enough to update the trip odometer distance.
	*
	* @return minimum distance, in meters.
	*/
	public double getMinimumDeltaPosition() {
	return this.minimumDeltaPosition;
	}

	/**
	* Sets the minimum distance between two position update that should be
	* considered significant enough to update the trip odometer distance.
	*
	* @param minimumDeltaPosition The minimum distance, in meters.
	*/
	public void setMinimumDeltaPosition(double minimumDeltaPosition) {
	this.minimumDeltaPosition = minimumDeltaPosition;
	}

	protected void updateApogySystemApiAdapter(ApogySystemApiAdapter oldApogySystemApiAdapter,
	ApogySystemApiAdapter newApogySystemApiAdapter) {
	// Unregister adapter from previous apogySystemApiAdapter is required.
	if (oldApogySystemApiAdapter != null) {
	oldApogySystemApiAdapter.eAdapters().remove(getPoseAdapter());
	}

	if (newApogySystemApiAdapter != null) {
	// Updates pose.
	updatePose(newApogySystemApiAdapter.getPoseTransform());

	// Register adapter to new apogySystemApiAdapter.
	newApogySystemApiAdapter.eAdapters().add(getPoseAdapter());
	}

	// Enables / Disable displays.
	setDisplaysEnabled(newApogySystemApiAdapter != null);
	}

	protected void setDisplaysEnabled(boolean enabled) {
	if (this.translationComposite != null && !this.translationComposite.isDisposed()) {
	this.translationComposite.setEnabled(enabled);
	}

	if (this.rotationComposite != null && !this.rotationComposite.isDisposed()) {
	this.rotationComposite.setEnabled(enabled);
	}

	if (this.txtTripDistance != null && !this.txtTripDistance.isDisposed()) {
	this.txtTripDistance.setEnabled(enabled);
	}

	if (this.btnTripReset != null && !this.btnTripReset.isDisposed()) {
	this.btnTripReset.setEnabled(enabled);
	}

	if (this.btnTripSet != null && !this.btnTripSet.isDisposed()) {
	this.btnTripSet.setEnabled(enabled);
	}
	}

	protected void updateDisplayedVariableFeatureReference() {
	if (this.txtVariableFeatureReference != null && !this.txtVariableFeatureReference.isDisposed()) {
	String text = ApogyCoreInvocatorFacade.INSTANCE
	.getVariableFeatureReferenceString(this.getVariableFeatureReference());
	this.txtVariableFeatureReference.setText(text);
	}
	}

	protected void updatePose(Matrix4x4 newPose) {
	if (newPose != null) {
	// Updates Position.
	Tuple3d pos = ApogyCommonMathFacade.INSTANCE.extractPosition(newPose);
	this.position.setX(pos.getX());
	this.position.setY(pos.getY());
	this.position.setZ(pos.getZ());

	// Updates Orientation.
	Tuple3d ori = ApogyCommonMathFacade.INSTANCE.extractOrientation(newPose);
	this.orientation.setX(Math.toDegrees(ori.getX()));
	this.orientation.setY(Math.toDegrees(ori.getY()));
	this.orientation.setZ(Math.toDegrees(ori.getZ()));

	// Update distance.
	if (this.previousPosition != null) {
	Point3d p0 = new Point3d(this.previousPosition.asTuple3d());
	Point3d p1 = new Point3d(pos.asTuple3d());

	double distanceDelta = p0.distance(p1);

	// Checks if the displacement is significant enough.
	if (distanceDelta >= this.minimumDeltaPosition) {
	this.tripDistance += distanceDelta;

	this.previousPosition = EcoreUtil.copy(this.position);
	}
	} else {
	this.previousPosition = EcoreUtil.copy(this.position);
	}

	// Update distance displays
	if (this.txtTripDistance != null && !this.txtTripDistance.isDisposed()) {
	this.txtTripDistance.setText(this.distanceFormat.format(this.tripDistance));
	}
	}
	}

	protected Adapter getPoseAdapter() {
	if (this.poseAdapter == null) {
	this.poseAdapter = new AdapterImpl() {
	@Override
	public void notifyChanged(Notification msg) {
	if (msg.getNotifier() instanceof PoseProvider) {
	int featureID = msg.getFeatureID(PoseProvider.class);
	switch (featureID) {
	case ApogyCorePackage.POSE_PROVIDER__POSE_TRANSFORM:
	if (msg.getNewValue() instanceof Matrix4x4) {
	final Matrix4x4 newPose = (Matrix4x4) msg.getNewValue();

	// Do this on UI Thread.
	if (!ApogySystemPoseComposite.this.isDisposed()) {
	getDisplay().asyncExec(new Runnable() {
	@Override
	public void run() {
	updatePose(newPose);
	}
	});
	}
	}
	break;

	default:
	break;
	}
	}
	}
	};
	}

	return this.poseAdapter;
	}

	private class OdometrySetValueDialog extends Dialog {
	public double tripDistanceValue = 0.0;

	public OdometrySetValueDialog(Shell parentShell) {
	super(parentShell);
	}

	@Override
	protected Control createDialogArea(Composite parent) {
	Composite container = (Composite) super.createDialogArea(parent);
	container.setLayout(new GridLayout(2, false));

	Label label = new Label(container, SWT.NONE);
	label.setText("Trip distance (m) :");

	Text text = new Text(container, SWT.BORDER);
	text.setText("0.0");
	GridData gd = new GridData(SWT.LEFT, SWT.CENTER, false, false);
	gd.minimumWidth = 250;
	gd.widthHint = 250;
	text.setLayoutData(gd);

	text.addKeyListener(new KeyListener() {
	Color color = new Color(getDisplay(), text.getBackground().getRed(), text.getBackground().getGreen(),
	text.getBackground().getBlue());

	@Override
	public void keyReleased(KeyEvent e) {
	try {
	OdometrySetValueDialog.this.tripDistanceValue = Double.parseDouble(text.getText());
	text.setBackground(this.color);
	} catch (Exception ex) {
	text.setBackground(new Color(getDisplay(), 255, 0, 0));
	}
	}

	@Override
	public void keyPressed(KeyEvent e) {
	try {
	OdometrySetValueDialog.this.tripDistanceValue = Double.parseDouble(text.getText());
	text.setBackground(this.color);
	} catch (Exception ex) {
	text.setBackground(new Color(getDisplay(), 255, 0, 0));
	}
	}
	});

	return container;
	}

	// overriding this methods allows you to set the
	// title of the custom dialog
	@Override
	protected void configureShell(Shell newShell) {
	super.configureShell(newShell);
	newShell.setText("Set current trip distance value");
	}

	@Override
	protected Point getInitialSize() {
	return new Point(450, 300);
	}

	}
	}