ADD: added eulerconversions for the heading pitch and roll; ADD: added a oriatation class

CMakeLists.txt

@@ -59,6 +59,12 @@ add_library(SimCore STATIC
     include/SimCore/Position.hpp
     src/SimCore/Position.cpp
 
+    include/SimCore/Orientation.hpp
+    src/SimCore/Orientation.cpp
+
+    include/SimCore/EulerConversion.hpp
+    src/SimCore/EulerConversion.cpp
+
     include/SimCore/SafeMap.hpp
     src/SimCore/SafeMap.cpp
 
@@ -125,6 +131,12 @@ IF (${TEST_SIMCORE_LIBRARY})
   target_link_libraries(test_PositionClass Catch2::Catch2 SimCore eigen loguru)
   catch_discover_tests(test_PositionClass)
 
+  add_executable(test_OrientationClass tests/test_OrientationClass.cpp)
+  target_link_libraries(test_OrientationClass Catch2::Catch2 SimCore eigen loguru)
+  catch_discover_tests(test_OrientationClass)
+
+  
+
   add_executable(test_GroundTruthTrackClass tests/test_GroundTruthTrackClass.cpp)
   target_link_libraries(test_GroundTruthTrackClass Catch2::Catch2 SimCore eigen loguru)
   catch_discover_tests(test_GroundTruthTrackClass)

include/SimCore/EulerConversion.hpp

@@ -0,0 +1,103 @@
+#pragma once
+#include <cmath>
+#include <math.h>
+
+namespace SimCore {
+
+    class EulerConversions{
+    public: 
+    
+    /**
+     * Gets a degree heading for an entity based on euler angles. All angular
+     * values passed in must be in radians.
+     *
+     * @param lat Entity's latitude, IN RADIANS
+     * @param lon Entity's longitude, IN RADIANS
+     * @param psi Psi angle, IN RADIANS
+     * @param theta Theta angle, IN RADIANS
+     * @return the heading, in degrees, with 0 being north, positive angles
+     * going clockwise, and negative angles going counterclockwise (i.e., 90 deg
+     * is east, -90 is west)
+     */
+    static double getOrientationFromEuler(double lat, double lon, double psi, double theta);
+
+    /**
+     * Gets a degree pitch for an entity based on euler angles. All angular
+     * values passed in must be in radians.
+     *
+     * @param lat Entity's latitude, IN RADIANS
+     * @param lon Entity's longitude, IN RADIANS
+     * @param psi Psi angle, IN RADIANS
+     * @param theta Theta angle, IN RADIANS
+     * @return the pitch, in degrees, with 0 being level. A negative values is
+     * when the entity's nose is pointing downward, positive value is when the
+     * entity's nose is pointing upward.
+     */
+     static double getPitchFromEuler(double lat, double lon, double psi, double theta);
+
+/**
+     * Gets the degree roll for an entity based on euler angles. All angular
+     * values passed in must be in radians.
+     *
+     * @param lat Entity's latitude, IN RADIANS
+     * @param lon Entity's longitude, IN RADIANS
+     * @param psi Psi angle, IN RADIANS
+     * @param theta Theta angle, IN RADIANS
+     * @param phi Phi angle, IN RADIANS
+     * @return the roll, in degrees, with 0 being level flight, + roll is
+     * clockwise when looking out the front of the entity.
+     */
+     static double getRollFromEuler(double lat, double lon, double psi, double theta, double phi);
+
+    /**
+     * Gets the Euler Theta value (in radians) from position and Tait-Brayn yaw
+     * and pitch angles
+     *
+     * @param lat Entity's latitude, IN RADIANS
+     * @param lon Entity's longitude, IN RADIANS
+     * @param yaw entity's yaw angle (also know as the entity's bearing or
+     * heading angle), in degrees
+     * @param pitch entity's pitch angle, in degrees
+     * @return the Theta value in radians
+     */
+     static double getThetaFromTaitBryanAngles(double lat, double lon, double yaw, double pitch);
+
+    /**
+     * Gets the Euler Psi value (in radians) from position and Tait-Brayn yaw
+     * and pitch angles
+     *
+     * @param lat Entity's latitude, IN RADIANS
+     * @param lon Entity's longitude, IN RADIANS
+     * @param yaw ettity's yaw angle (also know as the entity's bearing or
+     * heading angle), in degrees
+     * @param pitch entity's pitch angle, in degrees
+     * @return the Psi value in radians
+     */
+     static double getPsiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch);
+
+    /**
+     * Gets the Euler Phi value (in radians) from position and Tait-Brayn yaw,
+     * pitch and roll angles
+     *
+     * @param lat Entity's latitude, IN RADIANS
+     * @param lon Entity's longitude, IN RADIANS
+     * @param yaw yaw angle (also know as the entity's bearing or heading
+     * angle), in degrees
+     * @param pitch entity's pitch angle, in degrees
+     * @param roll entity's roll angle (0 is level flight, + roll is clockwise
+     * looking out the nose), in degrees
+     * @return the Phi value in radians
+     */
+     static double getPhiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch, double roll);
+
+    
+    private:
+
+    static double toDegrees(double radian);
+
+    constexpr static const  double _toDegrees = 57.2957795131;
+    constexpr static double _toRadians = 0.01745329252;
+
+};
+}    
+

