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.units.Angle;
19 import com.irurueta.units.AngleConverter;
20 import com.irurueta.units.AngleUnit;
21 import com.irurueta.units.Distance;
22 import com.irurueta.units.DistanceConverter;
23 import com.irurueta.units.DistanceUnit;
24
25 import java.io.Serializable;
26 import java.util.Objects;
27
28 /**
29 * Contains body curvilinear position with respect Earth, expressed in latitude, longitude and height.
30 */
31 public class NEDPosition implements Serializable, Cloneable {
32
33 /**
34 * Latitude expressed in radians (rad).
35 */
36 private double latitude;
37
38 /**
39 * Longitude expressed in radians (rad).
40 */
41 private double longitude;
42
43 /**
44 * Height expressed in meters (m).
45 */
46 private double height;
47
48 /**
49 * Constructor.
50 */
51 public NEDPosition() {
52 }
53
54 /**
55 * Constructor.
56 *
57 * @param latitude latitude expressed in radians (rad).
58 * @param longitude longitude expressed in radians (rad).
59 * @param height height expressed in meters (m).
60 */
61 public NEDPosition(final double latitude, final double longitude, final double height) {
62 setCoordinates(latitude, longitude, height);
63 }
64
65 /**
66 * Constructor.
67 *
68 * @param latitude latitude.
69 * @param longitude longitude.
70 * @param height height.
71 */
72 public NEDPosition(final Angle latitude, final Angle longitude, final Distance height) {
73 setCoordinates(latitude, longitude, height);
74 }
75
76 /**
77 * Constructor.
78 *
79 * @param input position to copy data from.
80 */
81 public NEDPosition(final NEDPosition input) {
82 copyFrom(input);
83 }
84
85 /**
86 * Gets latitude expressed in radians (rad).
87 *
88 * @return latitude expressed in radians.
89 */
90 public double getLatitude() {
91 return latitude;
92 }
93
94 /**
95 * Sets latitude expressed in radians (rad).
96 *
97 * @param latitude latitude expressed in radians to be set.
98 */
99 public void setLatitude(final double latitude) {
100 this.latitude = latitude;
101 }
102
103 /**
104 * Gets longitude expressed in radians (rad).
105 *
106 * @return longitude expressed in radians.
107 */
108 public double getLongitude() {
109 return longitude;
110 }
111
112 /**
113 * Sets longitude expressed in radians (rad).
114 *
115 * @param longitude longitude expressed in radians to be set.
116 */
117 public void setLongitude(final double longitude) {
118 this.longitude = longitude;
119 }
120
121 /**
122 * Gets height expressed in meters (m).
123 *
124 * @return height expressed in meters.
125 */
126 public double getHeight() {
127 return height;
128 }
129
130 /**
131 * Sets height expressed in meters (m).
132 *
133 * @param height height expressed in meters to be set.
134 */
135 public void setHeight(final double height) {
136 this.height = height;
137 }
138
139 /**
140 * Sets curvilinear position coordinates.
141 *
142 * @param latitude latitude expressed in radians (rad) to be set.
143 * @param longitude longitude expressed in radians (rad) to be set.
144 * @param height height expressed in meters (m) to be set.
145 */
146 public void setCoordinates(final double latitude, final double longitude, final double height) {
147 this.latitude = latitude;
148 this.longitude = longitude;
149 this.height = height;
150 }
151
152 /**
153 * Gets latitude angle.
154 *
155 * @param result instance where result will be stored.
156 */
157 public void getLatitudeAngle(final Angle result) {
158 result.setValue(latitude);
159 result.setUnit(AngleUnit.RADIANS);
160 }
161
162 /**
163 * Gets latitude angle.
164 *
165 * @return latitude angle.
166 */
167 public Angle getLatitudeAngle() {
168 return new Angle(latitude, AngleUnit.RADIANS);
169 }
170
171 /**
172 * Sets latitude angle.
173 *
174 * @param latitude latitude angle to be set.
175 */
176 public void setLatitudeAngle(final Angle latitude) {
177 this.latitude = AngleConverter.convert(latitude.getValue().doubleValue(), latitude.getUnit(),
178 AngleUnit.RADIANS);
179 }
180
181 /**
182 * Gets longitude angle.
183 *
184 * @param result instance where result will be stored.
185 */
186 public void getLongitudeAngle(final Angle result) {
187 result.setValue(longitude);
188 result.setUnit(AngleUnit.RADIANS);
189 }
190
191 /**
192 * Gets longitude angle.
193 *
194 * @return longitude angle.
195 */
196 public Angle getLongitudeAngle() {
197 return new Angle(longitude, AngleUnit.RADIANS);
198 }
199
200 /**
201 * Sets longitude angle.
202 *
203 * @param longitude longitude angle to be set.
204 */
205 public void setLongitudeAngle(final Angle longitude) {
206 this.longitude = AngleConverter.convert(longitude.getValue().doubleValue(), longitude.getUnit(),
207 AngleUnit.RADIANS);
208 }
209
210 /**
211 * Gets height distance.
212 *
213 * @param result instance where result will be stored.
214 */
215 public void getHeightDistance(final Distance result) {
216 result.setValue(height);
217 result.setUnit(DistanceUnit.METER);
218 }
219
220 /**
221 * Gets height distance.
222 *
223 * @return height distance.
224 */
225 public Distance getHeightDistance() {
226 return new Distance(height, DistanceUnit.METER);
227 }
228
229 /**
230 * Sets height distance.
231 *
232 * @param height height distance to be set.
233 */
234 public void setHeightDistance(final Distance height) {
235 this.height = DistanceConverter.convert(height.getValue().doubleValue(), height.getUnit(), DistanceUnit.METER);
236 }
237
238 /**
239 * Sets curvilinear position coordinates.
240 *
241 * @param latitude latitude to be set.
242 * @param longitude longitude to be set.
243 * @param height height to be set.
244 */
245 public void setCoordinates(
246 final Angle latitude, final Angle longitude, final Distance height) {
247 setLatitudeAngle(latitude);
248 setLongitudeAngle(longitude);
249 setHeightDistance(height);
250 }
251
252 /**
253 * Copies this instance data into provided instance.
254 *
255 * @param output destination instance where data will be copied to.
256 */
257 public void copyTo(final NEDPosition output) {
258 output.latitude = latitude;
259 output.longitude = longitude;
260 output.height = height;
261 }
262
263 /**
264 * Copies data of provided instance into this instance.
265 *
266 * @param input instance to copy data from.
267 */
268 public void copyFrom(final NEDPosition input) {
269 latitude = input.latitude;
270 longitude = input.longitude;
271 height = input.height;
272 }
273
274 /**
275 * Computes and returns hash code for this instance. Hash codes are almost unique
276 * values that are useful for fast classification and storage of objects in collections.
277 *
278 * @return Hash code.
279 */
280 @Override
281 public int hashCode() {
282 return Objects.hash(latitude, longitude, height);
283 }
284
285 /**
286 * Checks if provided object is a NEDPosition having exactly the same contents as
287 * this instance.
288 *
289 * @param obj Object to be compared.
290 * @return true if both objects are considered to be equal, false otherwise.
291 */
292 @Override
293 public boolean equals(final Object obj) {
294 if (obj == null) {
295 return false;
296 }
297 if (obj == this) {
298 return true;
299 }
300 if (!(obj instanceof NEDPosition other)) {
301 return false;
302 }
303
304 return equals(other);
305 }
306
307 /**
308 * Checks if provided instance has exactly the same contents as this instance.
309 *
310 * @param other instance to be compared.
311 * @return true if both instances are considered to be equal, false otherwise.
312 */
313 public boolean equals(final NEDPosition other) {
314 return equals(other, 0.0);
315 }
316
317 /**
318 * Checks if provided instance has contents similar to this instance up to provided
319 * threshold value.
320 *
321 * @param other instance to be compared.
322 * @param threshold maximum difference allowed between position coordinates.
323 * @return true if both instances are considered to be equal (up to provided threshold),
324 * false otherwise.
325 */
326 public boolean equals(final NEDPosition other, final double threshold) {
327 if (other == null) {
328 return false;
329 }
330
331 return Math.abs(latitude - other.latitude) <= threshold
332 && Math.abs(longitude - other.longitude) <= threshold
333 && Math.abs(height - other.height) <= threshold;
334 }
335
336 /**
337 * Makes a copy of this instance.
338 *
339 * @return a copy of this instance.
340 * @throws CloneNotSupportedException if clone fails for some reason.
341 */
342 @Override
343 protected Object clone() throws CloneNotSupportedException {
344 final var result = (NEDPosition) super.clone();
345 copyTo(result);
346 return result;
347 }
348 }