bundles/addons/org.eclipse.apogy.addons.sensors.range/src-gen-custom/org/eclipse/apogy/addons/sensors/range/impl/RangeScannerSimulatorCustomImpl.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.Date;
import java.util.List;

import javax.media.j3d.GeometryArray;
import javax.media.j3d.PickRay;
import javax.media.j3d.PointArray;
import javax.media.j3d.SceneGraphPath;
import javax.media.j3d.Shape3D;
import javax.media.j3d.TriangleArray;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import org.eclipse.apogy.addons.sensors.SensorStatus;
import org.eclipse.apogy.addons.sensors.range.RayData;
import org.eclipse.apogy.common.geometry.data25d.ApogyCommonGeometryData25DFactory;
import org.eclipse.apogy.common.geometry.data25d.Coordinates25D;
import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
import org.eclipse.apogy.common.geometry.data3d.CartesianTriangle;
import org.eclipse.apogy.common.geometry.data3d.CartesianTriangularMesh;
import org.eclipse.apogy.common.topology.ApogyCommonTopologyFacade;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import com.sun.j3d.utils.geometry.GeometryInfo;
import com.sun.j3d.utils.geometry.NormalGenerator;

public abstract class RangeScannerSimulatorCustomImpl<InputType> extends RangeScannerSimulatorImpl<InputType> {
	private static final Logger Logger = LoggerFactory.getLogger(RangeScannerSimulatorImpl.class);

	@Override
	public abstract CartesianTriangularMesh getCroppedMesh();

	@Override
	public abstract List<RayData> getSimulatedRays();

	@Override
	public org.eclipse.apogy.common.geometry.data25d.VolumetricCoordinatesSet25D process(InputType input)
			throws Exception {
		// Initialize the output if required
		if (getOutput() == null) {
			setOutput(ApogyCommonGeometryData25DFactory.eINSTANCE.createVolumetricCoordinatesSet25D());
		}

		// If the sensor is ready, initiate acquisition.
		if (getStatus() == SensorStatus.READY) {
			// Updates the scan settings.
			setInput(input);

			// Clears the previous scan data.
			getOutput().getPoints().clear();

			try {
				// Acquire data.
				acquireData();
				return getOutput();
			} catch (Exception e) {
				Logger.error(e.getMessage(), e);
			}
		}

		return null;
	};

	@Override
	public void acquireData() throws Exception {
		if (getMeshNode() == null) {
			throw new Exception("The mesh node is not set !");
		}

		// Computes the transform between the scanner and the mesh.
		Matrix4d scannerToMeshTransform = ApogyCommonTopologyFacade.INSTANCE.expressInFrame(this, getMeshNode());

		// Gets the cropped terrain.
		Date start = new Date();
		CartesianTriangularMesh croppedMesh = getCroppedMesh();
		Date end = new Date();
		setTimeCroppingMesh(end.getTime() - start.getTime());

		Logger.info("Cropped mesh contains " + croppedMesh.getPolygons().size() + " polygons and was created in "
				+ (getTimeCroppingMesh() / 1000.0) + " seconds.");

		// Gets a Shape3D representing the cropped terrain.
		Shape3D meshShape3D = getShape3D(croppedMesh);

		if (meshShape3D != null) {
			// Gets the list of RayData.
			start = new Date();
			List<RayData> rays = getSimulatedRays();
			end = new Date();
			setTimeGettingSimulatedRays(end.getTime() - start.getTime());

			Logger.info("Simulated rays contain " + rays.size() + " entries and were generated in "
					+ (getTimeGettingSimulatedRays() / 1000.0) + " seconds.");

			// Gets the simulated returns.
			start = new Date();
			List<Coordinates25D> returns = getSimulatedReturns(meshShape3D, scannerToMeshTransform, rays);
			end = new Date();
			setTimeFindingIntersections(end.getTime() - start.getTime());

			// Adds the returns to the data.
			getOutput().getPoints().addAll(returns);
		} else {
			getOutput().getPoints().clear();
		}
	}

	@Override
	public abstract RayData applyOrientationNoise(RayData rayData);

