PositionVector.java

/*
 * Copyright (c) 2005-2011 KOM - Multimedia Communications Lab
 *
 * This file is part of PeerfactSim.KOM.
 * 
 * PeerfactSim.KOM is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 * 
 * PeerfactSim.KOM is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with PeerfactSim.KOM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

package de.tud.kom.p2psim.impl.topology.util;

import java.awt.Point;
import java.util.Arrays;

import com.vividsolutions.jts.geom.Coordinate;

import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.Location;

/**
 * N-Dimensional Vector containing a Position. Wherever possible, applications
 * and overlay should only use the {@link Position}-interface to implement their
 * functionality.
 * 
 * Just a minor note: if you add functionality, change existing functionality,
 * or "fix bugs", please comment AND SIGN your changes - you might
 * (unintentionally) break other people's code! This is a core component within
 * Peerfact - so each change might have a lot of undesired side effects!
 * 
 * CHANGELOG
 * 
 * - 14.08.2008 Bjoern Richerzhagen: removed a number of unused, uncommented, and
 * dubious methods from this class. Fixed the replace-bug (discovered by Nils)
 * with little overhead. Implemented assertions with assertions - previous
 * method introduces unwanted overhead outside of development settings. Removed
 * the dubious equals-tolerance, which violates the hashCode contract. Seriously
 * guys...
 * 
 * - 05.09.2018 Julian Zobel: Added location support for third dimension (altitude)
 * and removed a bug in the moveStep() function.
 * 
 * - 10.09.2018 Julian Zobel: Adapted the replace function to work properly without 
 * assertions. Replace does now fully replace the position vector entries. PVs are now
 * always at least 3D (with 0 altitude)
 * 
 * - 14.03.2019 Louis Neumann: Added timestamp when creating a PositionVector and adjusted ageOfLocation()
 * 
 * - 04.06.2019 Julian Zobel: Clone function also clones the timestamp from the cloned position. 
 * 
 * @author Bjoern Richerzhagen, Julian Zobel, Louis Neumann
 * @version 1.2, 10.09.2018
 */
public class PositionVector implements Location {

	private int dimensions;

	/**
	 * The private (!) coordinates of this vector. Ensures, that all necessary
	 * transforms can be performed in the getter-methods.
	 */
	private double[] values;
	 
	private double accuracy = -1;
	/**
	 * Timestamp with time when created
	 */
	private long created;

	/**
	 * Create a new Position Vector
	 * 
	 * @param dimensions
	 */
	public PositionVector(int dimensions) {
		if (dimensions < 2) {
			throw new AssertionError("Vector cannot have less than 2 dimensions.");
		}
		
		// Position Vector is always at least 3D
		if(dimensions == 2)
			dimensions = 3;
		
		this.dimensions = dimensions;
		this.values = new double[dimensions];
		this.created = Time.getCurrentTime();
	}

	/**
	 * Constructors for position vectors, also usable for cloning.
	 * 
	 * @param vec
	 */
	
	public PositionVector(PositionVector vec) {
		this(vec.getDimensions());
		for (int i = 0; i < vec.getDimensions(); i++) {
			setEntry(i, vec.getEntry(i));
		}
		
		this.created = Time.getCurrentTime();
	}
	
	public PositionVector(double longitudeOrX, double latitudeOrY) {
		this(3);
		this.setLatitudeOrY(latitudeOrY);
		this.setLongitudeOrX(longitudeOrX);		
		this.setAltitude(0);
		this.created = Time.getCurrentTime();
	}
	
	public PositionVector(double longitudeOrX, double latitudeOrY, double altitude) {
		this(3);
		this.setLatitudeOrY(latitudeOrY);
		this.setLongitudeOrX(longitudeOrX);
		this.setAltitude(altitude);
		this.created = Time.getCurrentTime();
	}

