bundles/addons/org.eclipse.apogy.addons.mobility.pathplanners.graph/src-gen-custom/org/eclipse/apogy/addons/mobility/pathplanners/graph/impl/SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerCustomImpl.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,
  *     Sebastien Gemme - initial API and implementation
  *
  * SPDX-License-Identifier: EPL-1.0
  *
  *******************************************************************************/
 package org.eclipse.apogy.addons.mobility.pathplanners.graph.impl;

 import java.io.File;
 import java.io.IOException;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.Date;
 import java.util.List;
 import java.util.SortedSet;

 import org.eclipse.apogy.addons.geometry.paths.ApogyAddonsGeometryPathsFactory;
 import org.eclipse.apogy.addons.geometry.paths.PathUtilities;
 import org.eclipse.apogy.addons.geometry.paths.WayPointPath;
 import org.eclipse.apogy.addons.mobility.pathplanners.ApogyAddonsMobilityPathplannersPackage;
 import org.eclipse.apogy.addons.mobility.pathplanners.graph.ApogyAddonsMobilityPathplannersGraphFactory;
 import org.eclipse.apogy.addons.mobility.pathplanners.graph.ApogyAddonsMobilityPathplannersGraphPackage;
 import org.eclipse.apogy.addons.mobility.pathplanners.graph.CostBasedMeshWayPointPathPlanner;
 import org.eclipse.apogy.addons.mobility.pathplanners.graph.GraphUtilities;
 import org.eclipse.apogy.addons.mobility.pathplanners.graph.MobilityEdge;
 import org.eclipse.apogy.addons.mobility.pathplanners.graph.MobilityEdgeFactory;
 import org.eclipse.apogy.common.emf.transaction.ApogyCommonTransactionFacade;
 import org.eclipse.apogy.common.geometry.data.Mesh;
 import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFacade;
 import org.eclipse.apogy.common.geometry.data3d.CartesianCoordinatesSet;
 import org.eclipse.apogy.common.geometry.data3d.CartesianPlane;
 import org.eclipse.apogy.common.geometry.data3d.CartesianPolygon;
 import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
 import org.eclipse.apogy.common.geometry.data3d.CartesianTriangle;
 import org.eclipse.apogy.common.geometry.data3d.Geometry3DUtilities;
 import org.eclipse.apogy.common.topology.ApogyCommonTopologyFacade;
 import org.eclipse.apogy.common.topology.ContentNode;
 import org.eclipse.apogy.common.topology.TransformNode;
 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.common.util.URI;
 import org.eclipse.emf.ecore.resource.Resource;
 import org.eclipse.emf.ecore.resource.ResourceSet;
 import org.eclipse.emf.ecore.resource.impl.ResourceSetImpl;
 import org.jgrapht.graph.SimpleDirectedWeightedGraph;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;

 public class SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerCustomImpl<PolygonType extends CartesianPolygon>
 		extends SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerImpl<PolygonType> {

 	private static final Logger Logger = LoggerFactory
 			.getLogger(SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerImpl.class);
 	private Adapter costFunctionsAdapter = null;
 	private boolean graphIsDirty = true;

 	protected SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerCustomImpl() {
 		super();

 		// Register a listener to the CostFunction to ensure the
 		// SimpleDirectedWeightedGraph is updated.
 		this.eAdapters().add(getCostFunctionsAdapter());
 	}

 	@Override
 	@SuppressWarnings({ "rawtypes", "unchecked" })
 	public SimpleDirectedWeightedGraph getSimpleDirectedWeightedGraph() {
 		if (this.graphIsDirty) {
 			Logger.debug("Graph is dirty, regenerating it...");
 			try {
 				SimpleDirectedWeightedGraph graph = createGraph();
 				this.graphIsDirty = false;
 				ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 						ApogyAddonsMobilityPathplannersGraphPackage.Literals.SIMPLE_DIRECTED_WEIGHTED_GRAPH_BASED_MESH_WAY_POINT_PATH_PLANNER__SIMPLE_DIRECTED_WEIGHTED_GRAPH,
 						graph);
 			} catch (Exception e) {
 				Logger.error(e.getMessage(), e);
 				ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 						ApogyAddonsMobilityPathplannersGraphPackage.Literals.SIMPLE_DIRECTED_WEIGHTED_GRAPH_BASED_MESH_WAY_POINT_PATH_PLANNER__SIMPLE_DIRECTED_WEIGHTED_GRAPH,
 						null);
 			}
 		}
 		return super.getSimpleDirectedWeightedGraph();
 	}

 	@Override
 	public WayPointPath plan(CartesianPositionCoordinates currentPosition,
 			CartesianPositionCoordinates destinationPosition) throws Exception {
 		ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 				ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_POSITION,
 				currentPosition);
 		// setCurrentPosition(currentPosition);

 		ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 				ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_DESTINATION,
 				destinationPosition);
 		// setCurrentDestination(destinationPosition);

 		Date start = new Date();
 		WayPointPath path = ApogyAddonsGeometryPathsFactory.eINSTANCE.createWayPointPath();

 		// Gets the polygon path.
 		List<CartesianPolygon> polygonPath = null;
 		polygonPath = getPolygonPath(currentPosition, destinationPosition);

 		// Generates the WayPointPath that goes through the polygon path.
 		path = generatePath(currentPosition, destinationPosition, polygonPath);
 		Date end = new Date();

 		double time = (end.getTime() - start.getTime()) / 1000.0;
 		Logger.debug("Planned path between (" + currentPosition.getX() + ", " + currentPosition.getY() + ", "
 				+ currentPosition.getZ() + ") and (" + destinationPosition.getX() + ", " + destinationPosition.getY()
 				+ ", " + destinationPosition.getZ() + ") in " + time + " seconds.");

 		return path;
 	}

 	@Override
 	public WayPointPath process(CartesianCoordinatesSet input) throws Exception {

 		// Need at least two points to plan a path.
 		if (input.getPoints().size() < 2) {
 			Logger.debug("Needs to specify at least two points to plan a path ! Specified <" + input.getPoints().size()
 					+ "> points !");
 			throw new Exception("Needs to specify at least two points to plan a path ! Specified <"
 					+ input.getPoints().size() + "> points !");
 		}

 		WayPointPath path = ApogyAddonsGeometryPathsFactory.eINSTANCE.createWayPointPath();

 		try {
 			// Plan a path segment between each of the points specified as inputs.
 			getProgressMonitor().beginTask("Planning path.", input.getPoints().size() - 1);
 			for (int i = 1; i < input.getPoints().size(); i++) {
 				Logger.debug("Planning between point[" + (i - 1) + "] and point [" + i + "]...");

 				ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 						ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_POSITION,
 						input.getPoints().get(i - 1));
 				// setCurrentPosition(input.getPoints().get(i-1));

 				ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 						ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_DESTINATION,
 						input.getPoints().get(i));
 				// setCurrentDestination(input.getPoints().get(i));

 				WayPointPath pathSegment = plan(getCurrentPosition(), getCurrentDestination());

 				// Appends the current path segment to the overall path.
 				path = PathUtilities.append(path, pathSegment, true);

 				// Updates next start location to current destination.
 				ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
 						ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_POSITION,
 						getCurrentDestination());
 				// setCurrentPosition(getCurrentDestination());

 				getProgressMonitor().worked(1);

 				Logger.debug("Planning between point[" + (i - 1) + "] and point [" + i + "] done.");
 			}

 			Logger.debug("Planning completed.");
 		} finally {
 			getProgressMonitor().done();
 		}

 		return path;
 	}

 	/**
 	 * Create a SimpleDirectedWeightedGraph that represents the connectivity of a
 	 * CartesianCoordinatesMesh. This method can be overiden to creates the graph
 	 * differently.
 	 *
 	 * @return The SimpleDirectedWeightedGraph representing the polygon
 	 *         connectivity.
 	 */
 	@SuppressWarnings("rawtypes")
 	protected SimpleDirectedWeightedGraph createGraph() throws Exception {
 		Logger.debug("Creating Graph...");
 		if (getMesh() == null) {
 			Logger.debug("No mesh has been defined defined !");
 			throw new Exception("No mesh has been defined defined !");
 		} else if (getCostFunctions().size() == 0) {
 			Logger.debug("No cost function has been defined !");
 			throw new Exception("No cost function has been defined !");
 		} else {
 			Logger.debug("Creating Graph using <" + getCostFunctions() + "> cost function(s)...");

 			MobilityEdgeFactory edgeFactory = ApogyAddonsMobilityPathplannersGraphFactory.eINSTANCE
 					.createMobilityEdgeFactory();
 			edgeFactory.getCostFunctions().addAll(getCostFunctions());
 			Date start = new Date();
 			SimpleDirectedWeightedGraph tempGraph = GraphUtilities.createGraph(getMesh(), edgeFactory, null);
 			Date end = new Date();

 			double time = (end.getTime() - start.getTime()) / 1000.0;
 			Logger.debug("Created Graph with " + tempGraph.vertexSet().size() + " vertices and "
 					+ tempGraph.edgeSet().size() + " edges in " + time + " seconds.");
 			return tempGraph;
 		}
 	}

 	/**
 	 * Find the polygon on the mesh that is closest to the coordinates of the start
 	 * point.
 	 *
 	 * @param startPositionCoordinates Coordinates of the start point.
 	 * @return The CartesianPolygon to start from, null if none is found.
 	 */
 	protected CartesianPolygon getStartPolygon(CartesianPositionCoordinates startPositionCoordinates) {
 		Logger.debug("getStartPolygon()...");
 		CartesianPolygon start = null;

 		SortedSet<CartesianPolygon> sortedPolygons = Geometry3DUtilities
 				.sortCartesianPolygonByDistance(startPositionCoordinates, this.getMesh().getPolygons());

 		if (sortedPolygons.size() > 0) {
 			start = sortedPolygons.first();
 		}
 		Logger.debug("getStartPolygon(). Done.");

 		return start;
 	}

 	/**
 	 * Find the polygon on the mesh that is closest to the coordinates of the
 	 * destination point.
 	 *
 	 * @param endPositionCoordinates Coordinates of the end point.
 	 * @return The CartesianPolygon to go to, null if none is found.
 	 */
 	protected CartesianPolygon getEndPolygon(CartesianPositionCoordinates endPositionCoordinates) {
 		Logger.debug("getEndPolygon()...");

 		CartesianPolygon end = null;
 		SortedSet<CartesianPolygon> sortedPolygons = Geometry3DUtilities
 				.sortCartesianPolygonByDistance(endPositionCoordinates, this.getMesh().getPolygons());

 		if (sortedPolygons.size() > 0) {
 			end = sortedPolygons.first();
 		}
 		Logger.debug("getEndPolygon(). Done.");

 		return end;
 	}

 	/**
 	 * Gets the list of polygons that represents the safe path on the mesh.
 	 *
 	 * @return The list of safe polygons to travel on.
 	 */
 	protected List<CartesianPolygon> getPolygonPath(CartesianPositionCoordinates currentPosition,
 			CartesianPositionCoordinates destinationPosition) throws Exception {
 		List<CartesianPolygon> polygonPath = new ArrayList<CartesianPolygon>();

 		// Gets the start and end polygon.
 		CartesianPolygon startPolygon = getStartPolygon(currentPosition);
 		CartesianPolygon endPolygon = getEndPolygon(destinationPosition);

 		if ((startPolygon != null) || (endPolygon != null)) {
 			Logger.debug("Calling DijkstraShortestPath...");
 			List<MobilityEdge> edges = GraphUtilities.getDijkstraShortestPath(this.getSimpleDirectedWeightedGraph(),
 					startPolygon, endPolygon);
 			double pathCost = GraphUtilities.getPathCost(edges);
 			Logger.debug("Calling DijkstraShortestPath done.");

 			if (Double.isInfinite(pathCost)) {
 				Logger.debug("Infinite cost Path.");
 				throw (new Exception("Infinite cost Path."));
 			} else {
 				polygonPath = GraphUtilities.getPolygonPath(edges);
 			}
 		} else if (startPolygon == null) {
 			Logger.debug("Could not find a polygon for the start position !");
 			throw (new Exception("Could not find a polygon for the start position !"));
 		}
 		return polygonPath;
 	}

 	/**
 	 * Generates the actual WayPointPath using the list of polygons that represents
 	 * the safe path on the mesh.
 	 *
 	 * @param polygonPath The list of safe polygons to travel on.
 	 * @return The WayPointPath generated.
 	 */
 	protected WayPointPath generatePath(CartesianPositionCoordinates currentPosition,
 			CartesianPositionCoordinates destinationPosition, List<CartesianPolygon> polygonPath) {
 		WayPointPath path = null;

 		if (isEnablePathSimplification()) {
 			path = GraphUtilities.getSimplifiedPathThroughPolygonsCentroid(CartesianPlane.XY, currentPosition,
 					destinationPosition, polygonPath, getRobotWidthForPathSimplication());
 		} else {
 			path = GraphUtilities.getPathThroughPolygonsCentroid(polygonPath);

 			// Adds the start and end points.
 			path.getPoints().add(0,
 					ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(currentPosition));
 			path.getPoints().add(
 					ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(destinationPosition));
 		}

 		return path;
 	}

 	/**
 	 * Saves the polygon path return from the search algorithm to a topology file
 	 * for debugging.
 	 *
 	 * @param polygonPath THe polygon path.
 	 * @param path        The path of the folder where to save the data.
 	 * @param fileName    The file name to save the data to.
 	 */
 	@SuppressWarnings({ "unused", "rawtypes" })
 	private void savePolygonPath(List<CartesianTriangle> polygonPath, String path, String fileName) {
 		TransformNode root = ApogyCommonTopologyFacade.INSTANCE.createTransformNodeXYZ(0, 0, 0, 0, 0, 0);
 		Mesh pathMesh = ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianTriangularMesh(polygonPath);

 		ContentNode<Mesh> pathNode = ApogyCommonTopologyFacade.INSTANCE.createContentNode(pathMesh);
 		pathNode.setDescription("Polygon Path.");
 		root.getChildren().add(pathNode);

 		// Saves the topology.
 		ResourceSet resourceSet = new ResourceSetImpl();
 		Resource resource = resourceSet.createResource(URI.createFileURI(path + File.separator + fileName + ""));
 		resource.getContents().add(pathMesh);
 		try {
 			resource.save(Collections.EMPTY_MAP);
 		} catch (IOException e) {
 			Logger.error(e.getMessage(), e);
 		}
 	}

 	@SuppressWarnings("unchecked")
 	@Override
 	public void setMesh(@SuppressWarnings("rawtypes") Mesh newMesh) {
 		this.graphIsDirty = true;
 		super.setMesh(newMesh);
 	}

 	/**
 	 * Returns an Adapter that listens for change on the list of cost functions and
 	 * UNSET the graph to for it to update.
 	 *
 	 * @return The adapter.
 	 */
 	private Adapter getCostFunctionsAdapter() {
 		if (this.costFunctionsAdapter == null) {
 			this.costFunctionsAdapter = new AdapterImpl() {
 				@Override
 				public void notifyChanged(Notification msg) {
 					if (msg.getFeatureID(
 							CostBasedMeshWayPointPathPlanner.class) == ApogyAddonsMobilityPathplannersGraphPackage.COST_BASED_MESH_WAY_POINT_PATH_PLANNER__COST_FUNCTIONS) {
 						ApogyCommonTransactionFacade.INSTANCE.basicSet(
 								SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerCustomImpl.this,
 								ApogyAddonsMobilityPathplannersGraphPackage.Literals.SIMPLE_DIRECTED_WEIGHTED_GRAPH_BASED_MESH_WAY_POINT_PATH_PLANNER__SIMPLE_DIRECTED_WEIGHTED_GRAPH,
 								null);
 						eUnset(ApogyAddonsMobilityPathplannersGraphPackage.SIMPLE_DIRECTED_WEIGHTED_GRAPH_BASED_MESH_WAY_POINT_PATH_PLANNER__SIMPLE_DIRECTED_WEIGHTED_GRAPH);
 					}
 				}
 			};
 		}
 		return this.costFunctionsAdapter;
 	}
 } // SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerImpl
	/*******************************************************************************
	* 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,
	* Sebastien Gemme - initial API and implementation
	*
	* SPDX-License-Identifier: EPL-1.0
	*
	*******************************************************************************/
	package org.eclipse.apogy.addons.mobility.pathplanners.graph.impl;

	import java.io.File;
	import java.io.IOException;
	import java.util.ArrayList;
	import java.util.Collections;
	import java.util.Date;
	import java.util.List;
	import java.util.SortedSet;

	import org.eclipse.apogy.addons.geometry.paths.ApogyAddonsGeometryPathsFactory;
	import org.eclipse.apogy.addons.geometry.paths.PathUtilities;
	import org.eclipse.apogy.addons.geometry.paths.WayPointPath;
	import org.eclipse.apogy.addons.mobility.pathplanners.ApogyAddonsMobilityPathplannersPackage;
	import org.eclipse.apogy.addons.mobility.pathplanners.graph.ApogyAddonsMobilityPathplannersGraphFactory;
	import org.eclipse.apogy.addons.mobility.pathplanners.graph.ApogyAddonsMobilityPathplannersGraphPackage;
	import org.eclipse.apogy.addons.mobility.pathplanners.graph.CostBasedMeshWayPointPathPlanner;
	import org.eclipse.apogy.addons.mobility.pathplanners.graph.GraphUtilities;
	import org.eclipse.apogy.addons.mobility.pathplanners.graph.MobilityEdge;
	import org.eclipse.apogy.addons.mobility.pathplanners.graph.MobilityEdgeFactory;
	import org.eclipse.apogy.common.emf.transaction.ApogyCommonTransactionFacade;
	import org.eclipse.apogy.common.geometry.data.Mesh;
	import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFacade;
	import org.eclipse.apogy.common.geometry.data3d.CartesianCoordinatesSet;
	import org.eclipse.apogy.common.geometry.data3d.CartesianPlane;
	import org.eclipse.apogy.common.geometry.data3d.CartesianPolygon;
	import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
	import org.eclipse.apogy.common.geometry.data3d.CartesianTriangle;
	import org.eclipse.apogy.common.geometry.data3d.Geometry3DUtilities;
	import org.eclipse.apogy.common.topology.ApogyCommonTopologyFacade;
	import org.eclipse.apogy.common.topology.ContentNode;
	import org.eclipse.apogy.common.topology.TransformNode;
	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.common.util.URI;
	import org.eclipse.emf.ecore.resource.Resource;
	import org.eclipse.emf.ecore.resource.ResourceSet;
	import org.eclipse.emf.ecore.resource.impl.ResourceSetImpl;
	import org.jgrapht.graph.SimpleDirectedWeightedGraph;
	import org.slf4j.Logger;
	import org.slf4j.LoggerFactory;

	public class SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerCustomImpl<PolygonType extends CartesianPolygon>
	extends SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerImpl<PolygonType> {

	private static final Logger Logger = LoggerFactory
	.getLogger(SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerImpl.class);
	private Adapter costFunctionsAdapter = null;
	private boolean graphIsDirty = true;

	protected SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerCustomImpl() {
	super();

	// Register a listener to the CostFunction to ensure the
	// SimpleDirectedWeightedGraph is updated.
	this.eAdapters().add(getCostFunctionsAdapter());
	}

	@Override
	@SuppressWarnings({ "rawtypes", "unchecked" })
	public SimpleDirectedWeightedGraph getSimpleDirectedWeightedGraph() {
	if (this.graphIsDirty) {
	Logger.debug("Graph is dirty, regenerating it...");
	try {
	SimpleDirectedWeightedGraph graph = createGraph();
	this.graphIsDirty = false;
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsMobilityPathplannersGraphPackage.Literals.SIMPLE_DIRECTED_WEIGHTED_GRAPH_BASED_MESH_WAY_POINT_PATH_PLANNER__SIMPLE_DIRECTED_WEIGHTED_GRAPH,
	graph);
	} catch (Exception e) {
	Logger.error(e.getMessage(), e);
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsMobilityPathplannersGraphPackage.Literals.SIMPLE_DIRECTED_WEIGHTED_GRAPH_BASED_MESH_WAY_POINT_PATH_PLANNER__SIMPLE_DIRECTED_WEIGHTED_GRAPH,
	null);
	}
	}
	return super.getSimpleDirectedWeightedGraph();
	}

	@Override
	public WayPointPath plan(CartesianPositionCoordinates currentPosition,
	CartesianPositionCoordinates destinationPosition) throws Exception {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_POSITION,
	currentPosition);
	// setCurrentPosition(currentPosition);

	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_DESTINATION,
	destinationPosition);
	// setCurrentDestination(destinationPosition);

	Date start = new Date();
	WayPointPath path = ApogyAddonsGeometryPathsFactory.eINSTANCE.createWayPointPath();

	// Gets the polygon path.
	List<CartesianPolygon> polygonPath = null;
	polygonPath = getPolygonPath(currentPosition, destinationPosition);

	// Generates the WayPointPath that goes through the polygon path.
	path = generatePath(currentPosition, destinationPosition, polygonPath);
	Date end = new Date();

	double time = (end.getTime() - start.getTime()) / 1000.0;
	Logger.debug("Planned path between (" + currentPosition.getX() + ", " + currentPosition.getY() + ", "
	+ currentPosition.getZ() + ") and (" + destinationPosition.getX() + ", " + destinationPosition.getY()
	+ ", " + destinationPosition.getZ() + ") in " + time + " seconds.");

	return path;
	}

	@Override
	public WayPointPath process(CartesianCoordinatesSet input) throws Exception {

	// Need at least two points to plan a path.
	if (input.getPoints().size() < 2) {
	Logger.debug("Needs to specify at least two points to plan a path ! Specified <" + input.getPoints().size()
	+ "> points !");
	throw new Exception("Needs to specify at least two points to plan a path ! Specified <"
	+ input.getPoints().size() + "> points !");
	}

	WayPointPath path = ApogyAddonsGeometryPathsFactory.eINSTANCE.createWayPointPath();

	try {
	// Plan a path segment between each of the points specified as inputs.
	getProgressMonitor().beginTask("Planning path.", input.getPoints().size() - 1);
	for (int i = 1; i < input.getPoints().size(); i++) {
	Logger.debug("Planning between point[" + (i - 1) + "] and point [" + i + "]...");

	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_POSITION,
	input.getPoints().get(i - 1));
	// setCurrentPosition(input.getPoints().get(i-1));

	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_DESTINATION,
	input.getPoints().get(i));
	// setCurrentDestination(input.getPoints().get(i));

	WayPointPath pathSegment = plan(getCurrentPosition(), getCurrentDestination());

	// Appends the current path segment to the overall path.
	path = PathUtilities.append(path, pathSegment, true);

	// Updates next start location to current destination.
	ApogyCommonTransactionFacade.INSTANCE.basicSet(this,
	ApogyAddonsMobilityPathplannersPackage.Literals.WAY_POINT_PATH_PLANNER__CURRENT_POSITION,
	getCurrentDestination());
	// setCurrentPosition(getCurrentDestination());

	getProgressMonitor().worked(1);

	Logger.debug("Planning between point[" + (i - 1) + "] and point [" + i + "] done.");
	}

	Logger.debug("Planning completed.");
	} finally {
	getProgressMonitor().done();
	}

	return path;
	}

	/**
	* Create a SimpleDirectedWeightedGraph that represents the connectivity of a
	* CartesianCoordinatesMesh. This method can be overiden to creates the graph
	* differently.
	*
	* @return The SimpleDirectedWeightedGraph representing the polygon
	* connectivity.
	*/
	@SuppressWarnings("rawtypes")
	protected SimpleDirectedWeightedGraph createGraph() throws Exception {
	Logger.debug("Creating Graph...");
	if (getMesh() == null) {
	Logger.debug("No mesh has been defined defined !");
	throw new Exception("No mesh has been defined defined !");
	} else if (getCostFunctions().size() == 0) {
	Logger.debug("No cost function has been defined !");
	throw new Exception("No cost function has been defined !");
	} else {
	Logger.debug("Creating Graph using <" + getCostFunctions() + "> cost function(s)...");

	MobilityEdgeFactory edgeFactory = ApogyAddonsMobilityPathplannersGraphFactory.eINSTANCE
	.createMobilityEdgeFactory();
	edgeFactory.getCostFunctions().addAll(getCostFunctions());
	Date start = new Date();
	SimpleDirectedWeightedGraph tempGraph = GraphUtilities.createGraph(getMesh(), edgeFactory, null);
	Date end = new Date();

	double time = (end.getTime() - start.getTime()) / 1000.0;
	Logger.debug("Created Graph with " + tempGraph.vertexSet().size() + " vertices and "
	+ tempGraph.edgeSet().size() + " edges in " + time + " seconds.");
	return tempGraph;
	}
	}

	/**
	* Find the polygon on the mesh that is closest to the coordinates of the start
	* point.
	*
	* @param startPositionCoordinates Coordinates of the start point.
	* @return The CartesianPolygon to start from, null if none is found.
	*/
	protected CartesianPolygon getStartPolygon(CartesianPositionCoordinates startPositionCoordinates) {
	Logger.debug("getStartPolygon()...");
	CartesianPolygon start = null;

	SortedSet<CartesianPolygon> sortedPolygons = Geometry3DUtilities
	.sortCartesianPolygonByDistance(startPositionCoordinates, this.getMesh().getPolygons());

	if (sortedPolygons.size() > 0) {
	start = sortedPolygons.first();
	}
	Logger.debug("getStartPolygon(). Done.");

	return start;
	}

	/**
	* Find the polygon on the mesh that is closest to the coordinates of the
	* destination point.
	*
	* @param endPositionCoordinates Coordinates of the end point.
	* @return The CartesianPolygon to go to, null if none is found.
	*/
	protected CartesianPolygon getEndPolygon(CartesianPositionCoordinates endPositionCoordinates) {
	Logger.debug("getEndPolygon()...");

	CartesianPolygon end = null;
	SortedSet<CartesianPolygon> sortedPolygons = Geometry3DUtilities
	.sortCartesianPolygonByDistance(endPositionCoordinates, this.getMesh().getPolygons());

	if (sortedPolygons.size() > 0) {
	end = sortedPolygons.first();
	}
	Logger.debug("getEndPolygon(). Done.");

	return end;
	}

	/**
	* Gets the list of polygons that represents the safe path on the mesh.
	*
	* @return The list of safe polygons to travel on.
	*/
	protected List<CartesianPolygon> getPolygonPath(CartesianPositionCoordinates currentPosition,
	CartesianPositionCoordinates destinationPosition) throws Exception {
	List<CartesianPolygon> polygonPath = new ArrayList<CartesianPolygon>();

	// Gets the start and end polygon.
	CartesianPolygon startPolygon = getStartPolygon(currentPosition);
	CartesianPolygon endPolygon = getEndPolygon(destinationPosition);

	if ((startPolygon != null) \|\| (endPolygon != null)) {
	Logger.debug("Calling DijkstraShortestPath...");
	List<MobilityEdge> edges = GraphUtilities.getDijkstraShortestPath(this.getSimpleDirectedWeightedGraph(),
	startPolygon, endPolygon);
	double pathCost = GraphUtilities.getPathCost(edges);
	Logger.debug("Calling DijkstraShortestPath done.");

	if (Double.isInfinite(pathCost)) {
	Logger.debug("Infinite cost Path.");
	throw (new Exception("Infinite cost Path."));
	} else {
	polygonPath = GraphUtilities.getPolygonPath(edges);
	}
	} else if (startPolygon == null) {
	Logger.debug("Could not find a polygon for the start position !");
	throw (new Exception("Could not find a polygon for the start position !"));
	}
	return polygonPath;
	}

	/**
	* Generates the actual WayPointPath using the list of polygons that represents
	* the safe path on the mesh.
	*
	* @param polygonPath The list of safe polygons to travel on.
	* @return The WayPointPath generated.
	*/
	protected WayPointPath generatePath(CartesianPositionCoordinates currentPosition,
	CartesianPositionCoordinates destinationPosition, List<CartesianPolygon> polygonPath) {
	WayPointPath path = null;

	if (isEnablePathSimplification()) {
	path = GraphUtilities.getSimplifiedPathThroughPolygonsCentroid(CartesianPlane.XY, currentPosition,
	destinationPosition, polygonPath, getRobotWidthForPathSimplication());
	} else {
	path = GraphUtilities.getPathThroughPolygonsCentroid(polygonPath);

	// Adds the start and end points.
	path.getPoints().add(0,
	ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(currentPosition));
	path.getPoints().add(
	ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(destinationPosition));
	}

	return path;
	}

	/**
	* Saves the polygon path return from the search algorithm to a topology file
	* for debugging.
	*
	* @param polygonPath THe polygon path.
	* @param path The path of the folder where to save the data.
	* @param fileName The file name to save the data to.
	*/
	@SuppressWarnings({ "unused", "rawtypes" })
	private void savePolygonPath(List<CartesianTriangle> polygonPath, String path, String fileName) {
	TransformNode root = ApogyCommonTopologyFacade.INSTANCE.createTransformNodeXYZ(0, 0, 0, 0, 0, 0);
	Mesh pathMesh = ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianTriangularMesh(polygonPath);

	ContentNode<Mesh> pathNode = ApogyCommonTopologyFacade.INSTANCE.createContentNode(pathMesh);
	pathNode.setDescription("Polygon Path.");
	root.getChildren().add(pathNode);

	// Saves the topology.
	ResourceSet resourceSet = new ResourceSetImpl();
	Resource resource = resourceSet.createResource(URI.createFileURI(path + File.separator + fileName + ""));
	resource.getContents().add(pathMesh);
	try {
	resource.save(Collections.EMPTY_MAP);
	} catch (IOException e) {
	Logger.error(e.getMessage(), e);
	}
	}

	@SuppressWarnings("unchecked")
	@Override
	public void setMesh(@SuppressWarnings("rawtypes") Mesh newMesh) {
	this.graphIsDirty = true;
	super.setMesh(newMesh);
	}

	/**
	* Returns an Adapter that listens for change on the list of cost functions and
	* UNSET the graph to for it to update.
	*
	* @return The adapter.
	*/
	private Adapter getCostFunctionsAdapter() {
	if (this.costFunctionsAdapter == null) {
	this.costFunctionsAdapter = new AdapterImpl() {
	@Override
	public void notifyChanged(Notification msg) {
	if (msg.getFeatureID(
	CostBasedMeshWayPointPathPlanner.class) == ApogyAddonsMobilityPathplannersGraphPackage.COST_BASED_MESH_WAY_POINT_PATH_PLANNER__COST_FUNCTIONS) {
	ApogyCommonTransactionFacade.INSTANCE.basicSet(
	SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerCustomImpl.this,
	ApogyAddonsMobilityPathplannersGraphPackage.Literals.SIMPLE_DIRECTED_WEIGHTED_GRAPH_BASED_MESH_WAY_POINT_PATH_PLANNER__SIMPLE_DIRECTED_WEIGHTED_GRAPH,
	null);
	eUnset(ApogyAddonsMobilityPathplannersGraphPackage.SIMPLE_DIRECTED_WEIGHTED_GRAPH_BASED_MESH_WAY_POINT_PATH_PLANNER__SIMPLE_DIRECTED_WEIGHTED_GRAPH);
	}
	}
	};
	}
	return this.costFunctionsAdapter;
	}
	} // SimpleDirectedWeightedGraphBasedMeshWayPointPathPlannerImpl