1 package com.irurueta.navigation.frames;
2
3 import com.irurueta.algebra.Matrix;
4 import com.irurueta.algebra.WrongSizeException;
5 import com.irurueta.units.Speed;
6 import com.irurueta.units.SpeedConverter;
7 import com.irurueta.units.SpeedUnit;
8
9 import java.io.Serializable;
10 import java.util.Objects;
11
12 /**
13 * Contains body velocity with respect Earth, resolved about ECEF frame
14 * and expressed in meters per second (m/s).
15 */
16 public class ECEFVelocity implements Serializable, Cloneable {
17
18 /**
19 * Number of components.
20 */
21 public static final int COMPONENTS = 3;
22
23 /**
24 * X coordinate of velocity of body frame expressed in meters per second (m/s) with
25 * respect ECEF frame, resolved along the corresponding frame axes.
26 */
27 private double vx;
28
29 /**
30 * Y coordinate of velocity of body frame expressed in meters per second (m/s) with
31 * respect ECEF frame, resolved along the corresponding frame axes.
32 */
33 private double vy;
34
35 /**
36 * Z coordinate of velocity of body frame expressed in meters per second (m/s) with
37 * respect ECEF frame, resolved along the corresponding frame axes.
38 */
39 private double vz;
40
41 /**
42 * Constructor.
43 */
44 public ECEFVelocity() {
45 }
46
47 /**
48 * Constructor.
49 *
50 * @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved along
51 * ECEF-frame axes.
52 * @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved along
53 * ECEF-frame axes.
54 * @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved along
55 * ECEF-frame axes.
56 */
57 public ECEFVelocity(final double vx, final double vy, final double vz) {
58 setCoordinates(vx, vy, vz);
59 }
60
61 /**
62 * Constructor.
63 *
64 * @param vx x coordinate of velocity of body frame and resolved along ECEF-frame
65 * axes.
66 * @param vy y coordinate of velocity of body frame and resolved along ECEF-frame
67 * axes.
68 * @param vz z coordinate of velocity of body frame and resolved along ECEF-frame
69 * axes.
70 */
71 public ECEFVelocity(final Speed vx, final Speed vy, final Speed vz) {
72 setCoordinates(vx, vy, vz);
73 }
74
75 /**
76 * Constructor.
77 *
78 * @param input Body velocity to copy data from.
79 */
80 public ECEFVelocity(final ECEFVelocity input) {
81 copyFrom(input);
82 }
83
84 /**
85 * Gets x coordinate of velocity of body frame expressed in meters per second (m/s)
86 * resolved along ECEF-frame axes.
87 *
88 * @return x coordinate of velocity.
89 */
90 public double getVx() {
91 return vx;
92 }
93
94 /**
95 * Sets x coordinate of velocity of body frame expressed in meters per second (m/s)
96 * resolved along ECEF-frame axes.
97 *
98 * @param vx x coordinate of velocity.
99 */
100 public void setVx(final double vx) {
101 this.vx = vx;
102 }
103
104 /**
105 * Gets y coordinate of velocity of body frame expressed in meters per second (m/s)
106 * resolved along ECEF-frame axes.
107 *
108 * @return y coordinate of velocity.
109 */
110 public double getVy() {
111 return vy;
112 }
113
114 /**
115 * Sets y coordinate of velocity of body frame expressed in meters per second (m/s)
116 * resolved along ECEF-frame axes.
117 *
118 * @param vy y coordinate of velocity.
119 */
120 public void setVy(final double vy) {
121 this.vy = vy;
122 }
123
124 /**
125 * Gets z coordinate of velocity of body frame expressed in meters per second (m/s)
126 * resolved along ECEF-frame axes.
127 *
128 * @return z coordinate of velocity.
129 */
130 public double getVz() {
131 return vz;
132 }
133
134 /**
135 * Sets z coordinate of velocity of body frame expressed in meters per second (m/s)
136 * resolved along ECEF-frame axes.
137 *
138 * @param vz z coordinate of velocity.
139 */
140 public void setVz(final double vz) {
141 this.vz = vz;
142 }
143
144 /**
145 * Sets velocity coordinates of body frame expressed in meters per second (m/s)
146 * resolved along ECEF-frame axes.
147 *
148 * @param vx x coordinate of velocity.
149 * @param vy y coordinate of velocity.
150 * @param vz z coordinate of velocity.
151 */
152 public void setCoordinates(final double vx, final double vy, final double vz) {
153 this.vx = vx;
154 this.vy = vy;
155 this.vz = vz;
156 }
157
158 /**
159 * Gets cartesian x coordinate of velocity of body frame resolved along ECEF-frame
160 * axes.
161 *
162 * @param result instance where cartesian x coordinate of velocity will be stored.
163 */
164 public void getSpeedX(final Speed result) {
165 result.setValue(vx);
166 result.setUnit(SpeedUnit.METERS_PER_SECOND);
167 }
168
169 /**
170 * Gets x coordinate of velocity of body frame resolved along ECEF-frame axes.
171 *
172 * @return x coordinate of velocity of body frame resolved along ECEF-frame axes.
173 */
174 public Speed getSpeedX() {
175 return new Speed(vx, SpeedUnit.METERS_PER_SECOND);
176 }
177
178 /**
179 * Sets x coordinate of velocity of body frame resolved along ECEF-frame axes.
180 *
181 * @param vx x coordinate of body velocity to be set.
182 */
183 public void setVx(final Speed vx) {
184 this.vx = convertSpeed(vx);
185 }
186
187 /**
188 * Gets cartesian y coordinate of velocity of body frame resolved along ECEF-frame
189 * axes.
190 *
191 * @param result instance where cartesian y coordinate of velocity will be stored.
192 */
193 public void getSpeedY(final Speed result) {
194 result.setValue(vy);
195 result.setUnit(SpeedUnit.METERS_PER_SECOND);
196 }
197
198 /**
199 * Gets y coordinate of velocity of body frame resolved along ECEF-frame axes.
200 *
201 * @return y coordinate of velocity of body frame resolved along ECEF-frame axes.
202 */
203 public Speed getSpeedY() {
204 return new Speed(vy, SpeedUnit.METERS_PER_SECOND);
205 }
206
207 /**
208 * Sets y coordinate of velocity of body frame resolved along ECEF-frame axes.
209 *
210 * @param vy y coordinate of body velocity to be set.
211 */
212 public void setVy(final Speed vy) {
213 this.vy = convertSpeed(vy);
214 }
215
216 /**
217 * Gets cartesian z coordinate of velocity of body frame resolved along ECEF-frame
218 * axes.
219 *
220 * @param result instance where cartesian z coordinate of velocity will be stored.
221 */
222 public void getSpeedZ(final Speed result) {
223 result.setValue(vz);
224 result.setUnit(SpeedUnit.METERS_PER_SECOND);
225 }
226
227 /**
228 * Gets z coordinate of velocity of body frame resolved along ECEF-frame axes.
229 *
230 * @return z coordinate of velocity of body frame resolved along ECEF-frame axes.
231 */
232 public Speed getSpeedZ() {
233 return new Speed(vz, SpeedUnit.METERS_PER_SECOND);
234 }
235
236 /**
237 * Sets z coordinate of velocity of body frame resolved along ECEF-frame axes.
238 *
239 * @param vz z coordinate of body velocity to be set.
240 */
241 public void setVz(final Speed vz) {
242 this.vz = convertSpeed(vz);
243 }
244
245 /**
246 * Sets cartesian coordinates of body velocity resolved along ECEF-frame axes.
247 *
248 * @param vx x cartesian coordinate of body velocity.
249 * @param vy y cartesian coordinate of body velocity.
250 * @param vz z cartesian coordinate of body velocity.
251 */
252 public void setCoordinates(final Speed vx, final Speed vy, final Speed vz) {
253 setVx(vx);
254 setVy(vy);
255 setVz(vz);
256 }
257
258 /**
259 * Gets norm of velocity expressed in meters per second (m/s), which represents
260 * the speed of the body.
261 *
262 * @return norm of velocity expressed in meters per second (m/s).
263 */
264 public double getNorm() {
265 return Math.sqrt(vx * vx + vy * vy + vz * vz);
266 }
267
268 /**
269 * Gets norm of velocity, which represents the speed of the body.
270 *
271 * @param result velocity norm.
272 */
273 public void getNormAsSpeed(final Speed result) {
274 result.setValue(getNorm());
275 result.setUnit(SpeedUnit.METERS_PER_SECOND);
276 }
277
278 /**
279 * Gets norm of velocity, which represents the speed of the body.
280 *
281 * @return velocity norm.
282 */
283 public Speed getNormAsSpeed() {
284 return new Speed(getNorm(), SpeedUnit.METERS_PER_SECOND);
285 }
286
287 /**
288 * Copies this instance data into provided instance.
289 *
290 * @param output destination instance where data will be copied to.
291 */
292 public void copyTo(final ECEFVelocity output) {
293 output.vx = vx;
294 output.vy = vy;
295 output.vz = vz;
296 }
297
298 /**
299 * Copies data of provided instance into this instance.
300 *
301 * @param input instance to copy data from.
302 */
303 public void copyFrom(final ECEFVelocity input) {
304 vx = input.vx;
305 vy = input.vy;
306 vz = input.vz;
307 }
308
309 /**
310 * Gets velocity coordinates expressed in meters per second (m/s)
311 * as an array.
312 *
313 * @param result array instance where velocity coordinates will
314 * be stored in x,y,z order.
315 * @throws IllegalArgumentException if provided array does not have length 3.
316 */
317 public void asArray(final double[] result) {
318 if (result.length != COMPONENTS) {
319 throw new IllegalArgumentException();
320 }
321
322 result[0] = vx;
323 result[1] = vy;
324 result[2] = vz;
325 }
326
327 /**
328 * Gets velocity coordinates expressed in meters per second (m/s) as an array.
329 *
330 * @return array containing velocity coordinates in x,y,z order.
331 */
332 public double[] asArray() {
333 final var result = new double[COMPONENTS];
334 asArray(result);
335 return result;
336 }
337
338 /**
339 * Gets velocity coordinates expressed in meters per second (m/s) as a column
340 * matrix.
341 * If provided matrix does not have size 3x1, it will be resized.
342 *
343 * @param result matrix instance where velocity coordinates will be stored
344 * in x,y,z order.
345 */
346 @SuppressWarnings("DuplicatedCode")
347 public void asMatrix(final Matrix result) {
348 if (result.getRows() != COMPONENTS || result.getColumns() != 1) {
349 try {
350 result.resize(COMPONENTS, 1);
351 } catch (final WrongSizeException ignore) {
352 // never happens
353 }
354 }
355
356 result.setElementAtIndex(0, vx);
357 result.setElementAtIndex(1, vy);
358 result.setElementAtIndex(2, vz);
359 }
360
361 /**
362 * Gets velocity coordinates expressed in meters per second (m/s) as a column
363 * matrix.
364 *
365 * @return velocity coordinates stored in x,y,z order.
366 */
367 public Matrix asMatrix() {
368 Matrix result;
369 try {
370 result = new Matrix(COMPONENTS, 1);
371 asMatrix(result);
372 } catch (final WrongSizeException ignore) {
373 // never happens
374 result = null;
375 }
376 return result;
377 }
378
379 /**
380 * Computes and returns hash code for this instance. Hash codes are almost unique
381 * values that are useful for fast classification and storage of objects in
382 * collections.
383 *
384 * @return Hash code.
385 */
386 @Override
387 public int hashCode() {
388 return Objects.hash(vx, vy, vz);
389 }
390
391 /**
392 * Checks if provided object is an ECEFVelocity having exactly the same contents
393 * as this instance.
394 *
395 * @param o Object to be compared.
396 * @return true if both objects are considered to be equal, false otherwise.
397 */
398 @Override
399 public boolean equals(final Object o) {
400 if (this == o) {
401 return true;
402 }
403 if (o == null || getClass() != o.getClass()) {
404 return false;
405 }
406 final var other = (ECEFVelocity) o;
407 return equals(other);
408 }
409
410 /**
411 * Checks if provided instance has exactly the same contents as this instance.
412 *
413 * @param other instance to be compared.
414 * @return true if both instances are considered to be equal, false otherwise.
415 */
416 public boolean equals(final ECEFVelocity other) {
417 return equals(other, 0.0);
418 }
419
420 /**
421 * Checks if provided instance has contents similar to this instance up to provided
422 * threshold value.
423 *
424 * @param other instance to be compared.
425 * @param threshold maximum difference allowed between velocity coordinates.
426 * @return true if both instances are considered to be equal (up to provided
427 * threshold), false otherwise.
428 */
429 public boolean equals(final ECEFVelocity other, final double threshold) {
430 if (other == null) {
431 return false;
432 }
433
434 return Math.abs(vx - other.vx) <= threshold
435 && Math.abs(vy - other.vy) <= threshold
436 && Math.abs(vz - other.vz) <= threshold;
437 }
438
439 /**
440 * Makes a copy of this instance.
441 *
442 * @return a copy of this instance.
443 * @throws CloneNotSupportedException if clone fails for some reason.
444 */
445 @Override
446 protected Object clone() throws CloneNotSupportedException {
447 final var result = (ECEFVelocity) super.clone();
448 copyTo(result);
449 return result;
450 }
451
452 /**
453 * Converts speed instance into meters per second value.
454 *
455 * @param speed instance to be converted.
456 * @return converted value.
457 */
458 private double convertSpeed(final Speed speed) {
459 return SpeedConverter.convert(speed.getValue().doubleValue(),
460 speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
461 }
462 }