	@Override
	public abstract double applyRangeNoise(double range, RayData cleanRayData, RayData noisyRayData);

	@Override
	public abstract Coordinates25D createCoordinates25D(RayData rayData, double range);

	/**
	 * Converts a mesh to a Shape3D.
	 * 
	 * @param mesh The mesh.
	 * @return The Shape3D.
	 */
	protected Shape3D getShape3D(final CartesianTriangularMesh mesh) {
		if (mesh.getPoints().size() > 0) {
			// The switch that allows to switch the visibility on and off.
			GeometryArray meshData = null;

			// No polygons, it's a point array.
			if (mesh.getPolygons().size() == 0) {
				meshData = new PointArray(mesh.getPoints().size(), GeometryArray.COORDINATES);

				int pointId = 0;
				for (CartesianPositionCoordinates point : mesh.getPoints()) {
					Point3d coordinates = point.asPoint3d();

					meshData.setCoordinate(pointId, coordinates);

					pointId++;
				}
			} else {
				// We build the triangles.
				meshData = new TriangleArray(mesh.getPolygons().size() * 3,
						GeometryArray.COORDINATES | GeometryArray.NORMALS);

				int triangleId = 0;

				for (CartesianTriangle triangle : mesh.getPolygons()) {
					for (int i = 0; i < 3; i++) {
						Point3d point = triangle.getVertices().get(i).asPoint3d();
						meshData.setCoordinate(triangleId, point);
						triangleId++;
					}
				}

				NormalGenerator normalGenerator = new NormalGenerator();
				GeometryInfo geometryInfo = new GeometryInfo(meshData);
				normalGenerator.generateNormals(geometryInfo);

				meshData = geometryInfo.getGeometryArray();

			}

			return new Shape3D(meshData);
		}
		return null;
	}

	protected List<Coordinates25D> getSimulatedReturns(final Shape3D meshShape3D, final Matrix4d scannerToMeshTransform,
			final List<RayData> rays) {
		List<Coordinates25D> returns = new ArrayList<Coordinates25D>();

		// Create the PickRay use to compute the distance.
		PickRay pickRay = new PickRay();

		// Creates an empty scene.
		SceneGraphPath sceneGraphPath = new SceneGraphPath();

		// Creates the array used to store distance.
		double[] distance = new double[1];

		// Goes through the list of RayData and computes the intersection.
		for (RayData rayData : rays) {
			RayData noisyRayData = null;

			// Applies orientation noise if applicable.
			if (isNoiseEnabled()) {
				// Updates the position and direction of the pick ray.
				noisyRayData = applyOrientationNoise(rayData);
				pickRay.set(noisyRayData.getOrigin(), noisyRayData.getDirection());

				// Applies the transform onto the pick ray.
				Point3d origin = new Point3d(noisyRayData.getOrigin());
				scannerToMeshTransform.transform(origin);

				Vector3d direction = new Vector3d(noisyRayData.getDirection());
				scannerToMeshTransform.transform(direction);

				// Updates the position and direction of the pick ray.
				pickRay.set(origin, direction);
			} else {
				// Applies the transform onto the pick ray.
				Point3d origin = new Point3d(rayData.getOrigin());
				scannerToMeshTransform.transform(origin);

				Vector3d direction = new Vector3d(rayData.getDirection());
				scannerToMeshTransform.transform(direction);

				// Updates the position and direction of the pick ray.
				pickRay.set(origin, direction);
			}

			// Finds the intersection.
			boolean intersects = meshShape3D.intersect(sceneGraphPath, pickRay, distance);
			if (!intersects) {
				distance[0] = Double.NaN;
			} else {
				// Applies range noise if applicable.
				if (isNoiseEnabled()) {
					distance[0] = applyRangeNoise(distance[0], rayData, noisyRayData);
				}
			}

			// Creates and adds the coordinates of the intersection to the returns.
			// The coordinates contains the clean ray data with the noisy range.
			Coordinates25D coords = createCoordinates25D(rayData, distance[0]);
			if (coords != null)
				returns.add(createCoordinates25D(rayData, distance[0]));
		}

		return returns;
	}
} // RangeScannerSimulatorImpl