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

import java.util.ArrayList;
import java.util.List;

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

import org.eclipse.apogy.addons.sensors.fov.ApogyAddonsSensorsFOVFactory;
import org.eclipse.apogy.addons.sensors.fov.RectangularFrustrumFieldOfViewSamplingShape;
import org.eclipse.apogy.addons.sensors.range.ApogyAddonsSensorsRangeFacade;
import org.eclipse.apogy.addons.sensors.range.ApogyAddonsSensorsRangeFactory;
import org.eclipse.apogy.addons.sensors.range.RasterScanSettings;
import org.eclipse.apogy.addons.sensors.range.RayData;
import org.eclipse.apogy.common.geometry.data25d.ApogyCommonGeometryData25DFacade;
import org.eclipse.apogy.common.geometry.data25d.Coordinates25D;
import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFactory;
import org.eclipse.apogy.common.geometry.data3d.CartesianTriangle;
import org.eclipse.apogy.common.geometry.data3d.CartesianTriangularMesh;
import org.eclipse.apogy.common.geometry.data3d.CartesianTriangularMeshPolygonSampler;
import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
import org.eclipse.apogy.common.topology.ApogyCommonTopologyFacade;
import org.eclipse.apogy.common.topology.GroupNode;
import org.eclipse.apogy.common.topology.Node;
import org.eclipse.apogy.common.topology.TransformNode;
import org.eclipse.emf.common.util.EList;
import org.eclipse.emf.ecore.util.EcoreUtil;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

public class SimpleRasterScanRangeScannerSimulatorCustomImpl extends SimpleRasterScanRangeScannerSimulatorImpl {

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

	@Override
	public synchronized RasterScanSettings getScanSettings() {
		RasterScanSettings rasterScanSettings = getInput();

		if (rasterScanSettings == null) {
			// Search the list of children of the RasterScanRangeSensor to find a
			// RasterScanSettings.
			EList<Node> children = getChildren();
			for (Node node : children) {
				if (node instanceof RasterScanSettings) {
					rasterScanSettings = (RasterScanSettings) node;
				} else if (node instanceof GroupNode) {
					GroupNode groupNode = (GroupNode) node;
					EList<Node> groupChildren = groupNode.getChildren();
					for (Node n : groupChildren) {
						if (n instanceof RasterScanSettings) {
							rasterScanSettings = (RasterScanSettings) n;
						}
					}
				}

			}

			// If no RasterScanSettings is found, creates one.
			if (rasterScanSettings == null) {
				// Creates a transform node to position and orient the fov.
				TransformNode transformNode = ApogyCommonTopologyFacade.INSTANCE.createTransformNodeXYZ(0, 0, 0, 0, 0,
						0);
				transformNode.setDescription("Field Of View Transform");
				getChildren().add(transformNode);

				// Adds the rasterscan to the transform.
				rasterScanSettings = ApogyAddonsSensorsRangeFacade.INSTANCE.createRasterScanSettings(0, 100.0,
						Math.toRadians(90), Math.toRadians(45), 100, 100);
				transformNode.getChildren().add(rasterScanSettings);
			}
		}

		return rasterScanSettings;
	}

	@Override
	public void setInput(RasterScanSettings newInput) {
		RasterScanSettings currentScanSettings = getScanSettings();
		currentScanSettings.setDescription(newInput.getDescription());
		currentScanSettings.setHorizontalFieldOfViewAngle(newInput.getHorizontalFieldOfViewAngle());
		currentScanSettings.setHorizontalResolution(newInput.getHorizontalResolution());
		currentScanSettings.setRange(EcoreUtil.copy(newInput.getRange()));
		currentScanSettings.setVerticalFieldOfViewAngle(newInput.getVerticalFieldOfViewAngle());
		currentScanSettings.setVerticalResolution(newInput.getVerticalResolution());
		super.setInput(newInput);
	}