	public PositionVector(Location location) {			
		this.dimensions = 3;
		this.values = new double[3];
		
		if(location.hasAltitude()) {			
			this.setAltitude(location.getAltitude());
		}
		else {
			this.setAltitude(0);
		}
		
		this.setLatitudeOrY(location.getLatitudeOrY());
		this.setLongitudeOrX(location.getLongitudeOrX());
		this.created = Time.getCurrentTime();
	}	

	/**
	 * Convenience Constructor, initializes a Vector with values.length
	 * Dimensions and sets Entries, using the callback setEntry
	 * 
	 * @param values
	 */
	public PositionVector(double... values) {
		this(values.length);
		for (int i = 0; i < values.length; i++) {
			setEntry(i, values[i]);
		}
		this.created = Time.getCurrentTime();
	}
	
	@Override
	public PositionVector clone() {
		/*
		 * If you extend Position Vector, make sure to overwrite this method!
		 */
		PositionVector clone = new PositionVector(this); // use clone constructor
		clone.setTimestamp(this.created);
		return clone;
	}
	
	
	/*
	 *  
	 * 
	 */
	@Override
	public void setLatitudeOrY(double latitudeOrY)
	{
		this.setEntry(1, latitudeOrY);
	}
	
	@Override
	public void setLongitudeOrX(double longitudeOrX)
	{
		this.setEntry(0, longitudeOrX);
	}
	
	@Override
	public void setAltitude(double altitude) {
		this.setEntry(2, altitude);		
	}
	
	@Override
	public void setAccuracy(double accuracy)
			throws UnsupportedOperationException {
		if (accuracy < 0) {
			throw new AssertionError();
		}
		this.accuracy = accuracy;
	}
	
	@Override
	public double getAccuracy() {
		assert hasAccuracy() : "should check for hasAccuracy first!";
		return accuracy;
	}
	
	@Override
	public boolean hasAccuracy() {
		return accuracy != -1;
	}

	/**
	 * Number of Dimensions
	 * 
	 * @return
	 */
	public final int getDimensions() {
		return dimensions;
	}

	/**
	 * returns the nth position in the coord-Vector, starting with 0
	 * 
	 * @param dim
	 * @return
	 */
	public double getEntry(int dim) {
		return values[dim];
	}

	/**
	 * Saves a new value. Implementations might perform error control or
	 * additional scaling/translation
	 * 
	 * @param dim
	 * @param value
	 */
	public void setEntry(int dim, double value) {
		values[dim] = value;
	}

	/**
	 * Sets all entries.
	 * 
	 * @param values
	 */
	public void setEntries(double... values) {
		assert values.length == dimensions;
		for (int i = 0; i < values.length; i++) {
			setEntry(i, values[i]);
		}
	}

	/**
	 * Getter for the common X dimension.
	 * 
	 * @return value of dimension 0
	 */
	public double getX() {
		return getEntry(0);
	}

	/**
	 * Getter for the common Y dimension.
	 * 
	 * @return value of dimension 1
	 */
	public double getY() {
		return getEntry(1);
	}

	/**
	 * Getter for the common Z dimension.
	 * 
	 * @return value of dimension 2
	 */
	public double getZ() {
		return getEntry(2);
	}

	/**
	 * Modifies the current positionVector-instace by adding the delta-vector.
	 * Addition is done for each element of the vector.
	 * 
	 * @param delta
	 */
	public void add(PositionVector delta) {
		assert dimensions == delta.getDimensions();
		for (int i = 0; i < dimensions; i++) {
			setEntry(i, getEntry(i) + delta.getEntry(i));
		}
	}

	/**
	 * Subtract a vector from the given vector
	 * 
	 * @param delta
	 */
	public void subtract(PositionVector delta) {
		assert dimensions == delta.getDimensions();
		for (int i = 0; i < dimensions; i++) {
			setEntry(i, getEntry(i) - delta.getEntry(i));
		}
	}

