bundles/addons/org.eclipse.apogy.addons.geometry.paths/src-gen-custom/org/eclipse/apogy/addons/geometry/paths/impl/UniformDistanceWayPointPathInterpolatorCustomImpl.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.geometry.paths.impl;

import javax.vecmath.Point3d;

import org.eclipse.apogy.addons.geometry.paths.ApogyAddonsGeometryPathsFactory;
import org.eclipse.apogy.addons.geometry.paths.SegmentWayPointPathInterpolator;
import org.eclipse.apogy.addons.geometry.paths.WayPointPath;
import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFacade;
import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;

public class UniformDistanceWayPointPathInterpolatorCustomImpl extends UniformDistanceWayPointPathInterpolatorImpl {

	private double getHighResolutionMaximumDistanceInterval() {
		return getDistanceInterval() * 0.01;
	}

	private WayPointPath getHighResolutionWayPointPath(WayPointPath path) throws Exception {
		SegmentWayPointPathInterpolator interpolator = ApogyAddonsGeometryPathsFactory.eINSTANCE
				.createSegmentWayPointPathInterpolator();
		interpolator.setMaximumDistanceInterval(getHighResolutionMaximumDistanceInterval());
		return interpolator.process(path);
	}

	private double computeAverage(double[] values, int startIndex, int endIndex) {
		double average = 0.0;
		for (int i = startIndex; i < endIndex; i++) {
			average += values[i];
		}
		average = average / (endIndex - startIndex);
		return average;
	}

	private WayPointPath filterPath(WayPointPath path) throws Exception {
		WayPointPath result = ApogyAddonsGeometryPathsFactory.eINSTANCE.createWayPointPath();

		int n = Math.round((float) Math.ceil(getDistanceInterval() / getHighResolutionMaximumDistanceInterval()));
		int nlPath = (int) Math.round((float) path.getLength() / getDistanceInterval());

		// Computes the slopes and store them in a vector.
		double[][] slopes = new double[3][path.getPoints().size()];
		for (int i = 1; i < path.getPoints().size(); i++) {
			Point3d p0 = path.getPoints().get(i - 1).asPoint3d();
			Point3d p1 = path.getPoints().get(i).asPoint3d();

			slopes[0][i] = p1.x - p0.x;
			slopes[1][i] = p1.y - p0.y;
			slopes[2][i] = p1.z - p0.z;
		}

		// Adds the first point into the resulting path.
		result.getPoints().add(
				ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(path.getPoints().get(0)));

		// Interpolate the trajectory.
		int ihpath = 0;
		for (int j = 1; j < nlPath; j++) {
			// Computes the slopes average for the segment.
			double ux = computeAverage(slopes[0], ihpath, ihpath + n);
			double uy = computeAverage(slopes[1], ihpath, ihpath + n);
			double uz = computeAverage(slopes[2], ihpath, ihpath + n);

			// Normalises u.
			double norm = Math.sqrt((ux * ux) + (uy * uy) + (uz * uz));
			ux = ux / norm;
			uy = uy / norm;
			uz = uz / norm;

			ihpath = ihpath + n;
			Point3d lastPoint = result.getEndPoint().asPoint3d();
			CartesianPositionCoordinates newPoint = ApogyCommonGeometryData3DFacade.INSTANCE
					.createCartesianPositionCoordinates(lastPoint.x + ux * getDistanceInterval(),
							lastPoint.y + uy * getDistanceInterval(), lastPoint.z + uz * getDistanceInterval());

			// Adds the point just interpolated.
			result.getPoints().add(newPoint);
		}

		// Checks if the last point created is closer to the high resolution end
		// point than the distance interval
		if (result.getEndPoint().asPoint3d().distance(path.getEndPoint().asPoint3d()) <= getDistanceInterval()) {
			// Last point is close to the last point of the hight resolution
			// trajectory, no need to add an extra point, just
			// move the last point to match.
			result.getEndPoint().setX(path.getEndPoint().getX());
			result.getEndPoint().setY(path.getEndPoint().getY());
			result.getEndPoint().setZ(path.getEndPoint().getZ());
		} else {
			// Last point is far from the last point of the hight resolution
			// trajectory, need to add an extra point.
			result.getPoints().add(
					ApogyCommonGeometryData3DFacade.INSTANCE.createCartesianPositionCoordinates(path.getEndPoint()));
		}

		return result;
	}

	@Override
	public WayPointPath process(WayPointPath input) throws Exception {
		setInput(input);

		if (getProgressMonitor() != null)
			getProgressMonitor().beginTask("Interpolating WayPointPath.", 2);

		WayPointPath result = ApogyAddonsGeometryPathsFactory.eINSTANCE.createWayPointPath();
		setOutput(result);

		// First, interpolate the path at high resolution.
		WayPointPath highResolutionPath = getHighResolutionWayPointPath(input);
		if (getProgressMonitor() != null)
			getProgressMonitor().worked(1);

		// Filters the resulting path.
		result.getPoints().addAll(filterPath(highResolutionPath).getPoints());
		if (getProgressMonitor() != null)
			getProgressMonitor().worked(1);

		if (getProgressMonitor() != null)
			getProgressMonitor().done();

		return result;
	}
} // UniformDistanceWayPointPathInterpolatorImpl