util/src/dr/utils.h

Go to the documentation of this file.

// === utils.h =========================================================================================================
//                                               Sen Infrastructure
//                   Released under the Apache License v2.0 (SPDX-License-Identifier Apache-2.0).
//                                    See the LICENSE.txt file for more information.
//                   © Airbus SAS, Airbus Helicopters, and Airbus Defence and Space SAU/GmbH/SAS.
// =====================================================================================================================
 
#ifndef SEN_LIBS_UTIL_SRC_DR_UTILS_H
#define SEN_LIBS_UTIL_SRC_DR_UTILS_H
 
#include "mat3.h"
#include "quat.h"
 
// sen
#include "sen/util/dr/algorithms.h"
 
namespace sen::util
{
 
//--------------------------------------------------------------------------------------------------------------
// Helpers
//--------------------------------------------------------------------------------------------------------------

[[nodiscard]] Location toWorldLocation(const Vec3d& value);

[[nodiscard]] Vec3d fromWorldLocation(const Location& value);

[[nodiscard]] Vec3d fromVelocity(const Velocity& value);

[[nodiscard]] Velocity toVelocity(const Vec3d& value);

[[nodiscard]] Vec3d fromAcceleration(const Acceleration& value);

[[nodiscard]] Acceleration toAcceleration(const Vec3d& value);

[[nodiscard]] AngularAcceleration toAngularAcceleration(const Vec3d& value);

[[nodiscard]] Vec3d fromAngularVelocity(const AngularVelocity& value);
 
[[nodiscard]] Vec3d fromAngularAcceleration(const AngularAcceleration& value);

[[nodiscard]] AngularVelocity toAngularVelocity(const Vec3d& value);

[[nodiscard]] Orientation toOrientation(const Vec3d& value);

[[nodiscard]] Vec3d fromOrientationToVec(const Orientation& value);

[[nodiscard]] Quatd fromOrientationToQuat(const Orientation& value);

[[nodiscard]] Vec3d nedToEcef(const Vec3d& value, const GeodeticWorldLocation& latLonAlt) noexcept;

[[nodiscard]] Vec3d ecefToNed(const Vec3d& value, const GeodeticWorldLocation& latLonAlt) noexcept;

[[nodiscard]] Vec3d bodyToNed(const Vec3d& value, const Orientation& orientationNed) noexcept;

[[nodiscard]] Vec3d nedToBody(const Vec3d& value, const Orientation& orientationNed) noexcept;

[[nodiscard]] f64 toRad(f64 value) noexcept;

[[nodiscard]] f64 toDeg(f64 value) noexcept;

[[nodiscard]] Mat3d makeR1(const Vec3d& omega, f64 delta);

[[nodiscard]] Mat3d makeR2(const Vec3d& omega, f64 delta);

[[nodiscard]] Vec3d bodyToWorld(const Vec3d& value, const Orientation& bodyEulerAngles) noexcept;

[[nodiscard]] Vec3d worldToBody(const Vec3d& value, const Orientation& bodyEulerAngles) noexcept;

[[nodiscard]] Location extrapolateLocationWorld(const Situation& value, sen::Duration delta) noexcept;

[[nodiscard]] Location extrapolateLocationBody(const Situation& value, sen::Duration delta) noexcept;

[[nodiscard]] Velocity extrapolateVelocity(const Velocity& value,
                                           const Acceleration& acceleration,
                                           sen::Duration delta) noexcept;

[[nodiscard]] Orientation extrapolateOrientation(const Orientation& value,
                                                 sen::Duration delta,
                                                 const AngularVelocity& angularVelocity,
                                                 const AngularAcceleration& angularAcceleration = {}) noexcept;

[[nodiscard]] std::array<Vec3d, 3U> getNedTrihedron(const GeodeticWorldLocation& location);

[[nodiscard]] Orientation eulerAnglesFromTrihedrons(const Vec3d& xi,
                                                    const Vec3d& yi,
                                                    const Vec3d& zi,
                                                    const Vec3d& xf,
                                                    const Vec3d& yf);

struct HaversineData
{
  LengthMeters distance;
  AngleRadians heading;
  AngleRadians pitch;
};

[[nodiscard]] HaversineData haversine(const GeodeticWorldLocation& start, const GeodeticWorldLocation& end);

[[nodiscard]] f64 hypot(f64 var1, f64 var2) noexcept;

[[nodiscard]] bool hasVelocity(const Velocity& velocity) noexcept;

[[nodiscard]] bool hasAcceleration(const Acceleration& acceleration) noexcept;

[[nodiscard]] bool hasAngularVelocity(const AngularVelocity& omega) noexcept;
 
}  // namespace sen::util
 
#endif  // SEN_LIBS_UTIL_SRC_DR_UTILS_H