bundles/addons/org.eclipse.apogy.addons.vehicle/src-gen-custom/org/eclipse/apogy/addons/vehicle/impl/VehicleTrajectoryPickingToolCustomImpl.java

/*******************************************************************************
 * 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.addons.vehicle.impl;

import javax.vecmath.Matrix4d;
import javax.vecmath.Vector3d;

import org.eclipse.apogy.addons.geometry.paths.ApogyAddonsGeometryPathsFactory;
import org.eclipse.apogy.addons.geometry.paths.WayPointPath;
import org.eclipse.apogy.common.emf.transaction.ApogyCommonTransactionFacade;
import org.eclipse.apogy.common.geometry.data.ApogyCommonGeometryDataPackage;
import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFacade;
import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
import org.eclipse.apogy.common.topology.ui.NodeSelection;
import org.eclipse.apogy.common.topology.ui.viewer.MouseButton;
import org.eclipse.apogy.core.ApogySystemApiAdapter;
import org.eclipse.apogy.core.invocator.ApogyCoreInvocatorFacade;
import org.eclipse.apogy.core.invocator.TypeApiAdapter;

public class VehicleTrajectoryPickingToolCustomImpl extends VehicleTrajectoryPickingToolImpl {

	@Override
	public WayPointPath getLocalPath() {
		if (getActivePath() != null) {
			Matrix4d vehiculePose = getVehiculePose();
			Matrix4d inv = new Matrix4d(vehiculePose);
			inv.invert();

			WayPointPath localPath = ApogyAddonsGeometryPathsFactory.eINSTANCE.createWayPointPath();

			for (CartesianPositionCoordinates point : getActivePath().getPoints()) {
				// Transform the point in the vehicule frame.
				Vector3d pointPosition = new Vector3d(point.asPoint3d());

				Matrix4d pointPose = new Matrix4d();
				pointPose.setIdentity();
				pointPose.setTranslation(pointPosition);
				pointPose.invert();
				pointPose.mul(vehiculePose);
				pointPose.invert();

				Vector3d pointRelativePosition = new Vector3d();
				pointPose.get(pointRelativePosition);

				CartesianPositionCoordinates localPoint = ApogyCommonGeometryData3DFacade.INSTANCE
						.createCartesianPositionCoordinates(pointRelativePosition.x, pointRelativePosition.y,
								pointRelativePosition.z);

				// Adds the local point into the path.
				localPath.getPoints().add(localPoint);
			}

			return localPath;
		} else {
			return null;
		}
	}

	@Override
	public void setActive(boolean newActive) {
		try {
			super.setActive(newActive);
		} catch (Throwable t) {
		}

		// Clears the path.
		if (newActive && getActivePath() != null) {
			ApogyCommonTransactionFacade.INSTANCE.basicRemove(getActivePath(),
					ApogyCommonGeometryDataPackage.Literals.COORDINATES_SET__POINTS, getActivePath().getPoints());
		}
	}

	@Override
	public void selectionChanged(NodeSelection nodeSelection) {
		if (getActivePath() != null && getActivePath().getPoints().isEmpty()) {
			// Adds the current position of the vehicule.
			ApogyCommonTransactionFacade.INSTANCE.basicAdd(getActivePath(),
					ApogyCommonGeometryDataPackage.Literals.COORDINATES_SET__POINTS, getVechiculeCurrentPosition());
		}

		super.selectionChanged(nodeSelection);
	}

	@Override
	public void mouseButtonClicked(MouseButton mouseButtonClicked) {
		if (mouseButtonClicked == MouseButton.RIGHT) {
			if (getActivePath() != null && getActivePath().getPoints().size() > 1) {
				CartesianPositionCoordinates coord = getActivePath().getPoints()
						.get(getActivePath().getPoints().size() - 1);
				ApogyCommonTransactionFacade.INSTANCE.basicRemove(getActivePath(),
						ApogyCommonGeometryDataPackage.Literals.COORDINATES_SET__POINTS, coord);
			}
		}
	}

	/**
	 * Gets the current pose of the specified VehiculeVariableFeatureReference.
	 * 
	 * @return The matrix4d representing the vehicule pose.
	 */
	protected Matrix4d getVehiculePose() {
		Matrix4d m = new Matrix4d();
		m.setIdentity();

		if (getVehiculeVariableFeatureReference() != null) {
			TypeApiAdapter apiAdapter = ApogyCoreInvocatorFacade.INSTANCE
					.getTypeApiAdapter(getVehiculeVariableFeatureReference());
			if (apiAdapter instanceof ApogySystemApiAdapter) {
				ApogySystemApiAdapter apogySystemApiAdapter = (ApogySystemApiAdapter) apiAdapter;
				if (apogySystemApiAdapter.getPoseTransform() != null) {
					m = apogySystemApiAdapter.getPoseTransform().asMatrix4d();
				}
			}
		}

		return m;
	}

	protected CartesianPositionCoordinates getVechiculeCurrentPosition() {
		Matrix4d vehiclePose = getVehiculePose();
		Vector3d position = new Vector3d();
		vehiclePose.get(position);

		CartesianPositionCoordinates point = ApogyCommonGeometryData3DFacade.INSTANCE
				.createCartesianPositionCoordinates(position.x, position.y, position.z);

		return point;
	}
} // VehicleTrajectoryPickingToolImpl