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.frames;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.geometry.InvalidRotationMatrixException;
21 import com.irurueta.geometry.Rotation3D;
22 import com.irurueta.units.Angle;
23 import com.irurueta.units.AngleConverter;
24 import com.irurueta.units.AngleUnit;
25 import com.irurueta.units.Distance;
26 import com.irurueta.units.DistanceConverter;
27 import com.irurueta.units.DistanceUnit;
28 import com.irurueta.units.Speed;
29 import com.irurueta.units.SpeedConverter;
30 import com.irurueta.units.SpeedUnit;
31
32 import java.io.Serializable;
33 import java.util.Objects;
34
35 /**
36 * Contains position, velocity and coordinates transformation matrix expressed in NED frame.
37 * Position is expressed as latitude, longitude and height.
38 * Velocity of body frame is expressed with respect ECEF frame and resolved along north, east and down axes,
39 * as defined in {@link FrameType#LOCAL_NAVIGATION_FRAME}.
40 */
41 public class NEDFrame implements Frame, Serializable, Cloneable {
42
43 /**
44 * Number of coordinates representing velocity.
45 */
46 public static final int NUM_VELOCITY_COORDINATES = 3;
47
48 /**
49 * Latitude expressed in radians.
50 */
51 private double latitude;
52
53 /**
54 * Longitude expressed in radians.
55 */
56 private double longitude;
57
58 /**
59 * Height expressed in meters.
60 */
61 private double height;
62
63 /**
64 * Coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
65 * resolved along North axis.
66 */
67 private double vn;
68
69 /**
70 * Coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
71 * resolved along East axis.
72 */
73 private double ve;
74
75 /**
76 * Coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
77 * resolved along Down axis.
78 */
79 private double vd;
80
81 /**
82 * Body to NED coordinate transformation matrix.
83 */
84 private CoordinateTransformation c;
85
86 /**
87 * Constructor.
88 * Initializes position and velocity coordinates to zero and the coordinate transformation matrix to the
89 * identity.
90 */
91 public NEDFrame() {
92 c = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
93 }
94
95 /**
96 * Constructor.
97 *
98 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
99 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
100 */
101 public NEDFrame(final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
102 setCoordinateTransformation(c);
103 }
104
105 /**
106 * Constructor.
107 *
108 * @param latitude latitude expressed in radians.
109 * @param longitude longitude expressed in radians.
110 * @param height height expressed in meters.
111 */
112 public NEDFrame(final double latitude, final double longitude, final double height) {
113 this();
114 setPosition(latitude, longitude, height);
115 }
116
117 /**
118 * Constructor.
119 *
120 * @param latitude latitude expressed in radians to be set.
121 * @param longitude longitude expressed in radians to be set.
122 * @param height height to be set.
123 */
124 public NEDFrame(final double latitude, final double longitude, final Distance height) {
125 this();
126 setPosition(latitude, longitude, height);
127 }
128
129 /**
130 * Constructor.
131 *
132 * @param latitude latitude to be set.
133 * @param longitude longitude to be set.
134 * @param height height expressed in meters to be set.
135 */
136 public NEDFrame(final Angle latitude, final Angle longitude, final double height) {
137 this();
138 setPosition(latitude, longitude, height);
139 }
140
141 /**
142 * Constructor.
143 *
144 * @param latitude latitude to be set.
145 * @param longitude longitude to be set.
146 * @param height height to be set.
147 */
148 public NEDFrame(final Angle latitude, final Angle longitude, final Distance height) {
149 this();
150 setPosition(latitude, longitude, height);
151 }
152
153 /**
154 * Constructor.
155 *
156 * @param position curvilinear position containing latitude, longitude and height.
157 */
158 public NEDFrame(final NEDPosition position) {
159 this();
160 setPosition(position);
161 }
162
163 /**
164 * Constructor.
165 *
166 * @param latitude latitude expressed in radians.
167 * @param longitude longitude expressed in radians.
168 * @param height height expressed in meters.
169 * @param vn coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
170 * frame and resolved along North axis.
171 * @param ve coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
172 * frame and resolved along East axis.
173 * @param vd coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
174 * frame and resolved along Down axis.
175 */
176 public NEDFrame(final double latitude, final double longitude, final double height,
177 final double vn, final double ve, final double vd) {
178 this(latitude, longitude, height);
179 setVelocityCoordinates(vn, ve, vd);
180 }
181
182 /**
183 * Constructor.
184 *
185 * @param latitude latitude expressed in radians to be set.
186 * @param longitude longitude expressed in radians to be set.
187 * @param height height to be set.
188 * @param vn coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
189 * frame and resolved along North axis.
190 * @param ve coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
191 * frame and resolved along East axis.
192 * @param vd coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
193 * frame and resolved along Down axis.
194 */
195 public NEDFrame(final double latitude, final double longitude, final Distance height,
196 final double vn, final double ve, final double vd) {
197 this(latitude, longitude, height);
198 setVelocityCoordinates(vn, ve, vd);
199 }
200
201 /**
202 * Constructor.
203 *
204 * @param latitude latitude to be set.
205 * @param longitude longitude to be set.
206 * @param height height expressed in meters to be set.
207 * @param vn coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
208 * frame and resolved along North axis.
209 * @param ve coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
210 * frame and resolved along East axis.
211 * @param vd coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
212 * frame and resolved along Down axis.
213 */
214 public NEDFrame(final Angle latitude, final Angle longitude, final double height,
215 final double vn, final double ve, final double vd) {
216 this(latitude, longitude, height);
217 setVelocityCoordinates(vn, ve, vd);
218 }
219
220 /**
221 * Constructor.
222 *
223 * @param latitude latitude to be set.
224 * @param longitude longitude to be set.
225 * @param height height to be set.
226 * @param vn coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
227 * frame and resolved along North axis.
228 * @param ve coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
229 * frame and resolved along East axis.
230 * @param vd coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
231 * frame and resolved along Down axis.
232 */
233 public NEDFrame(final Angle latitude, final Angle longitude, final Distance height,
234 final double vn, final double ve, final double vd) {
235 this(latitude, longitude, height);
236 setVelocityCoordinates(vn, ve, vd);
237 }
238
239 /**
240 * Constructor.
241 *
242 * @param latitude latitude expressed in radians.
243 * @param longitude longitude expressed in radians.
244 * @param height height expressed in meters.
245 * @param speedN coordinate of velocity of body frame with respect ECEF frame and resolved along North axis.
246 * @param speedE coordinate of velocity of body frame with respect ECEF frame and resolved along East axis.
247 * @param speedD coordinate of velocity of body frame with respect ECEF frame and resolved along Down axis.
248 */
249 public NEDFrame(final double latitude, final double longitude, final double height,
250 final Speed speedN, final Speed speedE, final Speed speedD) {
251 this(latitude, longitude, height);
252 setSpeedCoordinates(speedN, speedE, speedD);
253 }
254
255 /**
256 * Constructor.
257 *
258 * @param latitude latitude expressed in radians.
259 * @param longitude longitude expressed in radians.
260 * @param height height to be set.
261 * @param speedN coordinate of velocity of body frame with respect ECEF frame and resolved along North axis.
262 * @param speedE coordinate of velocity of body frame with respect ECEF frame and resolved along East axis.
263 * @param speedD coordinate of velocity of body frame with respect ECEF frame and resolved along Down axis.
264 */
265 public NEDFrame(final double latitude, final double longitude, final Distance height,
266 final Speed speedN, final Speed speedE, final Speed speedD) {
267 this(latitude, longitude, height);
268 setSpeedCoordinates(speedN, speedE, speedD);
269 }
270
271 /**
272 * Constructor.
273 *
274 * @param latitude latitude to be set.
275 * @param longitude longitude to be set.
276 * @param height height expressed in meters to be set
277 * @param speedN coordinate of velocity of body frame with respect ECEF frame and resolved along North axis.
278 * @param speedE coordinate of velocity of body frame with respect ECEF frame and resolved along East axis.
279 * @param speedD coordinate of velocity of body frame with respect ECEF frame and resolved along Down axis.
280 */
281 public NEDFrame(final Angle latitude, final Angle longitude, final double height,
282 final Speed speedN, final Speed speedE, final Speed speedD) {
283 this(latitude, longitude, height);
284 setSpeedCoordinates(speedN, speedE, speedD);
285 }
286
287 /**
288 * Constructor.
289 *
290 * @param latitude latitude to be set.
291 * @param longitude longitude to be set.
292 * @param height height to be set.
293 * @param speedN coordinate of velocity of body frame with respect ECEF frame and resolved along North axis.
294 * @param speedE coordinate of velocity of body frame with respect ECEF frame and resolved along East axis.
295 * @param speedD coordinate of velocity of body frame with respect ECEF frame and resolved along Down axis.
296 */
297 public NEDFrame(final Angle latitude, final Angle longitude, final Distance height,
298 final Speed speedN, final Speed speedE, final Speed speedD) {
299 this(latitude, longitude, height);
300 setSpeedCoordinates(speedN, speedE, speedD);
301 }
302
303 /**
304 * Constructor.
305 *
306 * @param position curvilinear position to be set containing latitude, longitude and height.
307 * @param velocity velocity of body frame resolved along North, East, Down axes.
308 */
309 public NEDFrame(final NEDPosition position, final NEDVelocity velocity) {
310 this(position);
311 setVelocity(velocity);
312 }
313
314 /**
315 * Constructor.
316 *
317 * @param latitude latitude expressed in radians.
318 * @param longitude longitude expressed in radians.
319 * @param height height expressed in meters.
320 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
321 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
322 */
323 public NEDFrame(final double latitude, final double longitude, final double height,
324 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
325 this(latitude, longitude, height);
326 setCoordinateTransformation(c);
327 }
328
329 /**
330 * Constructor.
331 *
332 * @param latitude latitude to be set.
333 * @param longitude longitude to be set.
334 * @param height height expressed in meters to be set.
335 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
336 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
337 */
338 public NEDFrame(final Angle latitude, final Angle longitude, final double height,
339 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
340 this(latitude, longitude, height);
341 setCoordinateTransformation(c);
342 }
343
344 /**
345 * Constructor.
346 *
347 * @param latitude latitude to be set.
348 * @param longitude longitude to be set.
349 * @param height height to be set.
350 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
351 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
352 */
353 public NEDFrame(final Angle latitude, final Angle longitude, final Distance height,
354 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
355 this(latitude, longitude, height);
356 setCoordinateTransformation(c);
357 }
358
359 /**
360 * Constructor.
361 *
362 * @param position curvilinear position containing latitude, longitude and height.
363 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
364 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
365 */
366 public NEDFrame(final NEDPosition position, final CoordinateTransformation c)
367 throws InvalidSourceAndDestinationFrameTypeException {
368 this(position);
369 setCoordinateTransformation(c);
370 }
371
372 /**
373 * Constructor.
374 *
375 * @param latitude latitude expressed in radians.
376 * @param longitude longitude expressed in radians.
377 * @param height height expressed in meters.
378 * @param vn coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
379 * frame and resolved along North axis.
380 * @param ve coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
381 * frame and resolved along East axis.
382 * @param vd coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
383 * frame and resolved along Down axis.
384 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
385 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
386 */
387 public NEDFrame(final double latitude, final double longitude, final double height,
388 final double vn, final double ve, final double vd,
389 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
390 this(latitude, longitude, height, vn, ve, vd);
391 setCoordinateTransformation(c);
392 }
393
394 /**
395 * Constructor.
396 *
397 * @param latitude latitude expressed in radians to be set.
398 * @param longitude longitude expressed in radians to be set.
399 * @param height height to be set.
400 * @param vn coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
401 * frame and resolved along North axis.
402 * @param ve coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
403 * frame and resolved along East axis.
404 * @param vd coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
405 * frame and resolved along Down axis.
406 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
407 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
408 */
409 public NEDFrame(final double latitude, final double longitude, final Distance height,
410 final double vn, final double ve, final double vd,
411 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
412 this(latitude, longitude, height, vn, ve, vd);
413 setCoordinateTransformation(c);
414 }
415
416 /**
417 * Constructor.
418 *
419 * @param latitude latitude to be set.
420 * @param longitude longitude to be set.
421 * @param height height expressed in meters to be set.
422 * @param vn coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
423 * frame and resolved along North axis.
424 * @param ve coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
425 * frame and resolved along East axis.
426 * @param vd coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
427 * frame and resolved along Down axis.
428 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
429 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
430 */
431 public NEDFrame(final Angle latitude, final Angle longitude, final double height,
432 final double vn, final double ve, final double vd,
433 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
434 this(latitude, longitude, height, vn, ve, vd);
435 setCoordinateTransformation(c);
436 }
437
438 /**
439 * Constructor.
440 *
441 * @param latitude latitude to be set.
442 * @param longitude longitude to be set.
443 * @param height height to be set.
444 * @param vn coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
445 * frame and resolved along North axis.
446 * @param ve coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
447 * frame and resolved along East axis.
448 * @param vd coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF
449 * frame and resolved along Down axis.
450 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
451 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
452 */
453 public NEDFrame(final Angle latitude, final Angle longitude, final Distance height,
454 final double vn, final double ve, final double vd,
455 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
456 this(latitude, longitude, height, vn, ve, vd);
457 setCoordinateTransformation(c);
458 }
459
460 /**
461 * Constructor.
462 *
463 * @param latitude latitude expressed in radians.
464 * @param longitude longitude expressed in radians.
465 * @param height height expressed in meters.
466 * @param speedN coordinate of velocity of body frame with respect ECEF frame and resolved along North axis.
467 * @param speedE coordinate of velocity of body frame with respect ECEF frame and resolved along East axis.
468 * @param speedD coordinate of velocity of body frame with respect ECEF frame and resolved along Down axis.
469 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
470 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
471 */
472 public NEDFrame(final double latitude, final double longitude, final double height,
473 final Speed speedN, final Speed speedE, final Speed speedD,
474 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
475 this(latitude, longitude, height, speedN, speedE, speedD);
476 setCoordinateTransformation(c);
477 }
478
479 /**
480 * Constructor.
481 *
482 * @param latitude latitude expressed in radians.
483 * @param longitude longitude expressed in radians.
484 * @param height height to be set.
485 * @param speedN coordinate of velocity of body frame with respect ECEF frame and resolved along North axis.
486 * @param speedE coordinate of velocity of body frame with respect ECEF frame and resolved along East axis.
487 * @param speedD coordinate of velocity of body frame with respect ECEF frame and resolved along Down axis.
488 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
489 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
490 */
491 public NEDFrame(final double latitude, final double longitude, final Distance height,
492 final Speed speedN, final Speed speedE, final Speed speedD,
493 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
494 this(latitude, longitude, height, speedN, speedE, speedD);
495 setCoordinateTransformation(c);
496 }
497
498 /**
499 * Constructor.
500 *
501 * @param latitude latitude to be set.
502 * @param longitude longitude to be set.
503 * @param height height expressed in meters to be set
504 * @param speedN coordinate of velocity of body frame with respect ECEF frame and resolved along North axis.
505 * @param speedE coordinate of velocity of body frame with respect ECEF frame and resolved along East axis.
506 * @param speedD coordinate of velocity of body frame with respect ECEF frame and resolved along Down axis.
507 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
508 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
509 */
510 public NEDFrame(final Angle latitude, final Angle longitude, final double height,
511 final Speed speedN, final Speed speedE, final Speed speedD,
512 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
513 this(latitude, longitude, height, speedN, speedE, speedD);
514 setCoordinateTransformation(c);
515 }
516
517 /**
518 * Constructor.
519 *
520 * @param latitude latitude to be set.
521 * @param longitude longitude to be set.
522 * @param height height to be set.
523 * @param speedN coordinate of velocity of body frame with respect ECEF frame and resolved along North axis.
524 * @param speedE coordinate of velocity of body frame with respect ECEF frame and resolved along East axis.
525 * @param speedD coordinate of velocity of body frame with respect ECEF frame and resolved along Down axis.
526 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
527 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
528 */
529 public NEDFrame(final Angle latitude, final Angle longitude, final Distance height,
530 final Speed speedN, final Speed speedE, final Speed speedD,
531 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
532 this(latitude, longitude, height, speedN, speedE, speedD);
533 setCoordinateTransformation(c);
534 }
535
536 /**
537 * Constructor.
538 *
539 * @param position curvilinear position to be set containing latitude, longitude and height.
540 * @param velocity velocity of body frame resolved along North, East, Down axes.
541 * @param c Body to NED (Local Navigation frame) coordinate transformation matrix to be set.
542 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
543 */
544 public NEDFrame(final NEDPosition position, final NEDVelocity velocity,
545 final CoordinateTransformation c) throws InvalidSourceAndDestinationFrameTypeException {
546 this(position, velocity);
547 setCoordinateTransformation(c);
548 }
549
550 /**
551 * Constructor.
552 *
553 * @param input NED frame to copy data from.
554 */
555 public NEDFrame(final NEDFrame input) {
556 this();
557 copyFrom(input);
558 }
559
560 /**
561 * Gets latitude expressed in radians.
562 *
563 * @return latitude expressed in radians.
564 */
565 public double getLatitude() {
566 return latitude;
567 }
568
569 /**
570 * Sets latitude expressed in radians.
571 *
572 * @param latitude latitude expressed in radians to be set.
573 */
574 public void setLatitude(final double latitude) {
575 this.latitude = latitude;
576 }
577
578 /**
579 * Gets longitude expressed in radians.
580 *
581 * @return longitude expressed in radians.
582 */
583 public double getLongitude() {
584 return longitude;
585 }
586
587 /**
588 * Sets longitude expressed in radians.
589 *
590 * @param longitude longitude expressed in radians to be set.
591 */
592 public void setLongitude(final double longitude) {
593 this.longitude = longitude;
594 }
595
596 /**
597 * Gets height expressed in meters.
598 *
599 * @return height expressed in meters.
600 */
601 public double getHeight() {
602 return height;
603 }
604
605 /**
606 * Sets height expressed in meters.
607 *
608 * @param height height expressed in meters to be set.
609 */
610 public void setHeight(final double height) {
611 this.height = height;
612 }
613
614 /**
615 * Sets body position.
616 *
617 * @param latitude latitude expressed in radians to be set.
618 * @param longitude longitude expressed in radians to be set.
619 * @param height height expressed in meters to be set.
620 */
621 public void setPosition(final double latitude, final double longitude, final double height) {
622 this.latitude = latitude;
623 this.longitude = longitude;
624 this.height = height;
625 }
626
627 /**
628 * Gets latitude.
629 *
630 * @param result instance where latitude will be stored.
631 */
632 public void getLatitudeAngle(final Angle result) {
633 result.setValue(latitude);
634 result.setUnit(AngleUnit.RADIANS);
635 }
636
637 /**
638 * Gets latitude.
639 *
640 * @return latitude.
641 */
642 public Angle getLatitudeAngle() {
643 return new Angle(latitude, AngleUnit.RADIANS);
644 }
645
646 /**
647 * Sets latitude.
648 *
649 * @param latitudeAngle latitude to be set.
650 */
651 public void setLatitudeAngle(final Angle latitudeAngle) {
652 latitude = AngleConverter.convert(latitudeAngle.getValue().doubleValue(), latitudeAngle.getUnit(),
653 AngleUnit.RADIANS);
654 }
655
656 /**
657 * Gets longitude.
658 *
659 * @param result instance where longitude will be stored.
660 */
661 public void getLongitudeAngle(final Angle result) {
662 result.setValue(longitude);
663 result.setUnit(AngleUnit.RADIANS);
664 }
665
666 /**
667 * Gets longitude.
668 *
669 * @return longitude.
670 */
671 public Angle getLongitudeAngle() {
672 return new Angle(longitude, AngleUnit.RADIANS);
673 }
674
675 /**
676 * Sets longitude.
677 *
678 * @param longitudeAngle longitude to be set.
679 */
680 public void setLongitudeAngle(final Angle longitudeAngle) {
681 longitude = AngleConverter.convert(longitudeAngle.getValue().doubleValue(), longitudeAngle.getUnit(),
682 AngleUnit.RADIANS);
683 }
684
685 /**
686 * Gets height.
687 *
688 * @param result instance where height will be stored.
689 */
690 public void getHeightDistance(final Distance result) {
691 result.setValue(height);
692 result.setUnit(DistanceUnit.METER);
693 }
694
695 /**
696 * Gets height.
697 *
698 * @return height.
699 */
700 public Distance getHeightDistance() {
701 return new Distance(height, DistanceUnit.METER);
702 }
703
704 /**
705 * Sets height.
706 *
707 * @param heightDistance height to be set.
708 */
709 public void setHeightDistance(final Distance heightDistance) {
710 height = DistanceConverter.convert(heightDistance.getValue().doubleValue(), heightDistance.getUnit(),
711 DistanceUnit.METER);
712 }
713
714 /**
715 * Sets body position.
716 *
717 * @param latitude latitude expressed in radians to be set.
718 * @param longitude longitude expressed in radians to be set.
719 * @param height height to be set.
720 */
721 public void setPosition(final double latitude, final double longitude, final Distance height) {
722 this.latitude = latitude;
723 this.longitude = longitude;
724 setHeightDistance(height);
725 }
726
727 /**
728 * Sets body position.
729 *
730 * @param latitude latitude to be set.
731 * @param longitude longitude to be set.
732 * @param height height expressed in meters to be set.
733 */
734 public void setPosition(final Angle latitude, final Angle longitude, final double height) {
735 setLatitudeAngle(latitude);
736 setLongitudeAngle(longitude);
737 this.height = height;
738 }
739
740 /**
741 * Sets body position.
742 *
743 * @param latitude latitude to be set.
744 * @param longitude longitude to be set.
745 * @param height height to be set.
746 */
747 public void setPosition(final Angle latitude, final Angle longitude, final Distance height) {
748 setLatitudeAngle(latitude);
749 setLongitudeAngle(longitude);
750 setHeightDistance(height);
751 }
752
753 /**
754 * Gets curvilinear position, expressed in terms of latitude, longitude and height.
755 *
756 * @param result instance where curvilinear coordinates will be stored.
757 */
758 public void getPosition(final NEDPosition result) {
759 result.setCoordinates(latitude, longitude, height);
760 }
761
762 /**
763 * Gets curvilinear position, expressed in terms of latitude, longitude and height.
764 *
765 * @return curvilinear coordinates.
766 */
767 public NEDPosition getPosition() {
768 return new NEDPosition(latitude, longitude, height);
769 }
770
771 /**
772 * Sets curvilinear position, expressed in terms of latitude, longitude and height.
773 *
774 * @param position curvilinear position to be set.
775 */
776 public void setPosition(final NEDPosition position) {
777 latitude = position.getLatitude();
778 longitude = position.getLongitude();
779 height = position.getHeight();
780 }
781
782 /**
783 * Gets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
784 * resolved along North axis.
785 *
786 * @return North velocity coordinate value.
787 */
788 public double getVn() {
789 return vn;
790 }
791
792 /**
793 * Sets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
794 * resolved along North axis.
795 *
796 * @param vn North velocity coordinate value.
797 */
798 public void setVn(final double vn) {
799 this.vn = vn;
800 }
801
802 /**
803 * Gets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
804 * resolved along East axis.
805 *
806 * @return East velocity coordinate value.
807 */
808 public double getVe() {
809 return ve;
810 }
811
812 /**
813 * Sets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
814 * resolved along East axis.
815 *
816 * @param ve East velocity coordinate value.
817 */
818 public void setVe(final double ve) {
819 this.ve = ve;
820 }
821
822 /**
823 * Gets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
824 * resolved along Down axis.
825 *
826 * @return Down velocity coordinate value.
827 */
828 public double getVd() {
829 return vd;
830 }
831
832 /**
833 * Sets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
834 * resolved along Down axis.
835 *
836 * @param vd Down velocity coordinate value.
837 */
838 public void setVd(final double vd) {
839 this.vd = vd;
840 }
841
842 /**
843 * Sets velocity coordinates of body frame expressed in meters per second (m/s) resolved along North, East, Down
844 * axes.
845 *
846 * @param vn North velocity coordinate value.
847 * @param ve East velocity coordinate value.
848 * @param vd Down velocity coordinate value.
849 */
850 public void setVelocityCoordinates(final double vn, final double ve, final double vd) {
851 this.vn = vn;
852 this.ve = ve;
853 this.vd = vd;
854 }
855
856 /**
857 * Gets norm of velocity expressed in meters per second (m/s), which represents
858 * the speed of the body.
859 *
860 * @return norm of velocity expressed in meters per second (m/s).
861 */
862 public double getVelocityNorm() {
863 return Math.sqrt(vn * vn + ve * ve + vd * vd);
864 }
865
866 /**
867 * Gets norm of velocity, which represents the speed of the body.
868 *
869 * @param result velocity norm.
870 */
871 public void getVelocityNormAsSpeed(final Speed result) {
872 result.setValue(getVelocityNorm());
873 result.setUnit(SpeedUnit.METERS_PER_SECOND);
874 }
875
876 /**
877 * Gets norm of velocity, which represents the speed of the body.
878 *
879 * @return velocity norm.
880 */
881 public Speed getVelocityNormAsSpeed() {
882 return new Speed(getVelocityNorm(), SpeedUnit.METERS_PER_SECOND);
883 }
884
885 /**
886 * Gets coordinate of velocity of body frame with respect ECEF frame and
887 * resolved along North axis.
888 *
889 * @param result instance where North velocity coordinate will be stored.
890 */
891 public void getSpeedN(final Speed result) {
892 result.setValue(vn);
893 result.setUnit(SpeedUnit.METERS_PER_SECOND);
894 }
895
896 /**
897 * Gets coordinate of velocity of body frame with respect ECEF frame and
898 * resolved along North axis.
899 *
900 * @return North velocity coordinate.
901 */
902 public Speed getSpeedN() {
903 return new Speed(vn, SpeedUnit.METERS_PER_SECOND);
904 }
905
906 /**
907 * Sets coordinate of velocity of body frame with respect ECEF frame and
908 * resolved along North axis.
909 *
910 * @param speedN North velocity coordinate to be set.
911 */
912 public void setSpeedN(final Speed speedN) {
913 vn = SpeedConverter.convert(speedN.getValue().doubleValue(), speedN.getUnit(), SpeedUnit.METERS_PER_SECOND);
914 }
915
916 /**
917 * Gets coordinate of velocity of body frame with respect ECEF frame and
918 * resolved along East axis.
919 *
920 * @param result instance where East velocity coordinate will be stored.
921 */
922 public void getSpeedE(final Speed result) {
923 result.setValue(ve);
924 result.setUnit(SpeedUnit.METERS_PER_SECOND);
925 }
926
927 /**
928 * Gets coordinate of velocity of body frame with respect ECEF frame and
929 * resolved along East axis.
930 *
931 * @return East velocity coordinate.
932 */
933 public Speed getSpeedE() {
934 return new Speed(ve, SpeedUnit.METERS_PER_SECOND);
935 }
936
937 /**
938 * Sets coordinate of velocity of body frame with respect ECEF frame and
939 * resolved along East axis.
940 *
941 * @param speedE East velocity coordinate to be set.
942 */
943 public void setSpeedE(final Speed speedE) {
944 ve = SpeedConverter.convert(speedE.getValue().doubleValue(), speedE.getUnit(), SpeedUnit.METERS_PER_SECOND);
945 }
946
947 /**
948 * Gets coordinate of velocity of body frame with respect ECEF frame and
949 * resolved along Down axis.
950 *
951 * @param result instance where Down velocity coordinate will be stored.
952 */
953 public void getSpeedD(final Speed result) {
954 result.setValue(vd);
955 result.setUnit(SpeedUnit.METERS_PER_SECOND);
956 }
957
958 /**
959 * Gets coordinate of velocity of body frame with respect ECEF frame and
960 * resolved along Down axis.
961 *
962 * @return Down velocity coordinate.
963 */
964 public Speed getSpeedD() {
965 return new Speed(vd, SpeedUnit.METERS_PER_SECOND);
966 }
967
968 /**
969 * Sets coordinate of velocity of body frame with respect ECEF frame and
970 * resolved along Down axis.
971 *
972 * @param speedD Down velocity coordinate to be set.
973 */
974 public void setSpeedD(final Speed speedD) {
975 vd = SpeedConverter.convert(speedD.getValue().doubleValue(), speedD.getUnit(), SpeedUnit.METERS_PER_SECOND);
976 }
977
978 /**
979 * Sets velocity coordinates of body frame resolved along North, East, Down
980 * axes.
981 *
982 * @param speedN North velocity coordinate.
983 * @param speedE East velocity coordinate.
984 * @param speedD Down velocity coordinate.
985 */
986 public void setSpeedCoordinates(final Speed speedN, final Speed speedE, final Speed speedD) {
987 setSpeedN(speedN);
988 setSpeedE(speedE);
989 setSpeedD(speedD);
990 }
991
992 /**
993 * Gets velocity coordinates of body frame resolved along North, East, Down axes.
994 *
995 * @param result instance where velocity values will be stored.
996 */
997 public void getVelocity(final NEDVelocity result) {
998 result.setCoordinates(vn, ve, vd);
999 }
1000
1001 /**
1002 * Gets velocity coordinates of body frame resolved along North, East, Down axes.
1003 *
1004 * @return velocity coordinates.
1005 */
1006 public NEDVelocity getVelocity() {
1007 return new NEDVelocity(vn, ve, vd);
1008 }
1009
1010 /**
1011 * Sets velocity coordinates of body frame resolved along North, East, Down axes.
1012 *
1013 * @param velocity velocity to be set.
1014 */
1015 public void setVelocity(final NEDVelocity velocity) {
1016 vn = velocity.getVn();
1017 ve = velocity.getVe();
1018 vd = velocity.getVd();
1019 }
1020
1021 /**
1022 * Gets coordinate transformation matrix.
1023 *
1024 * @return coordinate transformation matrix.
1025 */
1026 @Override
1027 public CoordinateTransformation getCoordinateTransformation() {
1028 final var result = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
1029 getCoordinateTransformation(result);
1030 return result;
1031 }
1032
1033 /**
1034 * Gets coordinate transformation matrix.
1035 *
1036 * @param result instance where coordinate transformation matrix will be copied to.
1037 */
1038 @Override
1039 public void getCoordinateTransformation(final CoordinateTransformation result) {
1040 c.copyTo(result);
1041 }
1042
1043 /**
1044 * Gets coordinate transformation matrix.
1045 * This is equivalent to calling getCoordinateTransformation().getMatrix(), but more efficient
1046 *
1047 * @return coordinate transformation matrix.
1048 */
1049 @Override
1050 public Matrix getCoordinateTransformationMatrix() {
1051 Matrix result;
1052 try {
1053 result = new Matrix(CoordinateTransformation.ROWS, CoordinateTransformation.COLS);
1054 getCoordinateTransformationMatrix(result);
1055 } catch (final WrongSizeException ignore) {
1056 // never happens
1057 result = null;
1058 }
1059 return result;
1060 }
1061
1062 /**
1063 * Gets coordinate transformation matrix.
1064 * This is equivalent to calling getCoordinateTransformation().getMatrix(), but more efficient
1065 *
1066 * @param result instance where coordinate transformation matrix will be copied to.
1067 */
1068 @Override
1069 public void getCoordinateTransformationMatrix(final Matrix result) {
1070 c.matrix.copyTo(result);
1071 }
1072
1073 /**
1074 * Sets coordinate transformation matrix keeping current source and destination {@link FrameType}.
1075 * This is more efficient than getting a copy of coordinate transformation calling to
1076 * {@link #getCoordinateTransformation()}, setting coordinate matrix into copied coordinate transformation and
1077 * then setting the coordinate transformation calling
1078 * {@link #setCoordinateTransformation(CoordinateTransformation)}.
1079 *
1080 * @param matrix a 3x3 coordinate transformation matrix to be set.
1081 * @param threshold threshold to validate rotation matrix.
1082 * @throws InvalidRotationMatrixException if provided matrix is not a valid rotation matrix (3x3 and orthonormal).
1083 * @throws IllegalArgumentException if provided threshold is negative.
1084 */
1085 @Override
1086 public void setCoordinateTransformationMatrix(final Matrix matrix, final double threshold)
1087 throws InvalidRotationMatrixException {
1088 c.setMatrix(matrix,threshold);
1089 }
1090
1091 /**
1092 * Sts coordinate transformation matrix keeping current source and destination {@link FrameType}.
1093 * This is more efficient than getting a copy of coordinate transformation calling to
1094 * {@link #getCoordinateTransformation()}, setting coordinate matrix into copied coordinate transformation and
1095 * then setting the coordinate transformation calling
1096 * {@link #setCoordinateTransformation(CoordinateTransformation)}.
1097 *
1098 * @param matrix a 3x3 coordinate transformation matrix to be set.
1099 * @throws InvalidRotationMatrixException if provided matrix is not a valid rotation matrix (3x3 and orthonormal).
1100 */
1101 @Override
1102 public void setCoordinateTransformationMatrix(final Matrix matrix) throws InvalidRotationMatrixException {
1103 c.setMatrix(matrix);
1104 }
1105
1106 /**
1107 * Gets coordinate transformation as a new 3D rotation instance.
1108 * This is equivalent to calling getCoordinateTransformation().asRotation(), but more efficient.
1109 *
1110 * @return new coordinate transformation as a 3D rotation.
1111 * @throws InvalidRotationMatrixException if internal matrix cannot be converted to a 3D rotation.
1112 */
1113 @Override
1114 public Rotation3D getCoordinateTransformationRotation() throws InvalidRotationMatrixException {
1115 return c.asRotation();
1116 }
1117
1118 /**
1119 * Gets coordinate transformation as a 3D rotation.
1120 * This is equivalent to calling getCoordinateTransformation().asRotation(), but more efficient.
1121 *
1122 * @param result instance where coordinate transformation 3D rotation will be copied to.
1123 * @throws InvalidRotationMatrixException if internal matrix cannot be converted to a 3D rotation.
1124 */
1125 @Override
1126 public void getCoordinateTransformationRotation(final Rotation3D result) throws InvalidRotationMatrixException {
1127 c.asRotation(result);
1128 }
1129
1130 /**
1131 * Sets coordinate transformation from 3D rotation and keeping current source and destination {@link FrameType}.
1132 * This is more efficient than getting a copy of coordinate transformation calling to
1133 * {@link #getCoordinateTransformation()}, setting rotation into copied coordinate transformation and
1134 * then setting the coordinate transformation calling
1135 * {@link #setCoordinateTransformation(CoordinateTransformation)}.
1136 *
1137 * @param rotation set rotation into current coordinate rotation.
1138 */
1139 @Override
1140 public void setCoordinateTransformationRotation(final Rotation3D rotation) {
1141 c.fromRotation(rotation);
1142 }
1143
1144 /**
1145 * Sets coordinate transformation matrix.
1146 * Provided value must be a body to NED transformation matrix.
1147 *
1148 * @param c coordinate transformation matrix to be set.
1149 * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types are invalid.
1150 */
1151 @Override
1152 public void setCoordinateTransformation(final CoordinateTransformation c)
1153 throws InvalidSourceAndDestinationFrameTypeException {
1154 if (!isValidCoordinateTransformation(c)) {
1155 throw new InvalidSourceAndDestinationFrameTypeException();
1156 }
1157
1158 this.c = c;
1159 }
1160
1161 /**
1162 * Checks whether provided coordinate transformation matrix is valid or not.
1163 * Only body to NED transformation matrices are considered to be valid.
1164 *
1165 * @param c coordinate transformation matrix to be checked.
1166 * @return true if provided value is valid, false otherwise.
1167 */
1168 public static boolean isValidCoordinateTransformation(final CoordinateTransformation c) {
1169 return c.getSourceType() == FrameType.BODY_FRAME
1170 && c.getDestinationType() == FrameType.LOCAL_NAVIGATION_FRAME;
1171 }
1172
1173 /**
1174 * Copies this instance data into provided instance.
1175 *
1176 * @param output destination instance where data will be copied to.
1177 */
1178 public void copyTo(final NEDFrame output) {
1179 output.latitude = latitude;
1180 output.longitude = longitude;
1181 output.height = height;
1182
1183 output.vn = vn;
1184 output.ve = ve;
1185 output.vd = vd;
1186
1187 c.copyTo(output.c);
1188 }
1189
1190 /**
1191 * Copies data of provided instance into this instance.
1192 *
1193 * @param input instance to copy data from.
1194 */
1195 public void copyFrom(final NEDFrame input) {
1196 latitude = input.latitude;
1197 longitude = input.longitude;
1198 height = input.height;
1199
1200 vn = input.vn;
1201 ve = input.ve;
1202 vd = input.vd;
1203
1204 c.copyFrom(input.c);
1205 }
1206
1207 /**
1208 * Computes and returns hash code for this instance. Hash codes are almost unique
1209 * values that are useful for fast classification and storage of objects in collections.
1210 *
1211 * @return Hash code.
1212 */
1213 @Override
1214 public int hashCode() {
1215 return Objects.hash(latitude, longitude, height, vn, ve, vd, c);
1216 }
1217
1218 /**
1219 * Checks if provided object is an ECEFFrame having exactly the same contents as
1220 * this instance.
1221 *
1222 * @param obj Object to be compared.
1223 * @return true if both objects are considered to be equal, false otherwise.
1224 */
1225 @Override
1226 public boolean equals(final Object obj) {
1227 if (obj == null) {
1228 return false;
1229 }
1230 if (obj == this) {
1231 return true;
1232 }
1233 if (!(obj instanceof NEDFrame)) {
1234 return false;
1235 }
1236
1237 final NEDFrame other = (NEDFrame) obj;
1238 return equals(other);
1239 }
1240
1241 /**
1242 * Checks if provided instance has exactly the same contents as this instance.
1243 *
1244 * @param other instance to be compared.
1245 * @return true if both instances are considered to be equal, false otherwise.
1246 */
1247 public boolean equals(final NEDFrame other) {
1248 return equals(other, 0.0);
1249 }
1250
1251 /**
1252 * Checks if provided instance has contents similar to this instance up to provided
1253 * threshold value.
1254 *
1255 * @param other instance to be compared.
1256 * @param threshold maximum difference allowed between position, velocity and coordinate transformation matrix.
1257 * @return true if both instances are considered to be equal (up to provided threshold), false otherwise.
1258 */
1259 public boolean equals(final NEDFrame other, final double threshold) {
1260 if (other == null) {
1261 return false;
1262 }
1263
1264 return Math.abs(latitude - other.latitude) <= threshold
1265 && Math.abs(longitude - other.longitude) <= threshold
1266 && Math.abs(height - other.height) <= threshold
1267 && Math.abs(vn - other.vn) <= threshold
1268 && Math.abs(ve - other.ve) <= threshold
1269 && Math.abs(vd - other.vd) <= threshold
1270 && c.equals(other.c, threshold);
1271 }
1272
1273 /**
1274 * Makes a copy of this instance.
1275 *
1276 * @return a copy of this instance.
1277 * @throws CloneNotSupportedException if clone fails for some reason.
1278 */
1279 @Override
1280 protected Object clone() throws CloneNotSupportedException {
1281 final var result = (NEDFrame)super.clone();
1282 copyTo(result);
1283 return result;
1284 }
1285 }