1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16 package com.irurueta.navigation.frames.converters;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.geometry.InvalidRotationMatrixException;
21 import com.irurueta.navigation.frames.CoordinateTransformation;
22 import com.irurueta.navigation.frames.ECEFFrame;
23 import com.irurueta.navigation.frames.FrameType;
24 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
25 import com.irurueta.navigation.frames.NEDFrame;
26 import com.irurueta.navigation.geodesic.Constants;
27
28
29
30
31
32
33
34
35
36 public class NEDtoECEFFrameConverter implements FrameConverter<NEDFrame, ECEFFrame> {
37
38
39
40
41 public static final double EARTH_EQUATORIAL_RADIUS_WGS84 = Constants.EARTH_EQUATORIAL_RADIUS_WGS84;
42
43
44
45
46 public static final double EARTH_ECCENTRICITY = Constants.EARTH_ECCENTRICITY;
47
48
49
50
51
52
53
54 @Override
55 public ECEFFrame convertAndReturnNew(final NEDFrame source) {
56 final var result = new ECEFFrame();
57 convert(source, result);
58 return result;
59 }
60
61
62
63
64
65
66 @Override
67 public void convert(final NEDFrame source, final ECEFFrame destination) {
68 convertNEDtoECEF(source, destination);
69 }
70
71
72
73
74
75
76 @Override
77 public FrameType getSourceType() {
78 return FrameType.LOCAL_NAVIGATION_FRAME;
79 }
80
81
82
83
84
85
86 @Override
87 public FrameType getDestinationType() {
88 return FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME;
89 }
90
91
92
93
94
95
96
97 public static ECEFFrame convertNEDtoECEFAndReturnNew(final NEDFrame source) {
98 final var result = new ECEFFrame();
99 convertNEDtoECEF(source, result);
100 return result;
101 }
102
103
104
105
106
107
108 @SuppressWarnings("DuplicatedCode")
109 public static void convertNEDtoECEF(final NEDFrame source, final ECEFFrame destination) {
110 try {
111 final var latitude = source.getLatitude();
112 final var longitude = source.getLongitude();
113 final var height = source.getHeight();
114
115 final var cosLat = Math.cos(latitude);
116 final var sinLat = Math.sin(latitude);
117 final var cosLong = Math.cos(longitude);
118 final var sinLong = Math.sin(longitude);
119
120
121 final var eSinLat = EARTH_ECCENTRICITY * sinLat;
122 final var eSinLat2 = eSinLat * eSinLat;
123 var re = EARTH_EQUATORIAL_RADIUS_WGS84 / Math.sqrt(1.0 - eSinLat2);
124
125
126 final var e2 = EARTH_ECCENTRICITY * EARTH_ECCENTRICITY;
127 final var x = (re + height) * cosLat * cosLong;
128 final var y = (re + height) * cosLat * sinLong;
129 final var z = ((1.0 - e2) * re + height) * sinLat;
130
131
132 final var cne = CoordinateTransformation.nedToEcefMatrix(latitude, longitude);
133
134
135 final var vn = source.getVn();
136 final var ve = source.getVe();
137 final var vd = source.getVd();
138 final var vEbn = new Matrix(NEDFrame.NUM_VELOCITY_COORDINATES, 1);
139 vEbn.setElementAtIndex(0, vn);
140 vEbn.setElementAtIndex(1, ve);
141 vEbn.setElementAtIndex(2, vd);
142
143 final var vEbe = cne.multiplyAndReturnNew(vEbn);
144 final var vx = vEbe.getElementAtIndex(0);
145 final var vy = vEbe.getElementAtIndex(1);
146 final var vz = vEbe.getElementAtIndex(2);
147
148
149 final var cbn = source.getCoordinateTransformation().getMatrix();
150 cne.multiply(cbn);
151
152 final var c = new CoordinateTransformation(cne, FrameType.BODY_FRAME,
153 FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
154
155
156 destination.setX(x);
157 destination.setY(y);
158 destination.setZ(z);
159
160 destination.setVx(vx);
161 destination.setVy(vy);
162 destination.setVz(vz);
163
164 destination.setCoordinateTransformation(c);
165
166 } catch (final WrongSizeException | InvalidRotationMatrixException |
167 InvalidSourceAndDestinationFrameTypeException ignore) {
168
169 }
170 }
171 }