FrameBodyMagneticFluxDensity.java
/*
* Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.irurueta.navigation.inertial.calibration;
import com.irurueta.navigation.frames.ECEFFrame;
import com.irurueta.navigation.frames.NEDFrame;
import com.irurueta.navigation.frames.converters.ECEFtoNEDFrameConverter;
import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
import com.irurueta.navigation.inertial.BodyMagneticFluxDensity;
import com.irurueta.navigation.inertial.wmm.WMMEarthMagneticFluxDensityEstimator;
import java.io.Serial;
import java.io.Serializable;
import java.util.Date;
import java.util.GregorianCalendar;
import java.util.Objects;
/**
* Contains a body magnetic flux density along with the corresponding frame
* (position, orientation and velocity) where the measurement was made.
*/
public class FrameBodyMagneticFluxDensity implements Serializable, Cloneable {
/**
* Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
* instances.
*/
@Serial
private static final long serialVersionUID = 0L;
/**
* Current body magnetic flux density. Contains magnetometer measurements.
*/
private BodyMagneticFluxDensity magneticFluxDensity;
/**
* Contains current body position, velocity (which will typically be zero)
* and orientation resolved around ECEF axes.
*/
private ECEFFrame frame;
/**
* Contains year expressed in decimal format.
*/
private double year;
/**
* Constructor.
*/
public FrameBodyMagneticFluxDensity() {
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
*/
public FrameBodyMagneticFluxDensity(final BodyMagneticFluxDensity magneticFluxDensity) {
this.magneticFluxDensity = magneticFluxDensity;
}
/**
* Constructor.
*
* @param frame current ECEF frame associated to measurement.
*/
public FrameBodyMagneticFluxDensity(final ECEFFrame frame) {
this.frame = frame;
}
/**
* Constructor.
*
* @param frame current NED frame associated to measurement. Internally it
* will be converted to its corresponding ECEF frame.
*/
public FrameBodyMagneticFluxDensity(final NEDFrame frame) {
setNedFrame(frame);
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param frame current ECEF frame associated to measurement.
*/
public FrameBodyMagneticFluxDensity(final BodyMagneticFluxDensity magneticFluxDensity, final ECEFFrame frame) {
this(magneticFluxDensity);
this.frame = frame;
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param frame current NED frame associated to measurement.
* Internally it will be converted to its
* corresponding ECEF frame.
*/
public FrameBodyMagneticFluxDensity(final BodyMagneticFluxDensity magneticFluxDensity, final NEDFrame frame) {
this(magneticFluxDensity);
setNedFrame(frame);
}
/**
* Constructor.
*
* @param year time expressed as decimal year.
*/
public FrameBodyMagneticFluxDensity(final double year) {
this.year = year;
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param year time expressed as decimal year.
*/
public FrameBodyMagneticFluxDensity(final BodyMagneticFluxDensity magneticFluxDensity, final double year) {
this.magneticFluxDensity = magneticFluxDensity;
this.year = year;
}
/**
* Constructor.
*
* @param frame current ECEF frame associated to measurement.
* @param year time expressed as decimal year.
*/
public FrameBodyMagneticFluxDensity(final ECEFFrame frame, final double year) {
this.frame = frame;
this.year = year;
}
/**
* Constructor.
*
* @param frame current NED frame associated to measurement. Internally it
* will be converted to its corresponding ECEF frame.
* @param year time expressed as decimal year.
*/
public FrameBodyMagneticFluxDensity(final NEDFrame frame, final double year) {
setNedFrame(frame);
this.year = year;
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param frame current ECEF frame associated to measurement.
* @param year time expressed as decimal year.
*/
public FrameBodyMagneticFluxDensity(
final BodyMagneticFluxDensity magneticFluxDensity, final ECEFFrame frame, final double year) {
this(magneticFluxDensity);
this.frame = frame;
this.year = year;
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param frame current NED frame associated to measurement.
* Internally it will be converted to its
* corresponding ECEF frame.
* @param year time expressed as decimal year.
*/
public FrameBodyMagneticFluxDensity(
final BodyMagneticFluxDensity magneticFluxDensity, final NEDFrame frame, final double year) {
this(magneticFluxDensity);
setNedFrame(frame);
this.year = year;
}
/**
* Constructor.
*
* @param time a timestamp.
*/
public FrameBodyMagneticFluxDensity(final Date time) {
this(convertTime(time));
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param time a timestamp.
*/
public FrameBodyMagneticFluxDensity(final BodyMagneticFluxDensity magneticFluxDensity, final Date time) {
this(magneticFluxDensity, convertTime(time));
}
/**
* Constructor.
*
* @param frame current ECEF frame associated to measurement.
* @param time a timestamp.
*/
public FrameBodyMagneticFluxDensity(final ECEFFrame frame, final Date time) {
this(frame, convertTime(time));
}
/**
* Constructor.
*
* @param frame current NED frame associated to measurement. Internally it
* will be converted to its corresponding ECEF frame.
* @param time a timestamp.
*/
public FrameBodyMagneticFluxDensity(final NEDFrame frame, final Date time) {
this(frame, convertTime(time));
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param frame current ECEF frame associated to measurement.
* @param time a timestamp.
*/
public FrameBodyMagneticFluxDensity(
final BodyMagneticFluxDensity magneticFluxDensity, final ECEFFrame frame, final Date time) {
this(magneticFluxDensity, frame, convertTime(time));
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param frame current NED frame associated to measurement.
* Internally it will be converted to its
* corresponding ECEF frame.
* @param time a timestamp.
*/
public FrameBodyMagneticFluxDensity(
final BodyMagneticFluxDensity magneticFluxDensity, final NEDFrame frame, final Date time) {
this(magneticFluxDensity, frame, convertTime(time));
}
/**
* Constructor.
*
* @param calendar calendar containing a timestamp.
*/
public FrameBodyMagneticFluxDensity(final GregorianCalendar calendar) {
this(convertTime(calendar));
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param calendar calendar containing a timestamp.
*/
public FrameBodyMagneticFluxDensity(
final BodyMagneticFluxDensity magneticFluxDensity, final GregorianCalendar calendar) {
this(magneticFluxDensity, convertTime(calendar));
}
/**
* Constructor.
*
* @param frame current ECEF frame associated to measurement.
* @param calendar calendar containing a timestamp.
*/
public FrameBodyMagneticFluxDensity(final ECEFFrame frame, final GregorianCalendar calendar) {
this(frame, convertTime(calendar));
}
/**
* Constructor.
*
* @param frame current NED frame associated to measurement. Internally it
* will be converted to its corresponding ECEF frame.
* @param calendar calendar containing a timestamp.
*/
public FrameBodyMagneticFluxDensity(final NEDFrame frame, final GregorianCalendar calendar) {
this(frame, convertTime(calendar));
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param frame current ECEF frame associated to measurement.
* @param calendar calendar containing a timestamp.
*/
public FrameBodyMagneticFluxDensity(
final BodyMagneticFluxDensity magneticFluxDensity, final ECEFFrame frame,
final GregorianCalendar calendar) {
this(magneticFluxDensity, frame, convertTime(calendar));
}
/**
* Constructor.
*
* @param magneticFluxDensity current body magnetic flux density.
* @param frame current NED frame associated to measurement.
* Internally it will be converted to its
* corresponding ECEF frame.
* @param calendar calendar containing a timestamp.
*/
public FrameBodyMagneticFluxDensity(
final BodyMagneticFluxDensity magneticFluxDensity, final NEDFrame frame,
final GregorianCalendar calendar) {
this(magneticFluxDensity, frame, convertTime(calendar));
}
/**
* Constructor.
*
* @param input instance to copy data from.
*/
public FrameBodyMagneticFluxDensity(final FrameBodyMagneticFluxDensity input) {
copyFrom(input);
}
/**
* Gets current body magnetic flux density. Contains magnetometer
* measurements.
*
* @return current body magnetic flux density.
*/
public BodyMagneticFluxDensity getMagneticFluxDensity() {
return magneticFluxDensity;
}
/**
* Sets current body magnetic flux density. Contains magnetometer
* measurements.
*
* @param magneticFluxDensity current body magnetic flux density.
*/
public void setMagneticFluxDensity(final BodyMagneticFluxDensity magneticFluxDensity) {
this.magneticFluxDensity = magneticFluxDensity;
}
/**
* Gets current body position (which will typically remain constant),
* velocity(which will typically be zero) and orientation (which usually
* changes with each measurement to perform calibration of a single device)
* resolved around ECEF axes associated to body magnetic flux density
* measurement.
*
* @return current ECEF frame associated to body magnetic flux density
* measurement or null if not available.
*/
public ECEFFrame getFrame() {
return frame;
}
/**
* Sets current body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually
* changes with each measurement to perform calibration of a single
* device) resolved around ECEF axes associated to body magnetic flux
* density measurement.
*
* @param frame current ECEF frame.
*/
public void setFrame(final ECEFFrame frame) {
this.frame = frame;
}
/**
* Gets current body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually
* changes with each measurement to perform calibration of a single
* device) resolved around NED axes associated to body magnetic flux
* density measurement.
*
* @return current NED frame associated to body magnetic flux density
* measurement or null if not available.
*/
public NEDFrame getNedFrame() {
return frame != null ? ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame) : null;
}
/**
* Gets current body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually
* changes with each measurement to perform calibration of a single
* device) resolved around NED axes associated to body magnetic flux
* density measurement.
*
* @param result instance where result data will be stored if available.
* @return true if result instance was updated, false otherwise.
*/
public boolean getNedFrame(final NEDFrame result) {
if (frame != null) {
ECEFtoNEDFrameConverter.convertECEFtoNED(frame, result);
return true;
} else {
return false;
}
}
/**
* Sets current body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually
* changes with each measurement to perform calibration of a single
* device) resolved around NED axes associated to body magnetic flux
* density measurement.
* <p>
* This method will internally store the corresponding ECEF frame to provided
* NED frame value.
*
* @param nedFrame current NED frame associated to body magnetic flux
* density measurement to be set.
*/
public void setNedFrame(final NEDFrame nedFrame) {
if (nedFrame != null) {
if (frame != null) {
NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
} else {
frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
}
} else {
frame = null;
}
}
/**
* Gets year expressed in decimal format.
*
* @return year expressed in decimal format.
*/
public double getYear() {
return year;
}
/**
* Sets year expressed in decimal format.
*
* @param year year expressed in decimal format.
*/
public void setYear(final double year) {
this.year = year;
}
/**
* Sets decimal year from provided date instance.
*
* @param date a date instance containing a timestamp.
*/
public void setTime(final Date date) {
year = convertTime(date);
}
/**
* Sets decimal year from provided calendar instance.
*
* @param calendar a calendar instance containing a timestamp.
*/
public void setTime(final GregorianCalendar calendar) {
year = convertTime(calendar);
}
/**
* Converts a time instant contained ina date object to a
* decimal year.
*
* @param date a time instance to be converted.
* @return converted value expressed in decimal years.
*/
public static double convertTime(final Date date) {
final var calendar = new GregorianCalendar();
calendar.setTime(date);
return convertTime(calendar);
}
/**
* Converts a time instant contained in a gregorian calendar to a
* decimal year.
*
* @param calendar calendar containing a specific instant to be
* converted.
* @return converted value expressed in decimal years.
*/
public static double convertTime(final GregorianCalendar calendar) {
return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
}
/**
* Copies data of provided instance into this instance.
*
* @param input instance to copy dara from.
*/
public void copyFrom(final FrameBodyMagneticFluxDensity input) {
if (input.magneticFluxDensity != null) {
if (magneticFluxDensity == null) {
magneticFluxDensity = new BodyMagneticFluxDensity(input.magneticFluxDensity);
} else {
magneticFluxDensity.copyFrom(input.magneticFluxDensity);
}
} else {
magneticFluxDensity = null;
}
if (input.frame != null) {
if (frame == null) {
frame = new ECEFFrame(input.frame);
} else {
frame.copyFrom(input.frame);
}
} else {
frame = null;
}
year = input.year;
}
/**
* Copies this instance data into provided instance.
*
* @param output destination instance where data will be copied to.
*/
public void copyTo(final FrameBodyMagneticFluxDensity output) {
output.copyFrom(this);
}
/**
* Computes and returns hash code for this instance. Hash codes are almost unique
* values that are useful for fast classification and storage of objects in collections.
*
* @return Hash code.
*/
@Override
public int hashCode() {
return Objects.hash(magneticFluxDensity, frame, year);
}
/**
* Checks if provided instance has exactly the same contents as this instance.
*
* @param other instance to be compared.
* @return true if both instances are considered to be equal, false otherwise.
*/
public boolean equals(final FrameBodyMagneticFluxDensity other) {
return equals(other, 0.0);
}
/**
* Checks if provided instance has contents similar to this instance up to provided
* threshold value.
*
* @param other instance to be compared.
* @param threshold maximum allowed difference between magnetic flux density and
* frame values.
* @return true if both instances are considered to be equal (up to provided
* threshold), false otherwise.
*/
public boolean equals(final FrameBodyMagneticFluxDensity other, final double threshold) {
if (other == null) {
return false;
}
return ((other.magneticFluxDensity == null && magneticFluxDensity == null)
|| (magneticFluxDensity != null && magneticFluxDensity.equals(other.magneticFluxDensity, threshold)))
&& ((other.frame == null && frame == null)
|| (frame != null && frame.equals(other.frame, threshold)))
&& Math.abs(other.year - year) <= threshold;
}
/**
* Checks if provided object is a FrameBodyMagneticFluxDensity instance
* having exactly the same contents as this instance.
*
* @param obj object to be compared.
* @return true if both objects are considered to be equal, false otherwise.
*/
@Override
public boolean equals(final Object obj) {
if (this == obj) {
return true;
}
if (obj == null || getClass() != obj.getClass()) {
return false;
}
final var other = (FrameBodyMagneticFluxDensity) obj;
return equals(other);
}
/**
* Makes a copy of this instance.
*
* @return a copy of this instance.
* @throws CloneNotSupportedException if clone fails for some reason.
*/
@Override
protected Object clone() throws CloneNotSupportedException {
final var result = (FrameBodyMagneticFluxDensity) super.clone();
copyTo(result);
return result;
}
}