include/SimCore/Orientation.hpp

@@ -0,0 +1,41 @@
+#pragma once 
+
+#include <Eigen/Core>
+
+
+namespace SimCore 
+{
+    class Orientation
+    {
+
+        public: 
+        Orientation(double heading = 0, double pitch = 0, double roll = 0);
+
+        Orientation(Eigen::Vector3d angles, double lat, double lon);
+
+        void setHeading(double heading);
+        double getHeading();
+
+        void setPitch(double pitch);
+        double getPitch();
+
+        void setRoll(double roll);
+        double getRoll();
+
+        Eigen::Vector3d getEulerAngles(double lat ,double lon);
+
+
+        private:
+            
+
+             Eigen::Vector3d eulerAngels_;
+
+             double heading_;
+             double pitch_;
+             double roll_;
+
+
+    };
+
+
+}

include/SimCore/SimCore.hpp

@@ -19,6 +19,13 @@ Y,
 Z
 };
 
+enum EulerAngels : std::uint8_t
+{
+PSI = 0,  
+THETA, 
+PHI    
+};
+
 
 enum  ObjectSource : bool{
 

src/SimCore/EulerConversion.cpp

@@ -0,0 +1,145 @@
+#include <SimCore/EulerConversion.hpp>
+
+
+
+
+
+namespace SimCore 
+{
+ double EulerConversions::getOrientationFromEuler(double lat, double lon, double psi, double theta) {
+        double sinlat = sin(lat);
+        double sinlon = sin(lon);
+        double coslon = cos(lon);
+        double coslat = cos(lat);
+        double sinsin = sinlat * sinlon;
+
+        double cosTheta = cos(theta);
+        double cosPsi = cos(psi);
+        double sinPsi = sin(psi);
+        double sinTheta = sin(theta);
+
+        double cosThetaCosPsi = cosTheta * cosPsi;
+        double cosThetaSinPsi = cosTheta * sinPsi;
+        double sincos = sinlat * coslon;
+
+        double b11 = -sinlon * cosThetaCosPsi + coslon * cosThetaSinPsi;
+        double b12 = -sincos * cosThetaCosPsi - sinsin * cosThetaSinPsi - coslat * sinTheta;
+
+        return toDegrees(atan2(b11, b12));//range is -pi to pi
+    };
+
+
+
+    double EulerConversions::getPitchFromEuler(double lat, double lon, double psi, double theta) {
+        double sinlat = sin(lat);
+        double sinlon = sin(lon);
+        double coslon = cos(lon);
+        double coslat = cos(lat);
+        double cosLatCosLon = coslat * coslon;
+        double cosLatSinLon = coslat * sinlon;
+
+        double cosTheta = cos(theta);
+        double cosPsi = cos(psi);
+        double sinPsi = sin(psi);
+        double sinTheta = sin(theta);
+
+        return toDegrees(asin(cosLatCosLon * cosTheta * cosPsi + cosLatSinLon * cosTheta * sinPsi - sinlat * sinTheta));
+    };
+
+
+
+    double EulerConversions::getRollFromEuler(double lat, double lon, double psi, double theta, double phi) {
+        double sinlat = sin(lat);
+        double sinlon = sin(lon);
+        double coslon = cos(lon);
+        double coslat = cos(lat);
+        double cosLatCosLon = coslat * coslon;
+        double cosLatSinLon = coslat * sinlon;
+
+        double cosTheta = cos(theta);
+        double sinTheta = sin(theta);
+        double cosPsi = cos(psi);
+        double sinPsi = sin(psi);
+        double sinPhi = sin(phi);
+        double cosPhi = cos(phi);
+
+        double sinPhiSinTheta = sinPhi * sinTheta;
+        double cosPhiSinTheta = cosPhi * sinTheta;
+
+        double b23 = cosLatCosLon * (-cosPhi * sinPsi + sinPhiSinTheta * cosPsi)
+                + cosLatSinLon * (cosPhi * cosPsi + sinPhiSinTheta * sinPsi)
+                + sinlat * (sinPhi * cosTheta);
+
+        double b33 = cosLatCosLon * (sinPhi * sinPsi + cosPhiSinTheta * cosPsi)
+                + cosLatSinLon * (-sinPhi * cosPsi + cosPhiSinTheta * sinPsi)
+                + sinlat * (cosPhi * cosTheta);
+
+        return toDegrees(atan2(-b23, -b33));
+    };
+
+
+    double EulerConversions::getThetaFromTaitBryanAngles(double lat, double lon, double yaw, double pitch) {
+        double sinLat = sin(lat);
+        double cosLat = cos(lat);
+
+        double cosPitch = cos(pitch * _toRadians);
+        double sinPitch = sin(pitch * _toRadians);
+        double cosYaw = cos(yaw * _toRadians);
+
+        return asin(-cosLat * cosYaw * cosPitch - sinLat * sinPitch);
+    };
+
+
+
+    double EulerConversions::getPsiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch) {
+
+        double sinLat = sin(lat);
+        double sinLon = sin(lon);
+        double cosLon = cos(lon);
+        double cosLat = cos(lat);
+        double cosLatCosLon = cosLat * cosLon;
+        double cosLatSinLon = cosLat * sinLon;
+        double sinLatCosLon = sinLat * cosLon;
+        double sinLatSinLon = sinLat * sinLon;
+
+        double cosPitch = cos(pitch * _toRadians);
+        double sinPitch = sin(pitch * _toRadians);
+        double sinYaw = sin(yaw * _toRadians);
+        double cosYaw = cos(yaw * _toRadians);
+
+        double a_11 = -sinLon * sinYaw * cosPitch - sinLatCosLon * cosYaw * cosPitch + cosLatCosLon * sinPitch;
+        double a_12 = cosLon * sinYaw * cosPitch - sinLatSinLon * cosYaw * cosPitch + cosLatSinLon * sinPitch;
+
+        return atan2(a_12, a_11);
+    };
+
+
+
+    double EulerConversions::getPhiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch, double roll) {
+
+        double sinLat = sin(lat);
+        double cosLat = cos(lat);
+
+        double cosRoll = cos(roll * _toRadians);
+        double sinRoll = sin(roll * _toRadians);
+        double cosPitch = cos(pitch * _toRadians);
+        double sinPitch = sin(pitch * _toRadians);
+        double sinYaw = sin(yaw * _toRadians);
+        double cosYaw = cos(yaw * _toRadians);
+
+        double a_23 = cosLat * (-sinYaw * cosRoll + cosYaw * sinPitch * sinRoll) - sinLat * cosPitch * sinRoll;
+        double a_33 = cosLat * (sinYaw * sinRoll + cosYaw * sinPitch * cosRoll) - sinLat * cosPitch * cosRoll;
+
+        return atan2(a_23, a_33);
+    };
+
+
+
+    double EulerConversions::toDegrees(double radian)
+    {
+         return (radian *(180/M_PI));
+    }
+
+    
+
+}

