LaterationSolver.java
/*
* Copyright (C) 2018 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.lateration;
import com.irurueta.geometry.Point;
import com.irurueta.navigation.LockedException;
import com.irurueta.navigation.NotReadyException;
/**
* Solves the lateration problem.
* This is a formulation for a nonlinear least squares optimizer.
* This class is base on the implementation found at:
* <a href="https://github.com/lemmingapex/trilateration">https://github.com/lemmingapex/trilateration</a>
*
* @param <P> a {@link Point} type.
*/
@SuppressWarnings("Duplicates")
public abstract class LaterationSolver<P extends Point<?>> {
/**
* Minimum allowed distance for a given circle or sphere.
*/
public static final double EPSILON = 1e-7;
/**
* Known positions of static nodes.
*/
protected P[] positions;
/**
* Euclidean distances from static nodes to mobile node.
*/
protected double[] distances;
/**
* Listener to be notified of events raised by this instance.
*/
protected LaterationSolverListener<P> listener;
/**
* Estimated inhomogeneous position coordinates.
*/
protected double[] estimatedPositionCoordinates;
/**
* Indicates if this instance is locked because lateration is being solved.
*/
protected boolean locked;
/**
* Constructor.
*/
protected LaterationSolver() {
}
/**
* Constructor.
* Sets known positions and Euclidean distances.
*
* @param positions known positions of static nodes.
* @param distances euclidean distances from static nodes to mobile node.
* @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
* length is smaller than required (3 for 2D points or 4 for 3D points).
*/
protected LaterationSolver(final P[] positions, final double[] distances) {
internalSetPositionsAndDistances(positions, distances);
}
/**
* Constructor.
*
* @param listener listener to be notified of events raised by this instance.
*/
protected LaterationSolver(final LaterationSolverListener<P> listener) {
this.listener = listener;
}
/**
* Constructor.
* Sets known positions and Euclidean distances.
*
* @param positions known positions of static nodes.
* @param distances euclidean distances from static nodes to mobile node.
* @param listener listener to be notified of events raised by this instance.
* @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
* length is smaller than required (3 for 2D points or 4 for 3D points).
*/
protected LaterationSolver(
final P[] positions, final double[] distances, final LaterationSolverListener<P> listener) {
this(positions, distances);
this.listener = listener;
}
/**
* Gets listener to be notified of events raised by this instance.
*
* @return listener to be notified of events raised by this instance.
*/
public LaterationSolverListener<P> getListener() {
return listener;
}
/**
* Sets listener to be notified of events raised by this instance.
*
* @param listener listener to be notified of events raised by this instance.
* @throws LockedException if instance is busy solving the lateration problem.
*/
public void setListener(final LaterationSolverListener<P> listener) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets known positions of static nodes.
*
* @return known positions of static nodes.
*/
public P[] getPositions() {
return positions;
}
/**
* Gets euclidean distances from static nodes to mobile node.
*
* @return euclidean distances from static nodes to mobile node.
*/
public double[] getDistances() {
return distances;
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
public boolean isReady() {
return positions != null && distances != null && positions.length >= getMinRequiredPositionsAndDistances();
}
/**
* Returns boolean indicating if solver is locked because estimation is under
* progress.
*
* @return true if solver is locked, false otherwise.
*/
public boolean isLocked() {
return locked;
}
/**
* Sets known positions and Euclidean distances.
* If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
*
* @param positions known positions of static nodes.
* @param distances euclidean distances from static nodes to mobile node.
* @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
* length is smaller than required (2 points).
* @throws LockedException if instance is busy solving the lateration problem.
*/
public void setPositionsAndDistances(
final P[] positions, final double[] distances) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
internalSetPositionsAndDistances(positions, distances);
}
/**
* Gets estimated inhomogeneous position coordinates.
*
* @return estimated inhomogeneous position coordinates.
*/
public double[] getEstimatedPositionCoordinates() {
return estimatedPositionCoordinates;
}
/**
* Gets estimated position and stores result into provided instance.
*
* @param estimatedPosition instance where estimated position will be stored.
*/
public void getEstimatedPosition(final P estimatedPosition) {
if (estimatedPositionCoordinates != null) {
for (var i = 0; i < estimatedPositionCoordinates.length; i++) {
estimatedPosition.setInhomogeneousCoordinate(i, estimatedPositionCoordinates[i]);
}
}
}
/**
* Gets estimated position.
*
* @return estimated position.
*/
public abstract P getEstimatedPosition();
/**
* Gets number of dimensions of provided points.
*
* @return number of dimensions of provided points.
*/
public abstract int getNumberOfDimensions();
/**
* Solves the lateration problem.
*
* @throws LaterationException if lateration fails.
* @throws NotReadyException is solver is not ready.
* @throws LockedException if instance is busy solving the lateration problem.
*/
public abstract void solve() throws LaterationException, NotReadyException, LockedException;
/**
* Gets lateration solver type.
*
* @return lateration solver type.
*/
public abstract LaterationSolverType getType();
/**
* Minimum required number of positions and distances.
* This value will depend on actual implementation and whether we are solving a 2D or 3D problem.
*
* @return minimum required number of positions and distances.
*/
public abstract int getMinRequiredPositionsAndDistances();
/**
* Internally sets known positions and Euclidean distances.
* If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
*
* @param positions known positions of static nodes.
* @param distances euclidean distances from static nodes to mobile node.
* @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
* length is smaller than required (2 points).
*/
protected void internalSetPositionsAndDistances(final P[] positions, final double[] distances) {
if (positions == null || distances == null) {
throw new IllegalArgumentException();
}
if (positions.length < getMinRequiredPositionsAndDistances()) {
throw new IllegalArgumentException();
}
if (positions.length != distances.length) {
throw new IllegalArgumentException();
}
this.positions = positions;
this.distances = distances;
// fix distances if needed
for (var i = 0; i < this.distances.length; i++) {
if (this.distances[i] < EPSILON) {
this.distances[i] = EPSILON;
}
}
}
}