1 /*
2 * Copyright (C) 2018 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 inhomogeneous solution.
25 */
26 @SuppressWarnings("DuplicatedCode")
27 public class InhomogeneousLinearLeastSquaresLateration2DSolver extends
28 InhomogeneousLinearLeastSquaresLaterationSolver<Point2D> {
29
30 /**
31 * Constructor.
32 */
33 public InhomogeneousLinearLeastSquaresLateration2DSolver() {
34 super();
35 }
36
37 /**
38 * Constructor.
39 *
40 * @param positions known positions of static nodes.
41 * @param distances euclidean distances from static nodes to mobile node.
42 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
43 * length is smaller than required (3 points).
44 */
45 public InhomogeneousLinearLeastSquaresLateration2DSolver(final Point2D[] positions, final double[] distances) {
46 super(positions, distances);
47 }
48
49 /**
50 * Constructor.
51 *
52 * @param listener listener to be notified of events raised by this instance.
53 */
54 public InhomogeneousLinearLeastSquaresLateration2DSolver(final LaterationSolverListener<Point2D> listener) {
55 super(listener);
56 }
57
58 /**
59 * Constructor.
60 *
61 * @param positions known positions of static nodes.
62 * @param distances euclidean distances from static nodes to mobile node.
63 * @param listener listener to be notified of events raised by this instance.
64 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
65 * length is smaller than required (3 points).
66 */
67 public InhomogeneousLinearLeastSquaresLateration2DSolver(
68 final Point2D[] positions, final double[] distances, final LaterationSolverListener<Point2D> listener) {
69 super(positions, distances, listener);
70 }
71
72 /**
73 * Constructor.
74 *
75 * @param circles circles defining positions and distances.
76 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 3.
77 */
78 public InhomogeneousLinearLeastSquaresLateration2DSolver(final Circle[] circles) {
79 super();
80 internalSetCircles(circles);
81 }
82
83 /**
84 * Constructor.
85 *
86 * @param circles circles defining positions and distances.
87 * @param listener listener to be notified of events raised by this instance.
88 * @throws IllegalArgumentException if circles is null or if length of circles array is less than 3.
89 */
90 public InhomogeneousLinearLeastSquaresLateration2DSolver(
91 final Circle[] circles, final LaterationSolverListener<Point2D> listener) {
92 super(listener);
93 internalSetCircles(circles);
94 }
95
96 /**
97 * Gets circles defined by provided positions and distances.
98 *
99 * @return circles defined by provided positions and distances.
100 */
101 public Circle[] getCircles() {
102 if (positions == null) {
103 return null;
104 }
105
106 final var result = new Circle[positions.length];
107
108 for (var i = 0; i < positions.length; i++) {
109 result[i] = new Circle(positions[i], distances[i]);
110 }
111 return result;
112 }
113
114 /**
115 * Sets circles defining positions and euclidean distances.
116 *
117 * @param circles circles defining positions and distances.
118 * @throws IllegalArgumentException if circles is null or length of array of circles
119 * is less than 2.
120 * @throws LockedException if instance is busy solving the lateration problem.
121 */
122 public void setCircles(final Circle[] circles) throws LockedException {
123 if (isLocked()) {
124 throw new LockedException();
125 }
126 internalSetCircles(circles);
127 }
128
129 /**
130 * Gets number of dimensions of provided points.
131 *
132 * @return always returns 2 dimensions.
133 */
134 @Override
135 public int getNumberOfDimensions() {
136 return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
137 }
138
139 /**
140 * Minimum required number of positions and distances.
141 * At least 3 positions and distances will be required to linearly solve a 2D problem.
142 *
143 * @return minimum required number of positions and distances.
144 */
145 @Override
146 public int getMinRequiredPositionsAndDistances() {
147 return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
148 }
149
150 /**
151 * Gets estimated position.
152 *
153 * @return estimated position.
154 */
155 @Override
156 public Point2D getEstimatedPosition() {
157 if (estimatedPositionCoordinates == null) {
158 return null;
159 }
160
161 final var position = new InhomogeneousPoint2D();
162 getEstimatedPosition(position);
163 return position;
164 }
165
166 /**
167 * Internally sets circles defining positions and Euclidean distances.
168 *
169 * @param circles circles defining positions and distances.
170 * @throws IllegalArgumentException if circles is null or length of array of circles
171 * is less than 3.
172 */
173 private void internalSetCircles(final Circle[] circles) {
174 if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
175 throw new IllegalArgumentException();
176 }
177
178 final var positions = new Point2D[circles.length];
179 final var distances = new double[circles.length];
180 for (var i = 0; i < circles.length; i++) {
181 final var circle = circles[i];
182 positions[i] = circle.getCenter();
183 distances[i] = circle.getRadius();
184 }
185
186 internalSetPositionsAndDistances(positions, distances);
187 }
188 }