	/**
	 * Multiply this vector with a scalar value
	 * 
	 * @param multi
	 */
	public void multiplyScalar(double multi) {
		for (int i = 0; i < dimensions; i++) {
			setEntry(i, multi * getEntry(i));
		}
	}

	/**
	 * Converts this vector to its normalized form (ie. its length is equal to
	 * one)
	 */
	public void normalize() {
		double hyp = 0.0;

		for (int i = 0; i < dimensions; i++) {
			hyp += getEntry(i) * getEntry(i);
		}

		hyp = Math.sqrt(hyp);

		for (int i = 0; i < dimensions; i++) {
			setEntry(i, getEntry(i) / hyp);
		}
	}

	/**
	 * Additive arithmetic. Produces a new vector as result. Current vector is
	 * not changed. If you want the current vector instance to change, you
	 * should use add instead.
	 * 
	 * @param delta
	 * @return addition of this vector plus delta vector
	 */
	public PositionVector plus(PositionVector delta) {
		assert dimensions == delta.getDimensions();
		PositionVector result = new PositionVector(dimensions);
		for (int i = 0; i < dimensions; i++) {
			result.setEntry(i, this.getEntry(i) + delta.getEntry(i));
		}
		return result;
	}

	/**
	 * Subtractive arithmetic. Produces a new vector as result. Current vector
	 * is not changed. If you want the current vector instance to change, you
	 * should use subtract instead.
	 * 
	 * @param delta
	 * @return subtraction of this vector minus delta vector
	 */
	public PositionVector minus(PositionVector delta) {
		assert dimensions == delta.getDimensions();
		PositionVector result = new PositionVector(dimensions);
		for (int i = 0; i < dimensions; i++) {
			result.setEntry(i, this.getEntry(i) - delta.getEntry(i));
		}
		return result;
	}

	@Override
	public int getTransmissionSize() {
		return getDimensions() * 8;
	}

	/**
	 * Mainly for drawing purposes, Representation of the first two dimensions
	 * as a Point
	 * 
	 * @return
	 */
	public Point asPoint() {
		return new Point((int) getEntry(0), (int) getEntry(1));
	}

	/**
	 * Representation of the Vector as a Double-Array
	 * 
	 * @return
	 */
	public double[] asDoubleArray() {
		return Arrays.copyOf(values, dimensions);
	}

	/**
	 * Cast of 2 or 3-dimensional PositionVector to jts geometry library
	 * coordinates.
	 * 
	 * @return 2 or 3 dimensional coordinates
	 */
	public Coordinate asCoordinate() {
		if (dimensions < 2 || dimensions > 3) {
			throw new AssertionError(
					"Cast to Coordinate only possible with two or three dimensional PositionVector");
		}
		
		return new Coordinate(getX(), getY(), getZ());
	}

