1 /*
2 * Copyright (C) 2020 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.inertial.calibration;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.units.Measurement;
21
22 import java.io.Serializable;
23 import java.util.Objects;
24
25 /**
26 * Contains a triad of measurement data.
27 *
28 * @param <U> type of unit.
29 * @param <M> a type of measurement.
30 */
31 public abstract class Triad<U extends Enum<?>, M extends Measurement<U>, T extends Triad<U, M, T>>
32 implements Serializable {
33
34 /**
35 * Number of components of measurements.
36 */
37 public static final int COMPONENTS = 3;
38
39 /**
40 * Contains x coordinate of measurement value.
41 */
42 private double valueX;
43
44 /**
45 * Contains y coordinate of measurement value.
46 */
47 private double valueY;
48
49 /**
50 * Contains z coordinate of measurement value.
51 */
52 private double valueZ;
53
54 /**
55 * Contains unit of measurement.
56 */
57 private U unit;
58
59 /**
60 * Constructor.
61 *
62 * @param unit unit of measurement.
63 * @throws IllegalArgumentException if provided unit is null.
64 */
65 protected Triad(final U unit) {
66 if (unit == null) {
67 throw new IllegalArgumentException();
68 }
69
70 this.unit = unit;
71 }
72
73 /**
74 * Constructor.
75 *
76 * @param unit unit of measurement.
77 * @param valueX value of x-coordinate.
78 * @param valueY value of y-coordinate.
79 * @param valueZ value of z-coordinate.
80 * @throws IllegalArgumentException if provided unit is null.
81 */
82 protected Triad(final U unit, final double valueX, final double valueY, final double valueZ) {
83 this(unit);
84 setValueCoordinates(valueX, valueY, valueZ);
85 }
86
87 /**
88 * Copy constructor.
89 *
90 * @param other instance to copy from.
91 */
92 protected Triad(final Triad<U, M, T> other) {
93 copyFrom(other);
94 }
95
96 /**
97 * Gets x coordinate of measurement value expressed in current unit.
98 *
99 * @return x coordinate of measurement value.
100 */
101 public double getValueX() {
102 return valueX;
103 }
104
105 /**
106 * Sets x coordinate of measurement value using current unit.
107 *
108 * @param valueX x coordinate of measurement value.
109 */
110 public void setValueX(final double valueX) {
111 this.valueX = valueX;
112 }
113
114 /**
115 * Gets y coordinate of measurement value expressed in current unit.
116 *
117 * @return y coordinate of measurement value.
118 */
119 public double getValueY() {
120 return valueY;
121 }
122
123 /**
124 * Sets y coordinate of measurement value using current unit.
125 *
126 * @param valueY y coordinate of measurement value.
127 */
128 public void setValueY(final double valueY) {
129 this.valueY = valueY;
130 }
131
132 /**
133 * Gets z coordinate of measurement value expressed in current unit.
134 *
135 * @return z coordinate of measurement value.
136 */
137 public double getValueZ() {
138 return valueZ;
139 }
140
141 /**
142 * Sets z coordinate of measurement value using current unit.
143 *
144 * @param valueZ z coordinate of measurement value.
145 */
146 public void setValueZ(final double valueZ) {
147 this.valueZ = valueZ;
148 }
149
150 /**
151 * Sets coordinates of measurement using current unit.
152 *
153 * @param valueX x coordinate of measurement.
154 * @param valueY y coordinate of measurement.
155 * @param valueZ z coordinate of measurement.
156 */
157 public void setValueCoordinates(final double valueX, final double valueY, final double valueZ) {
158 this.valueX = valueX;
159 this.valueY = valueY;
160 this.valueZ = valueZ;
161 }
162
163 /**
164 * Gets unit of measurement.
165 *
166 * @return unit of measurement.
167 */
168 public U getUnit() {
169 return unit;
170 }
171
172 /**
173 * Sets unit of measurement.
174 *
175 * @param unit unit of measurement.
176 * @throws IllegalArgumentException if provided value is null.
177 */
178 public void setUnit(final U unit) {
179 if (unit == null) {
180 throw new IllegalArgumentException();
181 }
182 this.unit = unit;
183 }
184
185 /**
186 * Sets value coordinates and unit.
187 *
188 * @param valueX x coordinate of measurement.
189 * @param valueY y coordinate of measurement.
190 * @param valueZ z coordinate of measurement.
191 * @param unit unit of measurement.
192 * @throws IllegalArgumentException if provided unit is null.
193 */
194 public void setValueCoordinatesAndUnit(
195 final double valueX, final double valueY, final double valueZ, final U unit) {
196 setValueCoordinates(valueX, valueY, valueZ);
197 setUnit(unit);
198 }
199
200 /**
201 * Gets measurement values as an array expressed in current unit.
202 *
203 * @return array containing measurement values.
204 */
205 public double[] getValuesAsArray() {
206 return new double[]{valueX, valueY, valueZ};
207 }
208
209 /**
210 * Gets measurement values as an array expressed in current unit.
211 *
212 * @param result instance where result will be stored.
213 * @throws IllegalArgumentException if provided array does not have
214 * length 3.
215 */
216 public void getValuesAsArray(final double[] result) {
217 if (result.length != COMPONENTS) {
218 throw new IllegalArgumentException();
219 }
220 result[0] = valueX;
221 result[1] = valueY;
222 result[2] = valueZ;
223 }
224
225 /**
226 * Sets measurement coordinates from provided array.
227 *
228 * @param values array to set values from.
229 * @throws IllegalArgumentException if provided array does not have
230 * length 3.
231 */
232 public void setValueCoordinates(final double[] values) {
233 if (values.length != COMPONENTS) {
234 throw new IllegalArgumentException();
235 }
236
237 valueX = values[0];
238 valueY = values[1];
239 valueZ = values[2];
240 }
241
242 /**
243 * Gets measurement values as a column matrix expressed in current unit.
244 *
245 * @return matrix containing measurement values.
246 */
247 public Matrix getValuesAsMatrix() {
248 Matrix result = null;
249 try {
250 result = new Matrix(COMPONENTS, 1);
251 getValuesAsMatrix(result);
252 } catch (final WrongSizeException ignore) {
253 // never happens
254 }
255
256 return result;
257 }
258
259 /**
260 * Gets measurement values as a column matrix expressed in current unit.
261 *
262 * @param result instance where result will be stored.
263 * @throws IllegalArgumentException if provided matrix is not 3x1.
264 */
265 public void getValuesAsMatrix(final Matrix result) {
266 if (result.getRows() != COMPONENTS || result.getColumns() != 1) {
267 throw new IllegalArgumentException();
268 }
269 result.setElementAtIndex(0, valueX);
270 result.setElementAtIndex(1, valueY);
271 result.setElementAtIndex(2, valueZ);
272 }
273
274 /**
275 * Sets measurement coordinates from provided column matrix.
276 *
277 * @param values matrix to set values from.
278 * @throws IllegalArgumentException if provided matrix is not 3x1.
279 */
280 public void setValueCoordinates(final Matrix values) {
281 if (values.getRows() != COMPONENTS || values.getColumns() != 1) {
282 throw new IllegalArgumentException();
283 }
284
285 valueX = values.getElementAtIndex(0);
286 valueY = values.getElementAtIndex(1);
287 valueZ = values.getElementAtIndex(2);
288 }
289
290 /**
291 * Gets x coordinate of measurement value.
292 *
293 * @return x coordinate of measurement value.
294 */
295 public abstract M getMeasurementX();
296
297 /**
298 * Gets x coordinate of measurement value.
299 *
300 * @param result instance where x coordinate of measurement value
301 * will be stored.
302 */
303 public abstract void getMeasurementX(final M result);
304
305 /**
306 * Sets x coordinate of measurement value.
307 *
308 * @param measurementX x coordinate of measurement value.
309 */
310 public abstract void setMeasurementX(final M measurementX);
311
312 /**
313 * Gets y coordinate of measurement value.
314 *
315 * @return y coordinate of measurement value.
316 */
317 public abstract M getMeasurementY();
318
319 /**
320 * Gets y coordinate of measurement value.
321 *
322 * @param result instance where y coordinate of measurement value
323 * will be stored.
324 */
325 public abstract void getMeasurementY(final M result);
326
327 /**
328 * Sets y coordinate of measurement value.
329 *
330 * @param measurementY y coordinate of measurement value.
331 */
332 public abstract void setMeasurementY(final M measurementY);
333
334 /**
335 * Gets z coordinate of measurement value.
336 *
337 * @return z coordinate of measurement value.
338 */
339 public abstract M getMeasurementZ();
340
341 /**
342 * Gets z coordinate of measurement value.
343 *
344 * @param result instance where z coordinate of measurement value
345 * will be stored.
346 */
347 public abstract void getMeasurementZ(final M result);
348
349 /**
350 * Sets z coordinate of measurement value.
351 *
352 * @param measurementZ z coordinate of measurement value.
353 */
354 public abstract void setMeasurementZ(final M measurementZ);
355
356 /**
357 * Sets measurement coordinates.
358 *
359 * @param measurementX x coordinate of measurement value.
360 * @param measurementY y coordinate of measurement value.
361 * @param measurementZ z coordinate of measurement value.
362 */
363 public abstract void setMeasurementCoordinates(
364 final M measurementX, final M measurementY, final M measurementZ);
365
366 /**
367 * Gets squared norm expressed in squared current unit.
368 *
369 * @return squared norm for triad values.
370 */
371 public double getSqrNorm() {
372 return valueX * valueX + valueY * valueY + valueZ * valueZ;
373 }
374
375 /**
376 * Gets norm expressed in current unit.
377 *
378 * @return norm for triad values.
379 */
380 public double getNorm() {
381 return Math.sqrt(getSqrNorm());
382 }
383
384 /**
385 * Gets norm as a measurement.
386 *
387 * @return norm as a measurement.
388 */
389 public abstract M getMeasurementNorm();
390
391 /**
392 * Gets norm as a measurement.
393 *
394 * @param result instance where norm value will be stored.
395 */
396 public void getMeasurementNorm(final M result) {
397 result.setValue(getNorm());
398 result.setUnit(getUnit());
399 }
400
401 /**
402 * Copies this instance data into provided instance.
403 *
404 * @param output destination instance where data will be copied to.
405 */
406 public void copyTo(final Triad<U, M, T> output) {
407 output.copyFrom(this);
408 }
409
410 /**
411 * Copies data of provided instance into this instance.
412 *
413 * @param input instance to copy data from.
414 */
415 public void copyFrom(final Triad<U, M, T> input) {
416 valueX = input.valueX;
417 valueY = input.valueY;
418 valueZ = input.valueZ;
419 unit = input.unit;
420 }
421
422 /**
423 * Creates and returns a new instance having exactly the same contents
424 * as this instance.
425 *
426 * @return a copy of this instance.
427 */
428 public abstract T copy();
429
430 /**
431 * Computes and returns hash code for this instance. Hash codes are almost unique
432 * values that are useful for fas classification and storage of objects in collections.
433 *
434 * @return Hash code.
435 */
436 @Override
437 public int hashCode() {
438 return Objects.hash(valueX, valueY, valueZ, unit);
439 }
440
441 /**
442 * Checks if provided instance has exactly the same contents as this instance.
443 *
444 * @param other instance to be compared.
445 * @return true if both instances are considered to be equal, false otherwise.
446 */
447 public boolean equals(final Triad<U, M, T> other) {
448 return equals(other, 0.0);
449 }
450
451 /**
452 * Checks if provided instance has contents similar to this instance up to provided
453 * threshold value.
454 *
455 * @param other instance to be compared.
456 * @param threshold maximum allowed difference between values.
457 * @return true if both instances are considered to be equal (up to provided
458 * threshold), false otherwise.
459 */
460 public boolean equals(final Triad<U, M, T> other, final double threshold) {
461 if (other == null) {
462 return false;
463 }
464
465 return Math.abs(valueX - other.valueX) <= threshold && Math.abs(valueY - other.valueY) <= threshold
466 && Math.abs(valueZ - other.valueZ) <= threshold && Objects.equals(unit, other.unit);
467 }
468
469
470 /**
471 * Checks if provided object is a Triad instance having exactly the same contents
472 * as this instance.
473 *
474 * @param o object to be compared.
475 * @return true if both objects are considered to be equal, false otherwise.
476 */
477 @Override
478 public boolean equals(final Object o) {
479 if (this == o) {
480 return true;
481 }
482 if (o == null || getClass() != o.getClass()) {
483 return false;
484 }
485
486 //noinspection unchecked
487 final var triad = (Triad<U, M, T>) o;
488 return equals(triad);
489 }
490 }