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.Speed;
19 import com.irurueta.units.SpeedConverter;
20 import com.irurueta.units.SpeedUnit;
21
22 import java.io.Serializable;
23 import java.util.Objects;
24
25 /**
26 * Contains body velocity with respect Earth, resolved about north, east and down an expressed in meters per second
27 * (m/s).
28 */
29 public class NEDVelocity implements Serializable, Cloneable {
30 /**
31 * Coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
32 * resolved along North axis.
33 */
34 private double vn;
35
36 /**
37 * Coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
38 * resolved along East axis.
39 */
40 private double ve;
41
42 /**
43 * Coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
44 * resolved along Down axis.
45 */
46 private double vd;
47
48 /**
49 * Constructor.
50 */
51 public NEDVelocity() {
52 }
53
54 /**
55 * Constructor.
56 *
57 * @param vn North velocity coordinate value expressed in meters per second (m/s).
58 * @param ve East velocity coordinate value expressed in meters per second (m/s).
59 * @param vd Down velocity coordinate value expressed in meters per second (m/s).
60 */
61 public NEDVelocity(final double vn, final double ve, final double vd) {
62 setCoordinates(vn, ve, vd);
63 }
64
65 /**
66 * Constructor.
67 *
68 * @param vn North velocity coordinate value.
69 * @param ve East velocity coordinate value.
70 * @param vd Down velocity coordinate value.
71 */
72 public NEDVelocity(final Speed vn, final Speed ve, final Speed vd) {
73 setCoordinates(vn, ve, vd);
74 }
75
76 /**
77 * Constructor.
78 *
79 * @param input Body velocity to copy data from.
80 */
81 public NEDVelocity(final NEDVelocity input) {
82 copyFrom(input);
83 }
84
85 /**
86 * Gets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
87 * resolved along North axis.
88 *
89 * @return North velocity coordinate value.
90 */
91 public double getVn() {
92 return vn;
93 }
94
95 /**
96 * Sets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
97 * resolved along North axis.
98 *
99 * @param vn North velocity coordinate value.
100 */
101 public void setVn(final double vn) {
102 this.vn = vn;
103 }
104
105 /**
106 * Gets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
107 * resolved along East axis.
108 *
109 * @return East velocity coordinate value.
110 */
111 public double getVe() {
112 return ve;
113 }
114
115 /**
116 * Sets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
117 * resolved along East axis.
118 *
119 * @param ve East velocity coordinate value.
120 */
121 public void setVe(final double ve) {
122 this.ve = ve;
123 }
124
125 /**
126 * Gets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
127 * resolved along Down axis.
128 *
129 * @return Down velocity coordinate value.
130 */
131 public double getVd() {
132 return vd;
133 }
134
135 /**
136 * Sets coordinate of velocity of body frame expressed in meters per second (m/s) with respect ECEF frame and
137 * resolved along Down axis.
138 *
139 * @param vd Down velocity coordinate value.
140 */
141 public void setVd(final double vd) {
142 this.vd = vd;
143 }
144
145 /**
146 * Sets velocity coordinates of body frame expressed in meters per second (m/s) resolved along North, East, Down
147 * axes.
148 *
149 * @param vn North velocity coordinate value.
150 * @param ve East velocity coordinate value.
151 * @param vd Down velocity coordinate value.
152 */
153 public void setCoordinates(final double vn, final double ve, final double vd) {
154 this.vn = vn;
155 this.ve = ve;
156 this.vd = vd;
157 }
158
159 /**
160 * Gets norm of velocity expressed in meters per second (m/s), which represents
161 * the speed of the body.
162 *
163 * @return norm of velocity expressed in meters per second (m/s).
164 */
165 public double getNorm() {
166 return Math.sqrt(vn * vn + ve * ve + vd * vd);
167 }
168
169 /**
170 * Gets norm of velocity, which represents the speed of the body.
171 *
172 * @param result velocity norm.
173 */
174 public void getNormAsSpeed(final Speed result) {
175 result.setValue(getNorm());
176 result.setUnit(SpeedUnit.METERS_PER_SECOND);
177 }
178
179 /**
180 * Gets norm of velocity, which represents the speed of the body.
181 *
182 * @return velocity norm.
183 */
184 public Speed getNormAsSpeed() {
185 return new Speed(getNorm(), SpeedUnit.METERS_PER_SECOND);
186 }
187
188 /**
189 * Gets coordinate of velocity of body frame with respect ECEF frame and
190 * resolved along North axis.
191 *
192 * @param result instance where North velocity coordinate will be stored.
193 */
194 public void getSpeedN(final Speed result) {
195 result.setValue(vn);
196 result.setUnit(SpeedUnit.METERS_PER_SECOND);
197 }
198
199 /**
200 * Gets coordinate of velocity of body frame with respect ECEF frame and
201 * resolved along North axis.
202 *
203 * @return North velocity coordinate.
204 */
205 public Speed getSpeedN() {
206 return new Speed(vn, SpeedUnit.METERS_PER_SECOND);
207 }
208
209 /**
210 * Sets coordinate of velocity of body frame with respect ECEF frame and
211 * resolved along North axis.
212 *
213 * @param speedN North velocity coordinate to be set.
214 */
215 public void setSpeedN(final Speed speedN) {
216 vn = SpeedConverter.convert(speedN.getValue().doubleValue(), speedN.getUnit(), SpeedUnit.METERS_PER_SECOND);
217 }
218
219 /**
220 * Gets coordinate of velocity of body frame with respect ECEF frame and
221 * resolved along East axis.
222 *
223 * @param result instance where East velocity coordinate will be stored.
224 */
225 public void getSpeedE(final Speed result) {
226 result.setValue(ve);
227 result.setUnit(SpeedUnit.METERS_PER_SECOND);
228 }
229
230 /**
231 * Gets coordinate of velocity of body frame with respect ECEF frame and
232 * resolved along East axis.
233 *
234 * @return East velocity coordinate.
235 */
236 public Speed getSpeedE() {
237 return new Speed(ve, SpeedUnit.METERS_PER_SECOND);
238 }
239
240 /**
241 * Sets coordinate of velocity of body frame with respect ECEF frame and
242 * resolved along East axis.
243 *
244 * @param speedE East velocity coordinate to be set.
245 */
246 public void setSpeedE(final Speed speedE) {
247 ve = SpeedConverter.convert(speedE.getValue().doubleValue(), speedE.getUnit(), SpeedUnit.METERS_PER_SECOND);
248 }
249
250 /**
251 * Gets coordinate of velocity of body frame with respect ECEF frame and
252 * resolved along Down axis.
253 *
254 * @param result instance where Down velocity coordinate will be stored.
255 */
256 public void getSpeedD(final Speed result) {
257 result.setValue(vd);
258 result.setUnit(SpeedUnit.METERS_PER_SECOND);
259 }
260
261 /**
262 * Gets coordinate of velocity of body frame with respect ECEF frame and
263 * resolved along Down axis.
264 *
265 * @return Down velocity coordinate.
266 */
267 public Speed getSpeedD() {
268 return new Speed(vd, SpeedUnit.METERS_PER_SECOND);
269 }
270
271 /**
272 * Sets coordinate of velocity of body frame with respect ECEF frame and
273 * resolved along Down axis.
274 *
275 * @param speedD Down velocity coordinate to be set.
276 */
277 public void setSpeedD(final Speed speedD) {
278 vd = SpeedConverter.convert(speedD.getValue().doubleValue(), speedD.getUnit(), SpeedUnit.METERS_PER_SECOND);
279 }
280
281 /**
282 * Sets velocity coordinates of body frame resolved along North, East, Down
283 * axes.
284 *
285 * @param speedN North velocity coordinate.
286 * @param speedE East velocity coordinate.
287 * @param speedD Down velocity coordinate.
288 */
289 public void setCoordinates(final Speed speedN, final Speed speedE, final Speed speedD) {
290 setSpeedN(speedN);
291 setSpeedE(speedE);
292 setSpeedD(speedD);
293 }
294
295 /**
296 * Copies this instance data into provided instance.
297 *
298 * @param output destination instance where data will be copied to.
299 */
300 public void copyTo(final NEDVelocity output) {
301 output.vn = vn;
302 output.ve = ve;
303 output.vd = vd;
304 }
305
306 /**
307 * Copies data of provided instance into this instance.
308 *
309 * @param input instance to copy data from.
310 */
311 public void copyFrom(final NEDVelocity input) {
312 vn = input.vn;
313 ve = input.ve;
314 vd = input.vd;
315 }
316
317 /**
318 * Computes and returns hash code for this instance. Hash codes are almost unique
319 * values that are useful for fast classification and storage of objects in collections.
320 *
321 * @return Hash code.
322 */
323 @Override
324 public int hashCode() {
325 return Objects.hash(vn, ve, vd);
326 }
327
328 /**
329 * Checks if provided object is a BodyVelocity having exactly the same contents as
330 * this instance.
331 *
332 * @param obj Object to be compared.
333 * @return true if both objects are considered to be equal, false otherwise.
334 */
335 @Override
336 public boolean equals(final Object obj) {
337 if (obj == null) {
338 return false;
339 }
340 if (obj == this) {
341 return true;
342 }
343 if (!(obj instanceof NEDVelocity other)) {
344 return false;
345 }
346
347 return equals(other);
348 }
349
350 /**
351 * Checks if provided instance has exactly the same contents as this instance.
352 *
353 * @param other instance to be compared.
354 * @return true if both instances are considered to be equal, false otherwise.
355 */
356 public boolean equals(final NEDVelocity other) {
357 return equals(other, 0.0);
358 }
359
360 /**
361 * Checks if provided instance has contents similar to this instance up to provided
362 * threshold value.
363 *
364 * @param other instance to be compared.
365 * @param threshold maximum difference allowed between velocity coordinates.
366 * @return true if both instances are considered to be equal (up to provided threshold), false otherwise.
367 */
368 public boolean equals(final NEDVelocity other, final double threshold) {
369 if (other == null) {
370 return false;
371 }
372
373 return Math.abs(vn - other.vn) <= threshold
374 && Math.abs(ve - other.ve) <= threshold
375 && Math.abs(vd - other.vd) <= threshold;
376 }
377
378 /**
379 * Makes a copy of this instance.
380 *
381 * @return a copy of this instance.
382 * @throws CloneNotSupportedException if clone fails for some reason.
383 */
384 @Override
385 protected Object clone() throws CloneNotSupportedException {
386 final var result = (NEDVelocity) super.clone();
387 copyTo(result);
388 return result;
389 }
390 }