1 /*
2 * Copyright (C) 2019 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.lateration;
17
18 import com.irurueta.geometry.Circle;
19 import com.irurueta.geometry.InhomogeneousPoint2D;
20 import com.irurueta.geometry.Point2D;
21 import com.irurueta.navigation.LockedException;
22
23 /**
24 * Linearly solves the lateration problem using an homogeneous LMSE solution.
25 */
26 public class HomogeneousLinearLeastSquaresLateration2DSolver extends
27 HomogeneousLinearLeastSquaresLaterationSolver<Point2D> {
28
29 /**
30 * Constructor.
31 */
32 public HomogeneousLinearLeastSquaresLateration2DSolver() {
33 super();
34 }
35
36 /**
37 * Constructor.
38 *
39 * @param positions known positions of static nodes.
40 * @param distances euclidean distances from static nodes to mobile node.
41 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
42 * length is smaller than required (3 points).
43 */
44 public HomogeneousLinearLeastSquaresLateration2DSolver(final Point2D[] positions, final double[] distances) {
45 super(positions, distances);
46 }
47
48 /**
49 * Constructor.
50 *
51 * @param listener listener to be notified of events raised by this instance.
52 */
53 public HomogeneousLinearLeastSquaresLateration2DSolver(final LaterationSolverListener<Point2D> listener) {
54 super(listener);
55 }
56
57 /**
58 * Constructor.
59 *
60 * @param positions known positions of static nodes.
61 * @param distances euclidean distances from static nodes to mobile node.
62 * @param listener listener to be notified of events raised by this instance.
63 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
64 * length is smaller than required (3 points).
65 */
66 public HomogeneousLinearLeastSquaresLateration2DSolver(
67 final Point2D[] positions, final double[] distances, final LaterationSolverListener<Point2D> listener) {
68 super(positions, distances, listener);
69 }
70
71 /**
72 * Constructor.
73 *
74 * @param circles circles defining positions and distances.
75 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 3.
76 */
77 public HomogeneousLinearLeastSquaresLateration2DSolver(final Circle[] circles) {
78 super();
79 internalSetCircles(circles);
80 }
81
82 /**
83 * Constructor.
84 *
85 * @param circles circles defining positions and distances.
86 * @param listener listener to be notified of events raised by this instance.
87 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 3.
88 */
89 public HomogeneousLinearLeastSquaresLateration2DSolver(
90 final Circle[] circles, final LaterationSolverListener<Point2D> listener) {
91 super(listener);
92 internalSetCircles(circles);
93 }
94
95 /**
96 * Gets circles defined by provided positions and distances.
97 *
98 * @return circles defined by provided positions and distances.
99 */
100 public Circle[] getCircles() {
101 if (positions == null) {
102 return null;
103 }
104
105 final var result = new Circle[positions.length];
106
107 for (var i = 0; i < positions.length; i++) {
108 result[i] = new Circle(positions[i], distances[i]);
109 }
110 return result;
111 }
112
113 /**
114 * Sets circles defining positions and euclidean distances.
115 *
116 * @param circles circles defining positions and distances.
117 * @throws IllegalArgumentException if circles is null or length of array of circles
118 * is less than 2.
119 * @throws LockedException if instance is busy solving the lateration problem.
120 */
121 public void setCircles(final Circle[] circles) throws LockedException {
122 if (isLocked()) {
123 throw new LockedException();
124 }
125 internalSetCircles(circles);
126 }
127
128 /**
129 * Gets number of dimensions of provided points.
130 *
131 * @return always returns 2 dimensions.
132 */
133 @Override
134 public int getNumberOfDimensions() {
135 return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
136 }
137
138 /**
139 * Minimum required number of positions and distances.
140 * At least 3 positions and distances will be required to linearly solve a 2D problem.
141 *
142 * @return minimum required number of positions and distances.
143 */
144 @Override
145 public int getMinRequiredPositionsAndDistances() {
146 return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
147 }
148
149 /**
150 * Gets estimated position.
151 *
152 * @return estimated position.
153 */
154 @Override
155 public Point2D getEstimatedPosition() {
156 if (estimatedPositionCoordinates == null) {
157 return null;
158 }
159
160 final var position = new InhomogeneousPoint2D();
161 getEstimatedPosition(position);
162 return position;
163 }
164
165 /**
166 * Internally sets circles defining positions and Euclidean distances.
167 *
168 * @param circles circles defining positions and distances.
169 * @throws IllegalArgumentException if circles is null or length of array of circles
170 * is less than 3.
171 */
172 private void internalSetCircles(final Circle[] circles) {
173 if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
174 throw new IllegalArgumentException();
175 }
176
177 final var positions = new Point2D[circles.length];
178 final var distances = new double[circles.length];
179 for (var i = 0; i < circles.length; i++) {
180 final var circle = circles[i];
181 positions[i] = circle.getCenter();
182 distances[i] = circle.getRadius();
183 }
184
185 internalSetPositionsAndDistances(positions, distances);
186 }
187 }