1 /*
2 * Copyright (C) 2020 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.inertial.calibration;
17
18 import com.irurueta.navigation.inertial.BodyKinematics;
19 import com.irurueta.navigation.inertial.BodyKinematicsAndMagneticFluxDensity;
20 import com.irurueta.navigation.inertial.BodyMagneticFluxDensity;
21 import com.irurueta.units.Time;
22 import com.irurueta.units.TimeConverter;
23 import com.irurueta.units.TimeUnit;
24
25 import java.io.Serial;
26 import java.util.Objects;
27
28 /**
29 * Contains body kinematics describing the forces and angular rate applied to a body,
30 * along with the sensed magnetic flux density resolved around body coordinates and the
31 * corresponding timestamp when measure was made.
32 * Notice that timestamp does not need to be absolute.
33 * Usually timestamps are used in sequences of measurements of body kinematics, where
34 * the first measurement can have any timestamp value (e.g. zero), and hence the subsequent
35 * measurements will have timestamps relative to the first one.
36 */
37 public class TimedBodyKinematicsAndMagneticFluxDensity extends BodyKinematicsAndMagneticFluxDensity {
38
39 /**
40 * Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
41 * instances.
42 */
43 @Serial
44 private static final long serialVersionUID = 0L;
45
46 /**
47 * Timestamp value expressed in seconds.
48 */
49 private double timestampSeconds;
50
51 /**
52 * Constructor.
53 */
54 public TimedBodyKinematicsAndMagneticFluxDensity() {
55 super();
56 }
57
58 /**
59 * Constructor.
60 *
61 * @param kinematics body kinematics containing sensed specific force
62 * and angular rate.
63 */
64 public TimedBodyKinematicsAndMagneticFluxDensity(final BodyKinematics kinematics) {
65 super(kinematics);
66 }
67
68 /**
69 * Constructor.
70 *
71 * @param magneticFluxDensity body magnetic flux density.
72 */
73 public TimedBodyKinematicsAndMagneticFluxDensity(final BodyMagneticFluxDensity magneticFluxDensity) {
74 super(magneticFluxDensity);
75 }
76
77 /**
78 * Constructor.
79 *
80 * @param kinematics body kinematics containing sensed specific force
81 * and angular rate.
82 * @param magneticFluxDensity body magnetic flux density.
83 */
84 public TimedBodyKinematicsAndMagneticFluxDensity(
85 final BodyKinematics kinematics, final BodyMagneticFluxDensity magneticFluxDensity) {
86 super(kinematics, magneticFluxDensity);
87 }
88
89 /**
90 * Constructor.
91 *
92 * @param timestampSeconds timestamp value expressed in seconds.
93 */
94 public TimedBodyKinematicsAndMagneticFluxDensity(final double timestampSeconds) {
95 this.timestampSeconds = timestampSeconds;
96 }
97
98 /**
99 * Constructor.
100 *
101 * @param timestamp timestamp value.
102 */
103 public TimedBodyKinematicsAndMagneticFluxDensity(final Time timestamp) {
104 timestampSeconds = convertTime(timestamp);
105 }
106
107 /**
108 * Constructor.
109 *
110 * @param kinematics body kinematics containing sensed specific force
111 * and angular rate.
112 * @param timestampSeconds timestamp value expressed in seconds.
113 */
114 public TimedBodyKinematicsAndMagneticFluxDensity(final BodyKinematics kinematics, final double timestampSeconds) {
115 super(kinematics);
116 this.timestampSeconds = timestampSeconds;
117 }
118
119 /**
120 * Constructor.
121 *
122 * @param kinematics body kinematics containing sensed specific force
123 * and angular rate.
124 * @param timestamp timestamp value.
125 */
126 public TimedBodyKinematicsAndMagneticFluxDensity(final BodyKinematics kinematics, final Time timestamp) {
127 super(kinematics);
128 timestampSeconds = convertTime(timestamp);
129 }
130
131 /**
132 * Constructor.
133 *
134 * @param magneticFluxDensity body magnetic flux density.
135 * @param timestampSeconds timestamp value expressed in seconds.
136 */
137 public TimedBodyKinematicsAndMagneticFluxDensity(
138 final BodyMagneticFluxDensity magneticFluxDensity, final double timestampSeconds) {
139 super(magneticFluxDensity);
140 this.timestampSeconds = timestampSeconds;
141 }
142
143 /**
144 * Constructor.
145 *
146 * @param magneticFluxDensity body magnetic flux density.
147 * @param timestamp timestamp value.
148 */
149 public TimedBodyKinematicsAndMagneticFluxDensity(
150 final BodyMagneticFluxDensity magneticFluxDensity, final Time timestamp) {
151 super(magneticFluxDensity);
152 timestampSeconds = convertTime(timestamp);
153 }
154
155 /**
156 * Constructor.
157 *
158 * @param kinematics body kinematics containing sensed specific force
159 * and angular rate.
160 * @param magneticFluxDensity body magnetic flux density.
161 * @param timestampSeconds timestamp value expressed in seconds.
162 */
163 public TimedBodyKinematicsAndMagneticFluxDensity(
164 final BodyKinematics kinematics, final BodyMagneticFluxDensity magneticFluxDensity,
165 final double timestampSeconds) {
166 super(kinematics, magneticFluxDensity);
167 this.timestampSeconds = timestampSeconds;
168 }
169
170
171 /**
172 * Constructor.
173 *
174 * @param kinematics body kinematics containing sensed specific force
175 * and angular rate.
176 * @param magneticFluxDensity body magnetic flux density.
177 * @param timestamp timestamp value.
178 */
179 public TimedBodyKinematicsAndMagneticFluxDensity(
180 final BodyKinematics kinematics, final BodyMagneticFluxDensity magneticFluxDensity, final Time timestamp) {
181 super(kinematics, magneticFluxDensity);
182 timestampSeconds = convertTime(timestamp);
183 }
184
185 /**
186 * Constructor.
187 *
188 * @param input instance to copy data from.
189 */
190 public TimedBodyKinematicsAndMagneticFluxDensity(final TimedBodyKinematicsAndMagneticFluxDensity input) {
191 copyFrom(input);
192 }
193
194 /**
195 * Gets timestamp value expressed in seconds.
196 *
197 * @return timestamp value expressed in seconds.
198 */
199 public double getTimestampSeconds() {
200 return timestampSeconds;
201 }
202
203 /**
204 * Sets timestamp value expressed in seconds.
205 *
206 * @param timestampSeconds timestamp value expressed in seconds.
207 */
208 public void setTimestampSeconds(final double timestampSeconds) {
209 this.timestampSeconds = timestampSeconds;
210 }
211
212 /**
213 * Gets timestamp value.
214 *
215 * @return a new timestamp instance.
216 */
217 public Time getTimestamp() {
218 return new Time(timestampSeconds, TimeUnit.SECOND);
219 }
220
221 /**
222 * Gets timestamp value.
223 *
224 * @param result instance where result data will be stored.
225 */
226 public void getTimestamp(final Time result) {
227 result.setValue(timestampSeconds);
228 result.setUnit(TimeUnit.SECOND);
229 }
230
231 /**
232 * Sets timestamp.
233 *
234 * @param timestamp timestamp to be set.
235 */
236 public void setTimestamp(final Time timestamp) {
237 timestampSeconds = convertTime(timestamp);
238 }
239
240 /**
241 * Gets a timed body kinematics instance containing current body kinematics
242 * and timestamp.
243 *
244 * @return a timed body kinematics
245 */
246 public TimedBodyKinematics getTimedKinematics() {
247 return new TimedBodyKinematics(getKinematics(), timestampSeconds);
248 }
249
250 /**
251 * Gets a timed body kinematics instance containing current body kinematics
252 * and timestamp.
253 *
254 * @param result instance where result will be stored.
255 */
256 public void getTimedKinematics(final TimedBodyKinematics result) {
257 result.setKinematics(getKinematics());
258 result.setTimestampSeconds(timestampSeconds);
259 }
260
261 /**
262 * Sets data from provided timed body kinematics.
263 *
264 * @param timedKinematics timed body kinematics.
265 */
266 public void setTimedKinematics(final TimedBodyKinematics timedKinematics) {
267 setKinematics(timedKinematics.getKinematics());
268 timestampSeconds = timedKinematics.getTimestampSeconds();
269 }
270
271 /**
272 * Copies data of provided instance into this instance.
273 *
274 * @param input instance to copy data from.
275 */
276 public void copyFrom(final TimedBodyKinematicsAndMagneticFluxDensity input) {
277 super.copyFrom(input);
278 timestampSeconds = input.timestampSeconds;
279 }
280
281 /**
282 * Copies this instance data into provided instance.
283 *
284 * @param output destination instance where data will be copied to.
285 */
286 public void copyTo(final TimedBodyKinematicsAndMagneticFluxDensity output) {
287 output.copyFrom(this);
288 }
289
290 /**
291 * Computes and returns hash code for this instance. Hash codes are almost unique
292 * values that are useful for fast classification and storage of objects in collections.
293 *
294 * @return Hash code.
295 */
296 @Override
297 public int hashCode() {
298 return Objects.hash(super.hashCode(), timestampSeconds);
299 }
300
301 /**
302 * Checks if provided instance has exactly the same contents as this instance.
303 *
304 * @param other instance to be compared.
305 * @return true if both instances are considered to be equal, false otherwise.
306 */
307 public boolean equals(final TimedBodyKinematicsAndMagneticFluxDensity other) {
308 return equals(other, 0.0);
309 }
310
311 /**
312 * Checks if provided instance has contents similar to this instance up to provided
313 * threshold value.
314 *
315 * @param other instance to be compared.
316 * @param threshold maximum allowed difference between kinematics and timestamp values.
317 * @return true if both instances are considered to be equal (up to provided
318 * threshold), false otherwise.
319 */
320 public boolean equals(final TimedBodyKinematicsAndMagneticFluxDensity other, final double threshold) {
321 return super.equals(other, threshold) && Math.abs(other.timestampSeconds - timestampSeconds) <= threshold;
322 }
323
324 /**
325 * Checks if provided object is a TimedBodyKinematics instance having exactly the same
326 * contents as this instance.
327 *
328 * @param obj object to be compared.
329 * @return true if both objects are considered to be equal, false otherwise.
330 */
331 @Override
332 public boolean equals(final Object obj) {
333 if (this == obj) {
334 return true;
335 }
336 if (obj == null || getClass() != obj.getClass()) {
337 return false;
338 }
339 final var other = (TimedBodyKinematicsAndMagneticFluxDensity) obj;
340 return equals(other);
341 }
342
343 /**
344 * Makes a copy of this instance.
345 *
346 * @return a copy of this instance.
347 * @throws CloneNotSupportedException if clone fails for some reason.
348 */
349 @Override
350 protected Object clone() throws CloneNotSupportedException {
351 final var result = (TimedBodyKinematicsAndMagneticFluxDensity) super.clone();
352 copyTo(result);
353 return result;
354 }
355
356 /**
357 * Converts provided time instance to seconds.
358 *
359 * @param time timestamp to be converted.
360 * @return converted value expressed in seconds.
361 */
362 private static double convertTime(final Time time) {
363 return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
364 }
365 }