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

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

import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFacade;
import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFactory;
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;

public class CircularExclusionZoneCustomImpl extends CircularExclusionZoneImpl {

	private static CartesianPositionCoordinates center = ApogyCommonGeometryData3DFactory.eINSTANCE
			.createCartesianPositionCoordinates();

	@Override
	public boolean isPolygonInside(CartesianTriangle polygon) {
		return isPolygonInside((CartesianPolygon) polygon);
	}

	public boolean isPolygonInside(CartesianPolygon polygon) {
		boolean inside = false;

		// First checks if the center of the exclusion zones falls onto the polygon.
		Matrix4d centerTransform = ApogyCommonTopologyFacade.INSTANCE.expressNodeInRootFrame(this);
		Vector3d centerPosition = new Vector3d();
		centerTransform.get(centerPosition);

		CartesianPositionCoordinates point = ApogyCommonGeometryData3DFacade.INSTANCE
				.createCartesianPositionCoordinates(centerPosition.x, centerPosition.y, centerPosition.z);
		inside = Geometry3DUtilities.isInsidePolygon(point, polygon);

		if (!inside) {
			// Check each of the edge to see if it falls within the cylinder.
			int index = 0;
			CartesianPositionCoordinates from = null;
			CartesianPositionCoordinates to = null;
			while (index < polygon.getVertices().size() && !inside) {
				from = polygon.getVertices().get(index);

				if ((index + 1) >= polygon.getVertices().size()) {
					to = polygon.getVertices().get(0);
				} else {
					to = polygon.getVertices().get(index + 1);
				}

				inside = intersects(from, to);
				index++;
			}
		}

		if (isInvertSamplingShape()) {
			return !inside;
		} else {
			return inside;
		}
	}

	/**
	 * Returns the centre of the circular exclusion zone relative to the tolopology
	 * root node.
	 * 
	 * @return The centre of the exclusion zone.
	 */
	private CartesianPositionCoordinates projectOntoExclusionZone(CartesianPositionCoordinates point) {
		Matrix4d matrix = ApogyCommonTopologyFacade.INSTANCE.expressRootInNodeFrame(this);
		Point3d projected = new Point3d();
		matrix.transform(point.asPoint3d(), projected);
		CartesianPositionCoordinates projectedPoint = ApogyCommonGeometryData3DFacade.INSTANCE
				.createCartesianPositionCoordinates(projected.x, projected.y, projected.z);

		return Geometry3DUtilities.getFlattenCoordinate(CartesianPlane.XY, projectedPoint);
	}

	@Override
	public boolean isInside(CartesianPositionCoordinates point) {

		// Gets the point's flattened coordinates.
		CartesianPositionCoordinates projectedPoint = projectOntoExclusionZone(point);

		boolean inside = (Geometry3DUtilities.getDistance(projectedPoint, center) < getRadius());

		if (isInvertSamplingShape()) {
			return !inside;
		} else {
			return inside;
		}
	}

	@Override
	public boolean intersects(CartesianPositionCoordinates from, CartesianPositionCoordinates to) {

		// Check if from or to is inside the zone.
		if (isInside(from) || isInside(to)) {
			return true;
		} else {
			// Gets the centre's flattened coordinates.
			CartesianPositionCoordinates flattenedCenter = ApogyCommonGeometryData3DFactory.eINSTANCE
					.createCartesianPositionCoordinates();

			// Gets the point's flattened coordinates of the line.
			CartesianPositionCoordinates flattenedFrom = projectOntoExclusionZone(from);
			CartesianPositionCoordinates flattenedTo = projectOntoExclusionZone(to);

			// Gets the centre projection on the from->to line.
			CartesianPositionCoordinates intersect = Geometry3DUtilities.getProjection(flattenedCenter, flattenedFrom,
					flattenedTo);

			// Check if the distance between the projection and the centre is less than the
			// radius
			if (Geometry3DUtilities.getDistance(flattenedCenter, intersect) < getRadius()) {
				double u1u2Distance = Geometry3DUtilities.getDistance(flattenedFrom, flattenedTo);

				// Check if the projection falls between u1 and u2. This is to account for the
				// case
				// where the projection of the line passes through the circle.
				if ((Geometry3DUtilities.getDistance(intersect, flattenedFrom) < u1u2Distance)
						&& (Geometry3DUtilities.getDistance(intersect, flattenedTo) < u1u2Distance)) {
					return true;
				} else {
					return false;
				}
			} else {
				return false;
			}
		}
	}
} // CircularExclusionZoneImpl