	@Override
	public CartesianTriangularMesh getCroppedMesh() {
		CartesianTriangularMesh croppedMesh = null;

		// Gets the position on the lidar with respect to the meshNode.
		Matrix4d laserToMesh = ApogyCommonTopologyFacade.INSTANCE.expressInFrame(this, getMeshNode());

		// Sample the terrain to retain only polygon that are partially or fully within
		// the maximum range.
		CartesianTriangularMeshPolygonSampler meshSampler = ApogyCommonGeometryData3DFactory.eINSTANCE
				.createCartesianTriangularMeshPolygonSampler();

		// Setup the sampling shape.
		RectangularFrustrumFieldOfViewSamplingShape<CartesianTriangle> samplingShape = ApogyAddonsSensorsFOVFactory.eINSTANCE
				.createRectangularFrustrumFieldOfViewSamplingShape();
		samplingShape.setRectangularFrustrumFieldOfView(getScanSettings());
		samplingShape.setTransform(ApogyCommonMathFacade.INSTANCE.createMatrix4x4(laserToMesh));

		meshSampler.getPolygonSamplingShapes().add(samplingShape);

		try {
			croppedMesh = (CartesianTriangularMesh) meshSampler.process(this.meshNode.getContent());
		} catch (Exception e) {
			Logger.error(e.getMessage(), e);
		}

		return croppedMesh;
	}

	@Override
	public List<RayData> getSimulatedRays() {
		List<RayData> rays = new ArrayList<RayData>();

		double startAzimuth = -(getScanSettings().getHorizontalFieldOfViewAngle() / 2.0);
		double startElevation = -(getScanSettings().getVerticalFieldOfViewAngle() / 2.0);
		double azimuth = startAzimuth;
		double azimuthIncrement = getScanSettings().getHorizontalFieldOfViewAngle()
				/ (getScanSettings().getHorizontalResolution() - 1);
		double elevation = startElevation;
		double elevetationIncrement = getScanSettings().getVerticalFieldOfViewAngle()
				/ (getScanSettings().getVerticalResolution() - 1);

		int horizontalResolution = getScanSettings().getHorizontalResolution();
		int verticalResolution = getScanSettings().getVerticalResolution();

		for (int azimuthIndex = 0; azimuthIndex < horizontalResolution; azimuthIndex++) {
			elevation = startElevation;
			for (int elevationIndex = 0; elevationIndex < verticalResolution; elevationIndex++) {
				RayData rayData = ApogyAddonsSensorsRangeFactory.eINSTANCE.createRayData();
				rayData.setOrigin(new Point3d());

				Vector3d v = new Vector3d(-Math.sin(elevation), Math.cos(elevation) * Math.sin(azimuth),
						Math.cos(elevation) * Math.cos(azimuth));
				v.normalize();
				rayData.setDirection(v);

				rays.add(rayData);
				elevation += elevetationIncrement;
			}

			azimuth += azimuthIncrement;
		}

		return rays;
	}

	@Override
	public double applyRangeNoise(double range, RayData cleanRayData, RayData noisyRayData) {
		if (isNoiseEnabled()) {
			double noisyRange = range + (-0.5 + Math.random()) * getRangeNoiseAmplitude();
			return noisyRange;
		} else {
			return range;
		}
	}

	@Override
	public Coordinates25D createCoordinates25D(RayData rayData, double range) {
		if ((range != Double.NaN) && (getScanSettings().getRange().isWithinRange(range))) {
			Vector3d v = rayData.getDirection();
			v.normalize();
			v.scale(range);
			v.add(rayData.getOrigin());

			return ApogyCommonGeometryData25DFacade.INSTANCE.createCoordinates25D(v.x, v.y, v.z);
		} else {
			return null;
		}
	}

	@Override
	public RayData applyOrientationNoise(RayData rayData) {
		// TODO : Implements noise !
		RayData noisyOrientation = EcoreUtil.copy(rayData);
		return noisyOrientation;
	}

} // SimpleRasterScanRangeScannerSimulatorImpl