bundles/addons/org.eclipse.apogy.addons.geometry.paths/src-gen-custom/org/eclipse/apogy/addons/geometry/paths/impl/SegmentWayPointPathInterpolatorCustomImpl.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 java.util.ArrayList;
import java.util.List;

import javax.vecmath.Point3d;

import org.eclipse.apogy.addons.geometry.paths.ApogyAddonsGeometryPathsFactory;
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 SegmentWayPointPathInterpolatorCustomImpl extends SegmentWayPointPathInterpolatorImpl {

	/**
	 * Interpolates the segment p0-p1 using the current maximumDistanceInterval.
	 * 
	 * @param p0 The first point defining the segment.
	 * @param p1 The second point defining the segment.
	 * @return The list of interpolated point, excluding p1.
	 */
	protected List<CartesianPositionCoordinates> interpolateSegment(CartesianPositionCoordinates p0,
			CartesianPositionCoordinates p1) {
		List<CartesianPositionCoordinates> points = new ArrayList<CartesianPositionCoordinates>();

		Point3d point0 = p0.asPoint3d();
		Point3d point1 = p1.asPoint3d();

		// Check if the point are further apart than maximumDistanceInterval
		double distance = point0.distance(point1);
		if (distance > getMaximumDistanceInterval()) {
			double deltaX = point1.x - point0.x;
			double deltaY = point1.y - point0.y;
			double deltaZ = point1.z - point0.z;

			double deltaT = getMaximumDistanceInterval() / distance;
			double t = 0.0;
			while (t < 1.0) {
				double x = point0.x + (t * deltaX);
				double y = point0.y + (t * deltaY);
				double z = point0.z + (t * deltaZ);

				CartesianPositionCoordinates point = ApogyCommonGeometryData3DFacade.INSTANCE
						.createCartesianPositionCoordinates(x, y, z);
				points.add(point);

				t += deltaT;
			}
		} else {
			// Not far enough, just add p0.
			points.add(p0);
		}

		return points;
	}

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

		if (getProgressMonitor() != null)
			getProgressMonitor().beginTask("Interpolating WayPointPath.", input.getPoints().size());

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

		if (input.getPoints().size() > 1) {
			// Goes through each segment and interpolate it.
			for (int i = 0; i < input.getPoints().size() - 1; i++) {
				CartesianPositionCoordinates p0 = input.getPoints().get(i);
				CartesianPositionCoordinates p1 = input.getPoints().get(i + 1);

				result.getPoints().addAll(interpolateSegment(p0, p1));

				if (getProgressMonitor() != null)
					getProgressMonitor().worked(1);
			}
			// Adds the last point.
			CartesianPositionCoordinates lastPoint = ApogyCommonGeometryData3DFacade.INSTANCE
					.createCartesianPositionCoordinates(input.getEndPoint());

			result.getPoints().add(lastPoint);
		} else if (input.getPoints().size() == 1) {
			CartesianPositionCoordinates point = ApogyCommonGeometryData3DFacade.INSTANCE
					.createCartesianPositionCoordinates(input.getStartPoint());
			result.getPoints().add(point);
		}
		if (getProgressMonitor() != null)
			getProgressMonitor().done();

		return result;
	}
} // SegmentWayPointPathInterpolatorImpl