ECEFPosition.java
/*
* Copyright (C) 2019 Alberto Irurueta Carro (alberto@irurueta.com)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.irurueta.navigation.frames;
import com.irurueta.algebra.Matrix;
import com.irurueta.algebra.WrongSizeException;
import com.irurueta.geometry.InhomogeneousPoint3D;
import com.irurueta.geometry.Point3D;
import com.irurueta.units.Distance;
import com.irurueta.units.DistanceConverter;
import com.irurueta.units.DistanceUnit;
import java.io.Serializable;
import java.util.Objects;
/**
* Contains body cartesian position with respect Earth, resolved about ECEF frame
* and expressed in meters (m).
*/
public class ECEFPosition implements Serializable, Cloneable {
/**
* Number of components.
*/
public static final int COMPONENTS = 3;
/**
* Cartesian x coordinate of body position expressed in meters (m) with respect ECEF frame, resolved
* along ECEF axes.
*/
private double x;
/**
* Cartesian y coordinate of body position expressed in meters (m) with respect ECEF frame, resolved
* along ECEF axes.
*/
private double y;
/**
* Cartesian z coordinate of body position expressed in meters (m) with respect ECEF frame, resolved
* along ECEF axes.
*/
private double z;
/**
* Constructor.
*/
public ECEFPosition() {
}
/**
* Constructor.
*
* @param x cartesian x coordinate of body position expressed in meters (m) and
* resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m) and
* resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m) and
* resolved along ECEF-frame axes.
*/
public ECEFPosition(final double x, final double y, final double z) {
setCoordinates(x, y, z);
}
/**
* Constructor.
*
* @param x cartesian x coordinate of body position resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position resolved along ECEF-frame axes.
*/
public ECEFPosition(final Distance x, final Distance y, final Distance z) {
setCoordinates(x, y, z);
}
/**
* Constructor.
*
* @param input ECEF body position to copy data from.
*/
public ECEFPosition(final ECEFPosition input) {
copyFrom(input);
}
/**
* Gets cartesian x coordinate of body position expressed in meters (m) with respect
* ECEF frame, resolved along ECEF axes.
*
* @return cartesian x coordinate of body position.
*/
public double getX() {
return x;
}
/**
* Sets cartesian x coordinate of body position expressed in meters (m) with respect
* ECEF frame, resolved along ECEF axes.
*
* @param x cartesian x coordinate of body position.
*/
public void setX(final double x) {
this.x = x;
}
/**
* Gets cartesian y coordinate of body position expressed in meters (m) with respect
* ECEF frame, resolved along ECEF axes.
*
* @return cartesian y coordinate of body position.
*/
public double getY() {
return y;
}
/**
* Sets cartesian y coordinate of body position expressed in meters (m) with respect
* ECEF frame, resolved along ECEF axes.
*
* @param y cartesian y coordinate of body position.
*/
public void setY(final double y) {
this.y = y;
}
/**
* Gets cartesian z coordinate of body position expressed in meters (m) with respect
* ECEF frame, resolved along ECEF axes.
*
* @return z coordinate of body position.
*/
public double getZ() {
return z;
}
/**
* Sets cartesian z coordinate of body position expressed in meters (m) with respect
* ECEF frame, resolved along ECEF axes.
*
* @param z cartesian z coordinate of body position.
*/
public void setZ(final double z) {
this.z = z;
}
/**
* Sets cartesian coordinates of body position expressed in meters (m) and resolved
* along ECEF-frame axes.
*
* @param x cartesian x coordinate of body position.
* @param y cartesian y coordinate of body position.
* @param z cartesian z coordinate of body position.
*/
public void setCoordinates(final double x, final double y, final double z) {
this.x = x;
this.y = y;
this.z = z;
}
/**
* Gets body position expressed in meters (m) and resolved along ECEF-frame axes.
*
* @return body position.
*/
public Point3D getPosition() {
return new InhomogeneousPoint3D(x, y, z);
}
/**
* Gets body position expressed in meters (m) and resolved along ECEF-frame axes.
*
* @param result instance where position data is copied to.
*/
public void getPosition(final Point3D result) {
result.setInhomogeneousCoordinates(x, y, z);
}
/**
* Sets body position expressed in meters (m) and resolved along ECEF-frame axes.
*
* @param point body position to be set.
*/
public void setPosition(final Point3D point) {
x = point.getInhomX();
y = point.getInhomY();
z = point.getInhomZ();
}
/**
* Gets cartesian x coordinate of body position resolved along ECEF-frame axes.
*
* @param result instance where cartesian x coordinate of body position will be
* stored.
*/
public void getDistanceX(final Distance result) {
result.setValue(x);
result.setUnit(DistanceUnit.METER);
}
/**
* Gets cartesian x coordinate of body position resolved along ECEF-frame axes.
*
* @return x coordinate of body position resolved along ECEF-frame axes.
*/
public Distance getDistanceX() {
return new Distance(x, DistanceUnit.METER);
}
/**
* Sets cartesian x coordinate of body position resolved along ECEF-frame axes.
*
* @param x cartesian x coordinate of body position to be set.
*/
public void setX(final Distance x) {
this.x = convertDistance(x);
}
/**
* Gets cartesian y coordinate of body position resolved along ECEF-frame axes.
*
* @param result instance where cartesian y coordinate of body position will be
* stored.
*/
public void getDistanceY(final Distance result) {
result.setValue(y);
result.setUnit(DistanceUnit.METER);
}
/**
* Gets cartesian y coordinate of body position resolved along ECEF-frame axes.
*
* @return y coordinate of body position resolved along ECEF-frame axes.
*/
public Distance getDistanceY() {
return new Distance(y, DistanceUnit.METER);
}
/**
* Sets cartesian y coordinate of body position resolved along ECEF-frame axes.
*
* @param y cartesian y coordinate of body position to be set.
*/
public void setY(final Distance y) {
this.y = convertDistance(y);
}
/**
* Gets cartesian z coordinate of body position resolved along ECEF-frame axes.
*
* @param result instance where cartesian z coordinate of body position will be
* stored.
*/
public void getDistanceZ(final Distance result) {
result.setValue(z);
result.setUnit(DistanceUnit.METER);
}
/**
* Gets cartesian z coordinate of body position resolved along ECEF-frame axes.
*
* @return z coordinate of body position resolved along ECEF-frame axes.
*/
public Distance getDistanceZ() {
return new Distance(z, DistanceUnit.METER);
}
/**
* Sets cartesian z coordinate of body position resolved along ECEF-frame axes.
*
* @param z cartesian z coordinate of body position to be set.
*/
public void setZ(final Distance z) {
this.z = convertDistance(z);
}
/**
* Sets cartesian coordinates of body position and resolved along ECEF-frame axes.
*
* @param x cartesian x coordinate of body position.
* @param y cartesian y coordinate of body position.
* @param z cartesian z coordinate of body position.
*/
public void setCoordinates(final Distance x, final Distance y, final Distance z) {
setX(x);
setY(y);
setZ(z);
}
/**
* Gets norm of position expressed in meters (m), which represents the distance to
* Earth's center of mass.
*
* @return position norm expressed in meters (m).
*/
public double getNorm() {
return Math.sqrt(x * x + y * y + z * z);
}
/**
* Gets norm of position, which represents the distance to Earth's center of mass.
*
* @param result instance where result will be stored.
*/
public void getNormAsDistance(final Distance result) {
result.setValue(getNorm());
result.setUnit(DistanceUnit.METER);
}
/**
* Gets norm of position, which represents the distance to Earth's center of mass.
*
* @return position norm.
*/
public Distance getNormAsDistance() {
return new Distance(getNorm(), DistanceUnit.METER);
}
/**
* Copies this instance data into provided instance.
*
* @param output destination instance where data will be copied to.
*/
public void copyTo(final ECEFPosition output) {
output.x = x;
output.y = y;
output.z = z;
}
/**
* Copies data of provided instance into this instance.
*
* @param input instance to copy data from.
*/
public void copyFrom(final ECEFPosition input) {
x = input.x;
y = input.y;
z = input.z;
}
/**
* Gets position coordinates expressed in meters (m) as an array.
*
* @param result array instance where position coordinates will
* be stored in x,y,z order.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void asArray(final double[] result) {
if (result.length != COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = x;
result[1] = y;
result[2] = z;
}
/**
* Gets position coordinates expressed in meters (m) as an array.
*
* @return array containing position coordinates in x,y,z order.
*/
public double[] asArray() {
final var result = new double[COMPONENTS];
asArray(result);
return result;
}
/**
* Gets position coordinates expressed in meters (m) as a column matrix.
* If provided matrix does not have size 3x1, it will be resized.
*
* @param result matrix instance where gravity coordinates will be stored in
* x,y,z order.
*/
@SuppressWarnings("DuplicatedCode")
public void asMatrix(final Matrix result) {
if (result.getRows() != COMPONENTS || result.getColumns() != 1) {
try {
result.resize(COMPONENTS, 1);
} catch (final WrongSizeException ignore) {
// never happens
}
}
result.setElementAtIndex(0, x);
result.setElementAtIndex(1, y);
result.setElementAtIndex(2, z);
}
/**
* Gets position coordinates expressed in meters (m) as a column matrix.
*
* @return position coordinates stored in x,y,z order.
*/
public Matrix asMatrix() {
Matrix result;
try {
result = new Matrix(COMPONENTS, 1);
asMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Computes and returns hash code for this instance. Hash codes are almost unique
* values that are useful for fast classification and storage of objects in
* collections.
*
* @return Hash code.
*/
@Override
public int hashCode() {
return Objects.hash(x, y, z);
}
/**
* Checks if provided object is an ECEFPosition having exactly the same contents
* as this instance.
*
* @param o Object to be compared.
* @return true if both objects are considered to be equal, false otherwise.
*/
@Override
public boolean equals(Object o) {
if (this == o) {
return true;
}
if (o == null || getClass() != o.getClass()) {
return false;
}
final ECEFPosition other = (ECEFPosition) o;
return equals(other);
}
/**
* Checks if provided instance has exactly the same contents as this instance.
*
* @param other instance to be compared.
* @return true if both instances are considered to be equal, false otherwise.
*/
public boolean equals(final ECEFPosition other) {
return equals(other, 0.0);
}
/**
* Checks if provided instance has contents similar to this instance up to provided
* threshold value.
*
* @param other instance to be compared.
* @param threshold maximum difference allowed between position coordinates.
* @return true if both instances are considered to be equal (up to provided
* threshold), false otherwise.
*/
public boolean equals(final ECEFPosition other, final double threshold) {
if (other == null) {
return false;
}
return Math.abs(x - other.x) <= threshold
&& Math.abs(y - other.y) <= threshold
&& Math.abs(z - other.z) <= threshold;
}
/**
* Makes a copy of this instance.
*
* @return a copy of this instance.
* @throws CloneNotSupportedException if clone fails for some reason.
*/
@Override
protected Object clone() throws CloneNotSupportedException {
final var result = (ECEFPosition) super.clone();
copyTo(result);
return result;
}
/**
* Converts distance instance into meters value.
*
* @param distance instance to be converted.
* @return converted value.
*/
private double convertDistance(final Distance distance) {
return DistanceConverter.convert(distance.getValue().doubleValue(),
distance.getUnit(), DistanceUnit.METER);
}
}