/*
* 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 .
*
*/
package de.tud.kom.p2psim.impl.topology.util;
import java.awt.Point;
import java.util.Arrays;
import org.locationtech.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 {
/**
* 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 of creation
*/
private long timeOfCreation;
/**
* 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.timeOfCreation = 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));
}
}
public PositionVector(double longitudeOrX, double latitudeOrY) {
this(3);
this.setLatitudeOrY(latitudeOrY);
this.setLongitudeOrX(longitudeOrX);
this.setAltitude(0);
}
public PositionVector(double longitudeOrX, double latitudeOrY, double altitude) {
this(3);
this.setLatitudeOrY(latitudeOrY);
this.setLongitudeOrX(longitudeOrX);
this.setAltitude(altitude);
}
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.timeOfCreation = 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]);
}
}
@Override
public PositionVector clone() {
/*
* If you extend Position Vector, make sure to overwrite this method!
*/
PositionVector clone = new PositionVector(this); // use clone constructor
clone.timeOfCreation = this.timeOfCreation;
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 values.length;
}
/**
* 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 == this.values.length;
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 this.values.length == delta.getDimensions();
for (int i = 0; i < getDimensions(); i++) {
setEntry(i, getEntry(i) + delta.getEntry(i));
}
}
/**
* Subtract a vector from the given vector
*
* @param delta
*/
public void subtract(PositionVector delta) {
assert this.values.length == delta.getDimensions();
for (int i = 0; i < getDimensions(); i++) {
setEntry(i, getEntry(i) - delta.getEntry(i));
}
}
/**
* Multiply this vector with a scalar value
*
* @param multi
* @return the vector
*/
public PositionVector multiplyScalar(double multi) {
for (int i = 0; i < getDimensions(); i++) {
setEntry(i, multi * getEntry(i));
}
return this;
}
/**
* 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 < getDimensions(); i++) {
hyp += getEntry(i) * getEntry(i);
}
hyp = Math.sqrt(hyp);
for (int i = 0; i < getDimensions(); 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 getDimensions() == delta.getDimensions();
PositionVector result = new PositionVector(getDimensions());
for (int i = 0; i < getDimensions(); 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 getDimensions() == delta.getDimensions();
PositionVector result = new PositionVector(getDimensions());
for (int i = 0; i < getDimensions(); 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, getDimensions());
}
/**
* Cast of 2 or 3-dimensional PositionVector to jts geometry library
* coordinates.
*
* @return 2 or 3 dimensional coordinates
*/
public Coordinate asCoordinate() {
if (getDimensions() < 2 || getDimensions() > 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() {
String s = "PV(";
for(int i = 0; i < getDimensions(); i++) {
s += values[i];
if(i < getDimensions() - 1) {
s += ", ";
}
}
s += ")";
return s;
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + getDimensions();
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 (getDimensions() != other.getDimensions())
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 getDimensions() == divisor.getDimensions();
for (int i = 0; i < getDimensions(); 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 getDimensions() == multiplicator.getDimensions();
for (int i = 0; i < getDimensions(); 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.values = new double[vector.getDimensions()];
for (int i = 0; i < this.getDimensions(); i++) {
setEntry(i, vector.getEntry(i));
}
}
/**
* Returns a new PositionVector that is a copy 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 getDimensions() > 2;
}
@Override
public long getAgeOfLocation() {
return Time.getCurrentTime() - timeOfCreation;
}
@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 < getDimensions(); 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 {
try {
return dest.distanceTo(this);
}
catch (Exception e) {
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");
}
}
}