bundles/addons/org.eclipse.apogy.addons.sensors.fov/src-gen-custom/org/eclipse/apogy/addons/sensors/fov/impl/RectangularFrustrumFieldOfViewSamplingShapeCustomImpl.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.sensors.fov.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.CartesianPolygon;
import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
import org.eclipse.apogy.common.geometry.data3d.Geometry3DUtilities;
import org.eclipse.emf.ecore.util.EcoreUtil;

public class RectangularFrustrumFieldOfViewSamplingShapeCustomImpl<PolygonType extends CartesianPolygon>
		extends RectangularFrustrumFieldOfViewSamplingShapeImpl<PolygonType> {

	private static Point3d origin = new Point3d();

	@Override
	public boolean isPolygonInside(PolygonType polygon) {
		// Checks if the centroide of the polygon falls within the FOV.
		if (isInside(polygon.getCentroid()))
			return true;

		// Checks that at least one vertex of the polygon falls inside.
		for (CartesianPositionCoordinates point : polygon.getVertices()) {
			if (isInside(point)) {
				return true;
			}
		}

		// Checks whether any of the FOV edges intersects with the polygon.
		Matrix4d matrix = new Matrix4d(getTransform().asMatrix4d());
		matrix.invert();

		// Creates a version of the polygon that is expressed in the FOV frame.
		PolygonType transformedPolygon = createPolygonInFOVFrame(polygon);

		Vector3d[][] edges = getFOVEdges();
		for (int i = 0; i < edges.length; i++) {
			Vector3d u = edges[i][0];
			Vector3d v = edges[i][1];

			if (Geometry3DUtilities.getLineAndPolygonIntersectionPoint(u, v, transformedPolygon) != null) {
				return true;
			}
		}

		return false;
	}

	private PolygonType createPolygonInFOVFrame(PolygonType polygon) {
		PolygonType newPolygon = EcoreUtil.copy(polygon);
		newPolygon.getVertices().clear();

		Matrix4d matrix = new Matrix4d(getTransform().asMatrix4d());
		matrix.invert();

		for (CartesianPositionCoordinates p : polygon.getVertices()) {
			Point3d point3d = new Point3d();
			matrix.transform(p.asPoint3d(), point3d);
			CartesianPositionCoordinates transformedP = ApogyCommonGeometryData3DFacade.INSTANCE
					.createCartesianPositionCoordinates(point3d.x, point3d.y, point3d.z);
			newPolygon.getVertices().add(transformedP);
		}

		return newPolygon;
	}

	private Vector3d[][] getFOVEdges() {
		Vector3d[][] edges = new Vector3d[4][2];

		double azimuth = getRectangularFrustrumFieldOfView().getHorizontalFieldOfViewAngle() / 2.0;
		double elevation = getRectangularFrustrumFieldOfView().getVerticalFieldOfViewAngle() / 2.0;

		double x = Math.sin(elevation);
		double y = Math.cos(elevation) * Math.sin(azimuth);
		double z = Math.cos(elevation) * Math.cos(azimuth);

		double xShort = x * getRectangularFrustrumFieldOfView().getRange().getMinimumDistance();
		double yShort = y * getRectangularFrustrumFieldOfView().getRange().getMinimumDistance();
		double zShort = z * getRectangularFrustrumFieldOfView().getRange().getMinimumDistance();

		double xLong = x * getRectangularFrustrumFieldOfView().getRange().getMaximumDistance();
		double yLong = y * getRectangularFrustrumFieldOfView().getRange().getMaximumDistance();
		double zLong = z * getRectangularFrustrumFieldOfView().getRange().getMaximumDistance();

		edges[0][0] = new Vector3d(xShort, yShort, zShort);
		edges[0][1] = new Vector3d(xLong, yLong, zLong);

		edges[1][0] = new Vector3d(-xShort, yShort, zShort);
		edges[1][1] = new Vector3d(-xLong, yLong, zLong);

		edges[2][0] = new Vector3d(-xShort, -yShort, zShort);
		edges[2][1] = new Vector3d(-xLong, -yLong, zLong);

		edges[3][0] = new Vector3d(xShort, -yShort, zShort);
		edges[3][1] = new Vector3d(xLong, -yLong, zLong);

		return edges;
	}

	@Override
	public boolean isInside(CartesianPositionCoordinates point) {
		boolean isInside = false;

		// Transform the point into the FOV coordinates system.
		Matrix4d matrix = new Matrix4d(getTransform().asMatrix4d());
		matrix.invert();

		Point3d p = new Point3d();
		matrix.transform(point.asPoint3d(), p);

		double distance = p.distance(origin);
		if (getRectangularFrustrumFieldOfView().getRange().isWithinRange(distance)) {
			// Point is within range, check whether it is within the horizontal field of
			// view.
			double azimuth = Math.abs(Math.atan2(p.y, p.z));
			if (azimuth <= (getRectangularFrustrumFieldOfView().getHorizontalFieldOfViewAngle() / 2.0)) {
				// Checks whether is is with the vertical field of view.
				double elevation = Math.abs(Math.atan2(p.x, p.z));
				if (elevation <= (getRectangularFrustrumFieldOfView().getVerticalFieldOfViewAngle() / 2.0)) {
					isInside = true;
				}
			}
		}
		return isInside;
	}

} // RectangularFrustrumFieldOfViewSamplingShapeImpl