src/SimCore/Orientation.cpp

@@ -0,0 +1,65 @@
+#include "SimCore/EulerConversion.hpp"
+#include "SimCore/SimCore.hpp"
+#include <SimCore/Orientation.hpp>
+
+
+namespace SimCore 
+{
+
+    Orientation::Orientation(double heading, double pitch , double roll ):heading_(heading),pitch_(pitch),roll_(roll)
+    {
+    }
+
+
+    Orientation::Orientation(Eigen::Vector3d angles, double lat, double lon)
+    {
+        heading_ = EulerConversions::getOrientationFromEuler( lat,  lon,  angles[PSI],angles[THETA] );
+        pitch_ =  EulerConversions::getPitchFromEuler(lat, lon, angles[PSI],angles[THETA]);
+        roll_ = EulerConversions::getRollFromEuler( lat,  lon,  angles[PSI],angles[THETA],angles[PHI] );
+    }
+
+
+
+    void Orientation::setHeading(double heading)
+    {
+        heading_ = heading;
+    }
+
+    double Orientation::getHeading()
+    {
+    return heading_;
+    }
+
+    void Orientation::setPitch(double pitch)
+    {
+    pitch_ = pitch;
+    }
+
+    double Orientation::getPitch()
+    {
+    return pitch_;
+    }
+
+    void Orientation::setRoll(double roll)
+    {
+    roll_ = roll;
+    }
+
+    double Orientation::getRoll()
+    {
+        return roll_;
+    }
+
+    Eigen::Vector3d Orientation::getEulerAngles(double lat ,double lon)
+    {
+        eulerAngels_[PHI]  =  EulerConversions::getPhiFromTaitBryanAngles( lat,  lon,  heading_, pitch_, roll_);
+        eulerAngels_[THETA] = EulerConversions::getThetaFromTaitBryanAngles(lat, lon, heading_, pitch_);
+        eulerAngels_[PSI]  = EulerConversions::getPsiFromTaitBryanAngles(lat, lon, heading_, pitch_);
+
+        return eulerAngels_;
+
+    }
+
+    
+
+}

tests/test_OrientationClass.cpp

@@ -0,0 +1,35 @@
+#include <SimCore/SimCore.hpp>
+#define CATCH_CONFIG_MAIN
+#include <catch2/catch.hpp>
+#include <SimCore/Orientation.hpp>
+#include <loguru.hpp>
+
+
+
+SCENARIO("Testing the SimCorePositionClass")
+{
+  GIVEN("different position in different forms")
+  {
+  double lon = 55;
+  double lat = 10;
+
+
+
+
+
+    WHEN("constructing Position Object with data")
+    {
+    SimCore::Orientation Ori1(0,0,0);
+
+    LOG_S(INFO)<< Ori1.getEulerAngles(lat,lon);
+    
+      THEN("positions attributes are correct")
+      {
+        REQUIRE(Ori1.getHeading() == 0);
+    
+
+
+      } //THEN
+    } // WHEN
+  } // GIVEN
+} //SCENARIO