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.InhomogeneousPoint3D;
19 import com.irurueta.geometry.Point3D;
20 import com.irurueta.geometry.Sphere;
21 import com.irurueta.navigation.LockedException;
22
23 /**
24 * Linearly solves the lateration problem using an homogeneous LMSE solution.
25 */
26 @SuppressWarnings("DuplicatedCode")
27 public class HomogeneousLinearLeastSquaresLateration3DSolver extends
28 HomogeneousLinearLeastSquaresLaterationSolver<Point3D> {
29
30 /**
31 * Constructor.
32 */
33 public HomogeneousLinearLeastSquaresLateration3DSolver() {
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 (4 points).
44 */
45 public HomogeneousLinearLeastSquaresLateration3DSolver(final Point3D[] 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 HomogeneousLinearLeastSquaresLateration3DSolver(final LaterationSolverListener<Point3D> 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 (4 points).
66 */
67 public HomogeneousLinearLeastSquaresLateration3DSolver(
68 final Point3D[] positions, final double[] distances, final LaterationSolverListener<Point3D> listener) {
69 super(positions, distances, listener);
70 }
71
72 /**
73 * Constructor.
74 *
75 * @param spheres spheres defining positions and distances.
76 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 4.
77 */
78 public HomogeneousLinearLeastSquaresLateration3DSolver(final Sphere[] spheres) {
79 super();
80 internalSetSpheres(spheres);
81 }
82
83 /**
84 * Constructor.
85 *
86 * @param spheres spheres defining positions and distances.
87 * @param listener listener to be notified of events raised by this instance.
88 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 4.
89 */
90 public HomogeneousLinearLeastSquaresLateration3DSolver(
91 final Sphere[] spheres, final LaterationSolverListener<Point3D> listener) {
92 super(listener);
93 internalSetSpheres(spheres);
94 }
95
96 /**
97 * Gets spheres defined by provided positions and distances.
98 *
99 * @return spheres defined by provided positions and distances.
100 */
101 public Sphere[] getSpheres() {
102 if (positions == null) {
103 return null;
104 }
105
106 final var result = new Sphere[positions.length];
107
108 for (var i = 0; i < positions.length; i++) {
109 result[i] = new Sphere(positions[i], distances[i]);
110 }
111 return result;
112 }
113
114 /**
115 * Sets spheres defining positions and Euclidean distances.
116 *
117 * @param spheres spheres defining positions and distances.
118 * @throws IllegalArgumentException if spheres is null or length of array of spheres
119 * is less than 2.
120 * @throws LockedException if instance is busy solving the lateration problem.
121 */
122 public void setSpheres(final Sphere[] spheres) throws LockedException {
123 if (isLocked()) {
124 throw new LockedException();
125 }
126 internalSetSpheres(spheres);
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 Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH;
137 }
138
139 /**
140 * Minimum required number of positions and distances.
141 * At least 4 positions and distances will be required to linearly solve a 3D problem.
142 *
143 * @return minimum required number of positions and distances.
144 */
145 @Override
146 public int getMinRequiredPositionsAndDistances() {
147 return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
148 }
149
150 /**
151 * Gets estimated position.
152 *
153 * @return estimated position.
154 */
155 @Override
156 public Point3D getEstimatedPosition() {
157 if (estimatedPositionCoordinates == null) {
158 return null;
159 }
160
161 final var position = new InhomogeneousPoint3D();
162 getEstimatedPosition(position);
163 return position;
164 }
165
166 /**
167 * Internally sets spheres defining positions and Euclidean distances.
168 *
169 * @param spheres spheres defining positions and distances.
170 * @throws IllegalArgumentException if spheres is null or length of array of spheres
171 * is less than 4.
172 */
173 private void internalSetSpheres(final Sphere[] spheres) {
174 if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
175 throw new IllegalArgumentException();
176 }
177
178 final var positions = new Point3D[spheres.length];
179 final var distances = new double[spheres.length];
180 for (var i = 0; i < spheres.length; i++) {
181 final var sphere = spheres[i];
182 positions[i] = sphere.getCenter();
183 distances[i] = sphere.getRadius();
184 }
185
186 internalSetPositionsAndDistances(positions, distances);
187 }
188 }