	@Override
	public String toString() {				
		return "PV("+getDimensions()+") " + Arrays.toString(values) + "";			
	}
	
	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		result = prime * result + dimensions;
		result = prime * result + Arrays.hashCode(values);
		return result;
	}

	@Override
	public boolean equals(Object obj) {
		if (this == obj)
			return true;
		if (obj == null)
			return false;
		if (getClass() != obj.getClass())
			return false;
		PositionVector other = (PositionVector) obj;
		if (dimensions != other.dimensions)
			return false;

		return Arrays.equals(values, other.values);
	}

	/**
	 * Change this position vector by dividing each coordinate through the
	 * corresponding coordinate of the multiplicator
	 * 
	 * @param divisor
	 */
	public void divide(PositionVector divisor) {
		assert dimensions == divisor.getDimensions();
		for (int i = 0; i < dimensions; i++) {
			setEntry(i, this.getEntry(i) / divisor.getEntry(i));
		}
	}

	/**
	 * Change this position vector by multiplying each coordinate with the
	 * corresponding coordinate of the multiplicator
	 * 
	 * @param multiplicator
	 */
	public void multiply(PositionVector multiplicator) {
		assert dimensions == multiplicator.getDimensions();
		for (int i = 0; i < dimensions; i++) {
			setEntry(i, this.getEntry(i) * multiplicator.getEntry(i));
		}
	}

	/**
	 * Changes the current positionVector to be equal to the passed one.
	 * 
	 * @param vector
	 */
	public void replace(PositionVector vector) {
		this.dimensions = vector.getDimensions();
		this.values = new double[dimensions];
		for (int i = 0; i < this.dimensions; i++) {
			setEntry(i, vector.getEntry(i));
		}
	}

	/**
	 * Returns a new PositionVector that is a <strong>copy</strong> of the
	 * current position moved into the direction of destination with the given
	 * speed. Does not alter the current position-vector instance. Does not
	 * alter the destination vector instance.
	 * 
	 * IFF the speed would lead to us overshooting the destination, we will just
	 * move right onto the destination instead. This prevents oscillations in
	 * movement models.
	 * 
	 * FIXME BR: this method signature and purpose is not well defined. As it is
	 * only used in movement models, its functionality might be better
	 * implemented there...
	 * 
	 * @param destination
	 * @param speed
	 * @return
	 */
	@Deprecated
	public PositionVector moveStep(PositionVector destination, double speed) {

		if(speed == 0) {
			return new PositionVector(this);
		}
		
		double distance = destination.distanceTo(this);
		if (distance < speed) {
			/*
			 * We would overshoot the target.
			 */
			return new PositionVector(destination);
		}

		PositionVector direction = new PositionVector(destination);
		direction.subtract(this);
		direction.normalize();
		direction.multiplyScalar(speed);

		PositionVector newPosition = new PositionVector(this);
		newPosition.add(direction);

		return newPosition;
	}

	public double getLength() {
		double sum = 0;
		for (double val : values) {
			sum += val * val;
		}
		return Math.sqrt(sum);
	}

	@Override
	public void set(Location l) {
		if (l instanceof PositionVector) {
			this.replace((PositionVector) l);
		}
		else {
			throw new AssertionError("Cannot replace PositionVector with Location");
		}
	}

	@Override
	public double getLatitudeOrY() {
		return getY();
	}

	@Override
	public double getLongitudeOrX() {		
		return getX();
	}

	@Override
	public double getAltitude() {		
		return getZ();
	}
	
	@Override
	public boolean hasAltitude() {		
		return dimensions > 2;
	}
	
	@Override
	public long getAgeOfLocation() {
		return Time.getCurrentTime() - created; 		
	}
	
	private void setTimestamp(long locationTimestamp) {
		this.created = locationTimestamp;
	}

	@Override
	public double distanceTo(Location dest) {
		if (dest instanceof PositionVector) {
			PositionVector pv = (PositionVector) dest;
			if (pv.getDimensions() == getDimensions()) {
				double dist = 0;
				for (int i = 0; i < dimensions; i++) {
					// faster as Math.pow
					dist += (pv.getEntry(i) - getEntry(i))
							* (pv.getEntry(i) - getEntry(i));
				}
				return Math.sqrt(dist);
			} 
			else {
				throw new AssertionError(
						"Can not compute distance between Vectors of different length!");
			}
		} else {
			throw new AssertionError("Incompatible Types!");
		}
	}

	@Override
	public float bearingTo(Location dest) {
		if (dest instanceof PositionVector) {
			
			// This will be the angle between the difference vector (vec to dest) and the x-axis!
			PositionVector t = (PositionVector) dest;
			/*
			 * Calculates the angle using atan2 - this implies that the first
			 * two dimensions in your vector are the plane you are interested
			 * in.
			 */					
			return (float) Math.atan2(t.getEntry(1) - this.getEntry(1),
					t.getEntry(0) - this.getEntry(0));
		} else {
			throw new AssertionError(
					"Can only calculate an Angle on elements of type position vector");
		}
	}

	

	
}