bundles/common/org.eclipse.apogy.common.geometry.data3d/src-gen-custom/org/eclipse/apogy/common/geometry/data3d/impl/KDTreeBasedPointLocatorCustomImpl.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.common.geometry.data3d.impl;

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

import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import edu.wlu.cs.levy.CG.KDTree;
import edu.wlu.cs.levy.CG.KeyDuplicateException;
import edu.wlu.cs.levy.CG.KeySizeException;

public class KDTreeBasedPointLocatorCustomImpl extends KDTreeBasedPointLocatorImpl {

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

	List<CartesianPositionCoordinates> pointsToSearch = new ArrayList<CartesianPositionCoordinates>();

	private boolean isDirty;

	private KDTree kdTree;

	private double[] queryBuffer1;

	private double[] queryBuffer2;

	@Override
	protected void finalize() throws Throwable {
		this.pointsToSearch.clear();
		super.finalize();
	}

	@Override
	public List<CartesianPositionCoordinates> getPoints() {
		List<CartesianPositionCoordinates> points = new ArrayList<CartesianPositionCoordinates>();
		points.addAll(this.pointsToSearch);
		return points;
	}

	@Override
	public void addPoint(CartesianPositionCoordinates point) {
		this.isDirty = true;
		this.pointsToSearch.add(point);
	}

	@Override
	public void addPoints(List<CartesianPositionCoordinates> points) {
		this.isDirty = true;
		this.pointsToSearch.addAll(points);
	}

	@Override
	public void removePoint(CartesianPositionCoordinates point) {
		this.isDirty = true;
		this.pointsToSearch.remove(point);
	}

	@Override
	public void removePoints(List<CartesianPositionCoordinates> points) {
		this.isDirty = true;
		this.pointsToSearch.removeAll(points);
	}

	@Override
	public void clearPoints() {
		this.isDirty = true;
		this.pointsToSearch.clear();
	}

	@Override
	public CartesianPositionCoordinates findClosestPoint(CartesianPositionCoordinates point) {
		if (this.pointsToSearch.isEmpty()) {
			return null;
		} else {
			getQueryBuffer1()[0] = point.getX();
			getQueryBuffer1()[1] = point.getY();
			getQueryBuffer1()[2] = point.getZ();

			CartesianPositionCoordinates closestPoint = null;

			try {
				Object nearest = getKdTree().nearest(getQueryBuffer1());

				if (nearest != null) {
					closestPoint = this.pointsToSearch.get((Integer) nearest);
				}

			} catch (KeySizeException e) {
				Logger.error(e.getMessage(), e);
			}
			return closestPoint;
		}
	}

	@Override
	public List<CartesianPositionCoordinates> findClosestPoints(CartesianPositionCoordinates point,
			int maximumNumberOfNeighbors) {
		if (point == null) {
			throw new IllegalArgumentException();
		}

		if (maximumNumberOfNeighbors <= 0) {
			throw new IllegalArgumentException();
		}

		if (this.pointsToSearch.isEmpty()) {
			return null;
		} else {
			try {
				Object[] nearest = getKdTree().nearest(getQueryBuffer1(), maximumNumberOfNeighbors);

				List<CartesianPositionCoordinates> neighbors = new ArrayList<CartesianPositionCoordinates>();

				for (int i = 0; i < nearest.length; i++) {
					CartesianPositionCoordinates neighbor = this.pointsToSearch.get(((Integer) nearest[i]).intValue());
					neighbors.add(neighbor);
				}

				return neighbors;

			} catch (IllegalArgumentException e) {
				throw new IllegalArgumentException();
			} catch (KeySizeException e) {
				throw new IllegalArgumentException(e);
			}
		}
	}

	@Override
	public List<CartesianPositionCoordinates> findPointsWithinRadius(CartesianPositionCoordinates point,
			double radius) {
		if (point == null) {
			throw new IllegalArgumentException();
		}

		List<CartesianPositionCoordinates> neighbors = new ArrayList<CartesianPositionCoordinates>();

		if (this.pointsToSearch.isEmpty()) {
			return neighbors;
		} else {

			getQueryBuffer1()[0] = point.getX() - Math.abs(radius);
			getQueryBuffer1()[1] = point.getY() - Math.abs(radius);
			getQueryBuffer1()[2] = point.getZ() - Math.abs(radius);

			// The upper bound.
			getQueryBuffer2()[0] = point.getX() + Math.abs(radius);
			getQueryBuffer2()[1] = point.getY() + Math.abs(radius);
			getQueryBuffer2()[2] = point.getZ() + Math.abs(radius);

			try {
				Object[] range = getKdTree().range(getQueryBuffer1(), getQueryBuffer2());

				for (int i = 0; i < range.length; i++) {
					// Checks that the point is indeed within the radius.
					CartesianPositionCoordinates p = this.pointsToSearch.get((Integer) range[i]);

					if (p.asPoint3d().distance(point.asPoint3d()) < radius) {
						neighbors.add(this.pointsToSearch.get((Integer) range[i]));
					}
				}
			} catch (KeySizeException e) {
				Logger.error(e.getMessage(), e);
			}

			return neighbors;
		}
	}

	private KDTree getKdTree() {

		if (this.kdTree == null || this.isDirty) {
			if (this.pointsToSearch == null) {
				throw new IllegalArgumentException();
			}

			this.kdTree = new KDTree(3);

			// We build the KD tree.
			int pid = 0;
			for (CartesianPositionCoordinates point : this.pointsToSearch) {
				double key[] = new double[] { point.getX(), point.getY(), point.getZ() };
				try {
					this.kdTree.insert(key, pid++);
				} catch (KeySizeException e) {
					Logger.error(e.getMessage(), e);
				} catch (KeyDuplicateException e) {
				}
			}

			this.isDirty = false;
		}
		return this.kdTree;
	}

	private double[] getQueryBuffer1() {
		if (this.queryBuffer1 == null) {
			this.queryBuffer1 = new double[3];
		}
		return this.queryBuffer1;
	}

	private double[] getQueryBuffer2() {
		if (this.queryBuffer2 == null) {
			this.queryBuffer2 = new double[3];
		}
		return this.queryBuffer2;
	}
} // KDTreeBasedPointLocatorImpl