HeadingOperation.hpp

Go to the documentation of this file.

// ----------------- BEGIN LICENSE BLOCK ---------------------------------
//
// Copyright (C) 2018-2021 Intel Corporation
//
// SPDX-License-Identifier: MIT
//
// ----------------- END LICENSE BLOCK -----------------------------------
#pragma once
 
#include "ad/map/point/ECEFHeading.hpp"
#include "ad/map/point/ECEFPoint.hpp"
#include "ad/map/point/ENUHeading.hpp"
#include "ad/map/point/ENUPoint.hpp"
#include "ad/map/point/GeoPoint.hpp"
#include "ad/map/point/PointOperation.hpp"
#include "ad/physics/Angle.hpp"
#include "ad/physics/Distance.hpp"
 
namespace ad {
namespace map {
namespace point {
 
ECEFHeading createECEFHeading(ECEFPoint const &start, ECEFPoint const &end);
 
ECEFHeading createECEFHeading(ENUHeading const &yaw, GeoPoint const &enuReferencePoint);
 
ENUHeading createENUHeading(double yawAngleRadian);
 
ENUHeading createENUHeading(physics::Angle const &angle);
 
ENUHeading createENUHeading(ECEFHeading const &ecefHeading);
 
ENUHeading createENUHeading(ECEFHeading const &ecefHeading, GeoPoint const &enuReferencePoint);
 
ENUHeading createENUHeading(ECEFHeading const &ecefHeading, ECEFPoint const &enuReferencePoint);
 
ENUHeading createENUHeading(ENUPoint const &start, ENUPoint const &end);
 
ENUHeading normalizeENUHeading(ENUHeading const &heading);
 
inline ENUPoint getDirectionalVectorZPlane(ENUHeading const &heading)
{
  ENUPoint directionalVector;
  directionalVector.x = ENUCoordinate(std::cos(static_cast<double>(heading)));
  directionalVector.y = ENUCoordinate(std::sin(static_cast<double>(heading)));
  directionalVector.z = ENUCoordinate(0.);
  return directionalVector;
}
 
inline ENUPoint getOrthogonalVectorZPlane(ENUHeading const &heading)
{
  ENUPoint orthogonalVector;
  orthogonalVector.x = ENUCoordinate(-std::sin(static_cast<double>(heading)));
  orthogonalVector.y = ENUCoordinate(std::cos(static_cast<double>(heading)));
  orthogonalVector.z = ENUCoordinate(0.);
  return orthogonalVector;
}
 
inline void
getDirectionVectorsZPlane(ENUHeading const &heading, ENUPoint &directionalVector, ENUPoint &orthogonalVector)
{
  directionalVector = getDirectionalVectorZPlane(heading);
  orthogonalVector.x = -directionalVector.y;
  orthogonalVector.y = directionalVector.x;
  orthogonalVector.z = ENUCoordinate(0.);
}
 
} // namespace point
} // namespace map
} // namespace ad
 
inline ::ad::map::point::ECEFHeading operator+(::ad::map::point::ECEFHeading const &a,
                                               ::ad::map::point::ECEFHeading const &b)
{
  return ::ad::map::point::vectorAdd(a, b);
}
 
inline ::ad::map::point::ECEFHeading operator-(::ad::map::point::ECEFHeading const &a,
                                               ::ad::map::point::ECEFHeading const &b)
{
  return ::ad::map::point::vectorSub(a, b);
}
 
inline double operator*(::ad::map::point::ECEFHeading const &a, ::ad::map::point::ECEFHeading const &b)
{
  return ::ad::map::point::vectorDotProduct(a, b);
}
 
inline ::ad::map::point::ECEFHeading operator*(::ad::map::point::ECEFHeading const &a, ::ad::physics::Distance const &b)
{
  return ::ad::map::point::vectorMultiplyScalar(a, b);
}
 
inline ::ad::map::point::ECEFHeading operator*(::ad::physics::Distance const &b, ::ad::map::point::ECEFHeading const &a)
{
  return ::ad::map::point::vectorMultiplyScalar(a, b);
}
 
inline ::ad::map::point::ECEFHeading operator*(::ad::map::point::ECEFHeading const &a, double const &b)
{
  return ::ad::map::point::vectorMultiplyScalar(a, b);
}
 
inline ::ad::map::point::ECEFHeading operator*(double const &b, ::ad::map::point::ECEFHeading const &a)
{
  return ::ad::map::point::vectorMultiplyScalar(a, b);
}