NEDtoECEF.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <cmath>
 
#include "NumCpp/Coordinates/ReferenceFrames/ECEF.hpp"
#include "NumCpp/Coordinates/ReferenceFrames/LLA.hpp"
#include "NumCpp/Coordinates/ReferenceFrames/NED.hpp"
#include "NumCpp/Coordinates/Transforms/ECEFtoLLA.hpp"
#include "NumCpp/Coordinates/Transforms/LLAtoECEF.hpp"
#include "NumCpp/Coordinates/Transforms/NEDtoECEF.hpp"
#include "NumCpp/Functions/dot.hpp"
#include "NumCpp/NdArray.hpp"
 
namespace nc::coordinates::transforms
{
    [[nodiscard]] inline reference_frames::ECEF NEDtoECEF(const reference_frames::NED&  target,
                                                          const reference_frames::ECEF& referencePoint) noexcept
    {
        const auto referencePointLLA = ECEFtoLLA(referencePoint);
 
        const auto sinLat = std::sin(referencePointLLA.latitude);
        const auto cosLat = std::cos(referencePointLLA.latitude);
        const auto sinLon = std::sin(referencePointLLA.longitude);
        const auto cosLon = std::cos(referencePointLLA.longitude);
 
        auto rotationMatrix  = NdArray<double>(3, 3);
        rotationMatrix(0, 0) = -sinLat * cosLon;
        rotationMatrix(1, 0) = -sinLat * sinLon;
        rotationMatrix(2, 0) = cosLat;
        rotationMatrix(0, 1) = -sinLon;
        rotationMatrix(1, 1) = cosLon;
        rotationMatrix(2, 1) = 0.;
        rotationMatrix(0, 2) = -cosLat * cosLon;
        rotationMatrix(1, 2) = -cosLat * sinLon;
        rotationMatrix(2, 2) = -sinLat;
 
        auto targetVec = NdArray<double>(3, 1);
        targetVec[0]   = target.north();
        targetVec[1]   = target.east();
        targetVec[2]   = target.down();
 
        auto referencePointVec = NdArray<double>(3, 1);
        referencePointVec[0]   = referencePoint.x;
        referencePointVec[1]   = referencePoint.y;
        referencePointVec[2]   = referencePoint.z;
 
        const auto targetECEFVec = dot(rotationMatrix, targetVec) + referencePointVec;
        return { targetECEFVec[0], targetECEFVec[1], targetECEFVec[2] };
    }
 
    [[nodiscard]] inline reference_frames::ECEF NEDtoECEF(const reference_frames::NED& target,
                                                          const reference_frames::LLA& referencePoint) noexcept
    {
        return NEDtoECEF(target, LLAtoECEF(referencePoint));
    }
} // namespace nc::coordinates::transforms