bundles/addons/org.eclipse.apogy.addons.vehicle/src-gen-custom/org/eclipse/apogy/addons/vehicle/impl/PathPlannerToolCustomImpl.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.addons.vehicle.impl;

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

 import org.eclipse.apogy.addons.ApogyAddonsPackage;
 import org.eclipse.apogy.addons.geometry.paths.WayPointPath;
 import org.eclipse.apogy.addons.mobility.pathplanners.ApogyAddonsMobilityPathplannersPackage;
 import org.eclipse.apogy.addons.mobility.pathplanners.MeshWayPointPathPlanner;
 import org.eclipse.apogy.addons.mobility.pathplanners.graph.ApogyAddonsMobilityPathplannersGraphFactory;
 import org.eclipse.apogy.addons.mobility.pathplanners.graph.DistanceAndSlopesCostFunction;
 import org.eclipse.apogy.addons.mobility.pathplanners.graph.SimpleDirectedWeightedGraphBasedMeshWayPointPathPlanner;
 import org.eclipse.apogy.addons.vehicle.ApogyAddonsVehicleFactory;
 import org.eclipse.apogy.addons.vehicle.ApogyAddonsVehiclePackage;
 import org.eclipse.apogy.addons.vehicle.PathPlannerToolNode;
 import org.eclipse.apogy.common.emf.ApogyCommonEMFFacade;
 import org.eclipse.apogy.common.emf.transaction.ApogyCommonTransactionFacade;
 import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFacade;
 import org.eclipse.apogy.common.geometry.data3d.CartesianAxis;
 import org.eclipse.apogy.common.geometry.data3d.CartesianPolygon;
 import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
 import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
 import org.eclipse.apogy.common.math.Tuple3d;
 import org.eclipse.apogy.common.processors.ApogyCommonProcessorsPackage;
 import org.eclipse.apogy.common.topology.GroupNode;
 import org.eclipse.apogy.common.topology.Node;
 import org.eclipse.apogy.common.topology.ui.NodeSelection;
 import org.eclipse.apogy.core.environment.surface.CartesianTriangularMeshMapLayer;
 import org.eclipse.core.runtime.IProgressMonitor;
 import org.eclipse.core.runtime.IStatus;
 import org.eclipse.core.runtime.Status;
 import org.eclipse.core.runtime.jobs.Job;
 import org.eclipse.emf.ecore.util.EcoreUtil;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;

 public class PathPlannerToolCustomImpl extends PathPlannerToolImpl {

 	private static final Logger Logger = LoggerFactory.getLogger(PathPlannerToolImpl.class);

 	public static int FROM_NODE_INDEX = 0;
 	public static int TO_NODE_INDEX = 1;
 	private int nextNode = FROM_NODE_INDEX;

 	@Override
 	public void setMeshLayer(CartesianTriangularMeshMapLayer newMeshLayer) {
 		super.setMeshLayer(newMeshLayer);

 		if (getPathPlanner() != null && newMeshLayer != null) {
 			ApogyCommonTransactionFacade.INSTANCE.basicSet(getPathPlanner(),
 					ApogyAddonsMobilityPathplannersPackage.Literals.MESH_WAY_POINT_PATH_PLANNER__MESH,
 					newMeshLayer.getCurrentMesh());
 			// getPathPlanner().setMesh(newMeshLayer.getCurrentMesh());
 		}
 	}

 	@Override
 	@SuppressWarnings({ "rawtypes" })
 	public MeshWayPointPathPlanner getPathPlanner() {
 		if (this.pathPlanner == null) {
 			SimpleDirectedWeightedGraphBasedMeshWayPointPathPlanner<CartesianPolygon> tmp = ApogyAddonsMobilityPathplannersGraphFactory.eINSTANCE
 					.createSimpleDirectedWeightedGraphBasedMeshWayPointPathPlanner();
 			DistanceAndSlopesCostFunction das = ApogyAddonsMobilityPathplannersGraphFactory.eINSTANCE
 					.createDistanceAndSlopesCostFunction();
 			das.setGravityAxis(CartesianAxis.Z);
 			tmp.getCostFunctions().add(das);
 			tmp.setEnablePathSimplification(false);

 			setPathPlanner(tmp);
 		}
 		return this.pathPlanner;
 	}

 	@Override
 	public PathPlannerToolNode getPathPlannerToolNode() {
 		PathPlannerToolNode node = super.getPathPlannerToolNode();

 		if (node == null) {
 			node = ApogyAddonsVehicleFactory.eINSTANCE.createPathPlannerToolNode();

 			String nodeID = "PathPlannerToolNode_";
 			if (getName() != null) {
 				nodeID += getName() + "_" + ApogyCommonEMFFacade.INSTANCE.getID(this);
 			} else {
 				nodeID += ApogyCommonEMFFacade.INSTANCE.getID(this);
 			}
 			node.setNodeId(nodeID);

 			String description = "Node representing the tool named <" + getName() + ">";
 			if (getDescription() != null) {
 				description += ": " + getDescription();
 			}
 			node.setDescription(description);

 			setPathPlannerToolNode(node);
 		}

 		return node;
 	}

 	@Override
 	public void setRootNode(Node newRootNode) {
 		if (newRootNode instanceof GroupNode) {
 			((GroupNode) newRootNode).getChildren().add(getPathPlannerToolNode());
 		} else {
 			if (getPathPlannerToolNode().getParent() instanceof GroupNode) {
 				((GroupNode) getPathPlannerToolNode().getParent()).getChildren().remove(getPathPlannerToolNode());
 			}
 		}

 		super.setRootNode(newRootNode);
 	}

 	@Override
 	public void dispose() {
 		// Detach the PathPlannerToolNode from the topology if applicable.
 		if (getPathPlannerToolNode() != null) {
 			if (getPathPlannerToolNode().getParent() instanceof GroupNode) {
 				GroupNode parent = (GroupNode) getPathPlannerToolNode().getParent();
 				parent.getChildren().remove(getPathPlannerToolNode());
 			}
 		}

 		super.dispose();
 	}

 	@Override
 	public void selectionChanged(NodeSelection nodeSelection) {
 		if (!isDisposed()) {
 			Node node = nodeSelection.getSelectedNode();

 			Tuple3d relativePosition = null;
 			if (nodeSelection.getRelativeIntersectionPoint() != null) {
 				relativePosition = ApogyCommonMathFacade.INSTANCE
 						.createTuple3d(nodeSelection.getRelativeIntersectionPoint());
 			}

 			Tuple3d normal = null;
 			if (nodeSelection.getAbsoluteIntersectionNormal() != null) {
 				normal = ApogyCommonMathFacade.INSTANCE.createTuple3d(nodeSelection.getAbsoluteIntersectionNormal().x,
 						nodeSelection.getAbsoluteIntersectionNormal().y,
 						nodeSelection.getAbsoluteIntersectionNormal().z);
 			}

 			if (this.nextNode == TO_NODE_INDEX) {
 				if (!isFromNodeLock()) {
 					updateFromNode(node, relativePosition, normal);
 				} else if (!isToNodeLock()) {
 					updateToNode(node, relativePosition, normal);
 				}

 				this.nextNode = FROM_NODE_INDEX;
 			} else if (this.nextNode == FROM_NODE_INDEX) {
 				if (!isToNodeLock()) {
 					updateToNode(node, relativePosition, normal);
 				} else if (!isFromNodeLock()) {
 					updateFromNode(node, relativePosition, normal);
 				}

 				this.nextNode = TO_NODE_INDEX;
 			}
 		}

 	}

 	@Override
 	public void pointsRelativePoseChanged(Matrix4d newPose) {
 		if (getPathPlanner() != null) {
 			ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 					ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__PLANNED_PATH, null);
 			// setPlannedPath(null);

 			planPathInternal();
 		}
 	}

 	@Override
 	public boolean planPath() {
 		boolean success = false;
 		ApogyCommonTransactionFacade.INSTANCE.basicSet(this, ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__BUSY,
 				true);
 		// setBusy(true);
 		try {
 			if (getPathPlanner() != null && getMeshLayer() != null) {
 				CartesianPositionCoordinates from = getFromCartesianPositionCoordinates();
 				CartesianPositionCoordinates to = getToCartesianPositionCoordinates();

 				// If both current location and destination are defined.
 				if (from != null && to != null) {
 					Logger.info("Planning path from " + from.asPoint3d() + " to " + to.asPoint3d() + "...");

 					// Sets the mesh if not already done
 					if (getPathPlanner().getMesh() != getMeshLayer().getCurrentMesh()) {
 						ApogyCommonTransactionFacade.INSTANCE.basicSet(getPathPlanner(),
 								ApogyAddonsMobilityPathplannersPackage.Literals.MESH_WAY_POINT_PATH_PLANNER__MESH,
 								getMeshLayer().getCurrentMesh());
 						// getPathPlanner().setMesh(getMeshLayer().getCurrentMesh());
 					}

 					// Plans a new path.
 					WayPointPath newPath = getPathPlanner().plan(from, to);
 					newPath.setNodeId(getName() + " Plan Path");
 					newPath.setDescription("Planned Path");

 					ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 							ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__PLANNED_PATH, newPath);
 					// setPlannedPath(newPath);

 					if (newPath != null) {
 						success = true;
 						Logger.info("Path planning sucessfully completed.");
 					}
 				} else {
 					String message = "Failed to plan path : ";
 					if (from == null)
 						message += "Current location is undefined.";
 					if (to == null)
 						message += " Destination is undefined.";

 					Logger.error(message);

 					ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 							ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__PLANNED_PATH, null);
 				}
 			}
 		} catch (Throwable t) {
 			Logger.error(t.getMessage(), t);
 		}

 		ApogyCommonTransactionFacade.INSTANCE.basicSet(this, ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__BUSY,
 				false);
 		return success;
 	}

 	protected void updateFromNode(Node node, Tuple3d relativePosition, Tuple3d normal) {
 		ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 				ApogyAddonsPackage.Literals.ABSTRACT_TWO_POINTS3_DTOOL__FROM_NODE, node);
 		// setFromNode(node);
 		if (relativePosition != null) {
 			ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 					ApogyAddonsPackage.Literals.ABSTRACT_TWO_POINTS3_DTOOL__FROM_RELATIVE_POSITION,
 					EcoreUtil.copy(relativePosition));
 			// setFromRelativePosition(EcoreUtil.copy(relativePosition));

 			if (isAutoPathPlanEnabled() && getPathPlanner() != null) {
 				planPathInternal();
 			}
 		}
 	}

 	protected void updateToNode(Node node, Tuple3d relativePosition, Tuple3d normal) {
 		ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 				ApogyAddonsPackage.Literals.ABSTRACT_TWO_POINTS3_DTOOL__TO_NODE, node);
 		// setToNode(node);

 		if (relativePosition != null) {
 			ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 					ApogyAddonsPackage.Literals.ABSTRACT_TWO_POINTS3_DTOOL__TO_RELATIVE_POSITION,
 					EcoreUtil.copy(relativePosition));
 			// setToRelativePosition(EcoreUtil.copy(relativePosition));

 			if (isAutoPathPlanEnabled() && getPathPlanner() != null) {
 				planPathInternal();
 			}
 		}
 	}

 	private void planPathInternal() {
 		if (!isBusy()) {
 			Job job = new Job("Planning Path") {
 				@Override
 				protected IStatus run(final IProgressMonitor monitor) {
 					try {
 						ApogyCommonTransactionFacade.INSTANCE.basicSet(getPathPlanner(),
 								ApogyCommonProcessorsPackage.Literals.MONITORABLE__PROGRESS_MONITOR, monitor);
 						// getPathPlanner().setProgressMonitor(monitor);

 						planPath();
 					} catch (Throwable t) {
 						Logger.error(t.getMessage(), t);
 					}

 					return Status.OK_STATUS;
 				}
 			};
 			job.schedule();
 		} else {
 			Logger.warn("Path Planner is busy.");
 		}
 	}

 	private CartesianPositionCoordinates getFromCartesianPositionCoordinates() {
 		CartesianPositionCoordinates from = null;
 		if (getFromAbsolutePosition() != null && getMeshLayer() != null) {
 			// Express to in the mesh coordinate system.
 			Matrix4d meshToWorld = getMeshLayer().getMap().getTransformation().asMatrix4d();
 			Vector3d fromVector = new Vector3d(getFromAbsolutePosition().asTuple3d());
 			meshToWorld.transform(fromVector);

 			from = ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(fromVector.x,
 					fromVector.y, fromVector.z);
 		}

 		return from;
 	}

 	private CartesianPositionCoordinates getToCartesianPositionCoordinates() {
 		CartesianPositionCoordinates to = null;
 		if (getToAbsolutePosition() != null && getMeshLayer() != null) {
 			// Express to in the mesh coordinate system.
 			Matrix4d meshToWorld = getMeshLayer().getMap().getTransformation().asMatrix4d();
 			Vector3d toVector = new Vector3d(getToAbsolutePosition().asTuple3d());
 			meshToWorld.transform(toVector);

 			to = ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(toVector.x, toVector.y,
 					toVector.z);
 		}

 		return to;
 	}

 } // PathPlannerToolImpl
	/*******************************************************************************
	* 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.ApogyAddonsPackage;
	import org.eclipse.apogy.addons.geometry.paths.WayPointPath;
	import org.eclipse.apogy.addons.mobility.pathplanners.ApogyAddonsMobilityPathplannersPackage;
	import org.eclipse.apogy.addons.mobility.pathplanners.MeshWayPointPathPlanner;
	import org.eclipse.apogy.addons.mobility.pathplanners.graph.ApogyAddonsMobilityPathplannersGraphFactory;
	import org.eclipse.apogy.addons.mobility.pathplanners.graph.DistanceAndSlopesCostFunction;
	import org.eclipse.apogy.addons.mobility.pathplanners.graph.SimpleDirectedWeightedGraphBasedMeshWayPointPathPlanner;
	import org.eclipse.apogy.addons.vehicle.ApogyAddonsVehicleFactory;
	import org.eclipse.apogy.addons.vehicle.ApogyAddonsVehiclePackage;
	import org.eclipse.apogy.addons.vehicle.PathPlannerToolNode;
	import org.eclipse.apogy.common.emf.ApogyCommonEMFFacade;
	import org.eclipse.apogy.common.emf.transaction.ApogyCommonTransactionFacade;
	import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFacade;
	import org.eclipse.apogy.common.geometry.data3d.CartesianAxis;
	import org.eclipse.apogy.common.geometry.data3d.CartesianPolygon;
	import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
	import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
	import org.eclipse.apogy.common.math.Tuple3d;
	import org.eclipse.apogy.common.processors.ApogyCommonProcessorsPackage;
	import org.eclipse.apogy.common.topology.GroupNode;
	import org.eclipse.apogy.common.topology.Node;
	import org.eclipse.apogy.common.topology.ui.NodeSelection;
	import org.eclipse.apogy.core.environment.surface.CartesianTriangularMeshMapLayer;
	import org.eclipse.core.runtime.IProgressMonitor;
	import org.eclipse.core.runtime.IStatus;
	import org.eclipse.core.runtime.Status;
	import org.eclipse.core.runtime.jobs.Job;
	import org.eclipse.emf.ecore.util.EcoreUtil;
	import org.slf4j.Logger;
	import org.slf4j.LoggerFactory;

	public class PathPlannerToolCustomImpl extends PathPlannerToolImpl {

	private static final Logger Logger = LoggerFactory.getLogger(PathPlannerToolImpl.class);

	public static int FROM_NODE_INDEX = 0;
	public static int TO_NODE_INDEX = 1;
	private int nextNode = FROM_NODE_INDEX;

	@Override
	public void setMeshLayer(CartesianTriangularMeshMapLayer newMeshLayer) {
	super.setMeshLayer(newMeshLayer);

	if (getPathPlanner() != null && newMeshLayer != null) {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(getPathPlanner(),
	ApogyAddonsMobilityPathplannersPackage.Literals.MESH_WAY_POINT_PATH_PLANNER__MESH,
	newMeshLayer.getCurrentMesh());
	// getPathPlanner().setMesh(newMeshLayer.getCurrentMesh());
	}
	}

	@Override
	@SuppressWarnings({ "rawtypes" })
	public MeshWayPointPathPlanner getPathPlanner() {
	if (this.pathPlanner == null) {
	SimpleDirectedWeightedGraphBasedMeshWayPointPathPlanner<CartesianPolygon> tmp = ApogyAddonsMobilityPathplannersGraphFactory.eINSTANCE
	.createSimpleDirectedWeightedGraphBasedMeshWayPointPathPlanner();
	DistanceAndSlopesCostFunction das = ApogyAddonsMobilityPathplannersGraphFactory.eINSTANCE
	.createDistanceAndSlopesCostFunction();
	das.setGravityAxis(CartesianAxis.Z);
	tmp.getCostFunctions().add(das);
	tmp.setEnablePathSimplification(false);

	setPathPlanner(tmp);
	}
	return this.pathPlanner;
	}

	@Override
	public PathPlannerToolNode getPathPlannerToolNode() {
	PathPlannerToolNode node = super.getPathPlannerToolNode();

	if (node == null) {
	node = ApogyAddonsVehicleFactory.eINSTANCE.createPathPlannerToolNode();

	String nodeID = "PathPlannerToolNode_";
	if (getName() != null) {
	nodeID += getName() + "_" + ApogyCommonEMFFacade.INSTANCE.getID(this);
	} else {
	nodeID += ApogyCommonEMFFacade.INSTANCE.getID(this);
	}
	node.setNodeId(nodeID);

	String description = "Node representing the tool named <" + getName() + ">";
	if (getDescription() != null) {
	description += ": " + getDescription();
	}
	node.setDescription(description);

	setPathPlannerToolNode(node);
	}

	return node;
	}

	@Override
	public void setRootNode(Node newRootNode) {
	if (newRootNode instanceof GroupNode) {
	((GroupNode) newRootNode).getChildren().add(getPathPlannerToolNode());
	} else {
	if (getPathPlannerToolNode().getParent() instanceof GroupNode) {
	((GroupNode) getPathPlannerToolNode().getParent()).getChildren().remove(getPathPlannerToolNode());
	}
	}

	super.setRootNode(newRootNode);
	}

	@Override
	public void dispose() {
	// Detach the PathPlannerToolNode from the topology if applicable.
	if (getPathPlannerToolNode() != null) {
	if (getPathPlannerToolNode().getParent() instanceof GroupNode) {
	GroupNode parent = (GroupNode) getPathPlannerToolNode().getParent();
	parent.getChildren().remove(getPathPlannerToolNode());
	}
	}

	super.dispose();
	}

	@Override
	public void selectionChanged(NodeSelection nodeSelection) {
	if (!isDisposed()) {
	Node node = nodeSelection.getSelectedNode();

	Tuple3d relativePosition = null;
	if (nodeSelection.getRelativeIntersectionPoint() != null) {
	relativePosition = ApogyCommonMathFacade.INSTANCE
	.createTuple3d(nodeSelection.getRelativeIntersectionPoint());
	}

	Tuple3d normal = null;
	if (nodeSelection.getAbsoluteIntersectionNormal() != null) {
	normal = ApogyCommonMathFacade.INSTANCE.createTuple3d(nodeSelection.getAbsoluteIntersectionNormal().x,
	nodeSelection.getAbsoluteIntersectionNormal().y,
	nodeSelection.getAbsoluteIntersectionNormal().z);
	}

	if (this.nextNode == TO_NODE_INDEX) {
	if (!isFromNodeLock()) {
	updateFromNode(node, relativePosition, normal);
	} else if (!isToNodeLock()) {
	updateToNode(node, relativePosition, normal);
	}

	this.nextNode = FROM_NODE_INDEX;
	} else if (this.nextNode == FROM_NODE_INDEX) {
	if (!isToNodeLock()) {
	updateToNode(node, relativePosition, normal);
	} else if (!isFromNodeLock()) {
	updateFromNode(node, relativePosition, normal);
	}

	this.nextNode = TO_NODE_INDEX;
	}
	}

	}

	@Override
	public void pointsRelativePoseChanged(Matrix4d newPose) {
	if (getPathPlanner() != null) {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__PLANNED_PATH, null);
	// setPlannedPath(null);

	planPathInternal();
	}
	}

	@Override
	public boolean planPath() {
	boolean success = false;
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this, ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__BUSY,
	true);
	// setBusy(true);
	try {
	if (getPathPlanner() != null && getMeshLayer() != null) {
	CartesianPositionCoordinates from = getFromCartesianPositionCoordinates();
	CartesianPositionCoordinates to = getToCartesianPositionCoordinates();

	// If both current location and destination are defined.
	if (from != null && to != null) {
	Logger.info("Planning path from " + from.asPoint3d() + " to " + to.asPoint3d() + "...");

	// Sets the mesh if not already done
	if (getPathPlanner().getMesh() != getMeshLayer().getCurrentMesh()) {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(getPathPlanner(),
	ApogyAddonsMobilityPathplannersPackage.Literals.MESH_WAY_POINT_PATH_PLANNER__MESH,
	getMeshLayer().getCurrentMesh());
	// getPathPlanner().setMesh(getMeshLayer().getCurrentMesh());
	}

	// Plans a new path.
	WayPointPath newPath = getPathPlanner().plan(from, to);
	newPath.setNodeId(getName() + " Plan Path");
	newPath.setDescription("Planned Path");

	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__PLANNED_PATH, newPath);
	// setPlannedPath(newPath);

	if (newPath != null) {
	success = true;
	Logger.info("Path planning sucessfully completed.");
	}
	} else {
	String message = "Failed to plan path : ";
	if (from == null)
	message += "Current location is undefined.";
	if (to == null)
	message += " Destination is undefined.";

	Logger.error(message);

	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__PLANNED_PATH, null);
	}
	}
	} catch (Throwable t) {
	Logger.error(t.getMessage(), t);
	}

	ApogyCommonTransactionFacade.INSTANCE.basicSet(this, ApogyAddonsVehiclePackage.Literals.PATH_PLANNER_TOOL__BUSY,
	false);
	return success;
	}

	protected void updateFromNode(Node node, Tuple3d relativePosition, Tuple3d normal) {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsPackage.Literals.ABSTRACT_TWO_POINTS3_DTOOL__FROM_NODE, node);
	// setFromNode(node);
	if (relativePosition != null) {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsPackage.Literals.ABSTRACT_TWO_POINTS3_DTOOL__FROM_RELATIVE_POSITION,
	EcoreUtil.copy(relativePosition));
	// setFromRelativePosition(EcoreUtil.copy(relativePosition));

	if (isAutoPathPlanEnabled() && getPathPlanner() != null) {
	planPathInternal();
	}
	}
	}

	protected void updateToNode(Node node, Tuple3d relativePosition, Tuple3d normal) {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsPackage.Literals.ABSTRACT_TWO_POINTS3_DTOOL__TO_NODE, node);
	// setToNode(node);

	if (relativePosition != null) {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsPackage.Literals.ABSTRACT_TWO_POINTS3_DTOOL__TO_RELATIVE_POSITION,
	EcoreUtil.copy(relativePosition));
	// setToRelativePosition(EcoreUtil.copy(relativePosition));

	if (isAutoPathPlanEnabled() && getPathPlanner() != null) {
	planPathInternal();
	}
	}
	}

	private void planPathInternal() {
	if (!isBusy()) {
	Job job = new Job("Planning Path") {
	@Override
	protected IStatus run(final IProgressMonitor monitor) {
	try {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(getPathPlanner(),
	ApogyCommonProcessorsPackage.Literals.MONITORABLE__PROGRESS_MONITOR, monitor);
	// getPathPlanner().setProgressMonitor(monitor);

	planPath();
	} catch (Throwable t) {
	Logger.error(t.getMessage(), t);
	}

	return Status.OK_STATUS;
	}
	};
	job.schedule();
	} else {
	Logger.warn("Path Planner is busy.");
	}
	}

	private CartesianPositionCoordinates getFromCartesianPositionCoordinates() {
	CartesianPositionCoordinates from = null;
	if (getFromAbsolutePosition() != null && getMeshLayer() != null) {
	// Express to in the mesh coordinate system.
	Matrix4d meshToWorld = getMeshLayer().getMap().getTransformation().asMatrix4d();
	Vector3d fromVector = new Vector3d(getFromAbsolutePosition().asTuple3d());
	meshToWorld.transform(fromVector);

	from = ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(fromVector.x,
	fromVector.y, fromVector.z);
	}

	return from;
	}

	private CartesianPositionCoordinates getToCartesianPositionCoordinates() {
	CartesianPositionCoordinates to = null;
	if (getToAbsolutePosition() != null && getMeshLayer() != null) {
	// Express to in the mesh coordinate system.
	Matrix4d meshToWorld = getMeshLayer().getMap().getTransformation().asMatrix4d();
	Vector3d toVector = new Vector3d(getToAbsolutePosition().asTuple3d());
	meshToWorld.transform(toVector);

	to = ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(toVector.x, toVector.y,
	toVector.z);
	}

	return to;
	}

	} // PathPlannerToolImpl