Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: MIT
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : /**
       9                 :            :  * @file
      10                 :            :  */
      11                 :            : 
      12                 :            : #pragma once
      13                 :            : 
      14                 :            : #include "ad/map/point/ECEFHeading.hpp"
      15                 :            : #include "ad/map/point/ECEFPoint.hpp"
      16                 :            : #include "ad/map/point/ENUHeading.hpp"
      17                 :            : #include "ad/map/point/ENUPoint.hpp"
      18                 :            : #include "ad/map/point/GeoPoint.hpp"
      19                 :            : #include "ad/map/point/PointOperation.hpp"
      20                 :            : #include "ad/physics/Angle.hpp"
      21                 :            : #include "ad/physics/Distance.hpp"
      22                 :            : 
      23                 :            : /** @brief namespace ad */
      24                 :            : namespace ad {
      25                 :            : /** @brief namespace map */
      26                 :            : namespace map {
      27                 :            : /** @brief namespace point */
      28                 :            : namespace point {
      29                 :            : 
      30                 :            : /**
      31                 :            :  * @brief create a heading in ECEF as a directional vector
      32                 :            :  *
      33                 :            :  * @param[in] start  ECEFPoint defining the start of the directional vector
      34                 :            :  * @param[in] end ECEFPoint defining the end of the directional vector
      35                 :            :  *
      36                 :            :  * @returns heading = norm(end - start)
      37                 :            :  */
      38                 :            : ECEFHeading createECEFHeading(ECEFPoint const &start, ECEFPoint const &end);
      39                 :            : 
      40                 :            : /**
      41                 :            :  * @brief create a heading in ECEF as a directional vector
      42                 :            :  *
      43                 :            :  * @param[in] yaw ENUHeading the heading in ENU coordinate frame
      44                 :            :  * @param[in] enuReferencePoint the GeoPoint defining the ENU reference point
      45                 :            :  *
      46                 :            :  * @returns heading in ECEF as directional vector
      47                 :            :  */
      48                 :            : ECEFHeading createECEFHeading(ENUHeading const &yaw, GeoPoint const &enuReferencePoint);
      49                 :            : 
      50                 :            : /**
      51                 :            :  * @brief create a ENUHeading from yaw angle in radians
      52                 :            :  *
      53                 :            :  * Heading in ENU coordinate system as angle measured from East to North axis (yaw) in radians
      54                 :            :  *
      55                 :            :  * @param[in] yawAngleRadian heading provided as yaw angle in radians
      56                 :            :  *
      57                 :            :  * @returns heading with given yaw angle is normalized in the range -M_PI < heading <= M_PI
      58                 :            :  */
      59                 :            : ENUHeading createENUHeading(double yawAngleRadian);
      60                 :            : 
      61                 :            : /**
      62                 :            :  * @brief create a ENUHeading from angle
      63                 :            :  *
      64                 :            :  * Heading in ENU coordinate system as angle measured from East to North axis (yaw) in radians
      65                 :            :  *
      66                 :            :  * @param[in] angle heading provided as yaw angle in radians
      67                 :            :  *
      68                 :            :  * @returns heading with given yaw angle is normalized in the range -M_PI < heading <= M_PI
      69                 :            :  */
      70                 :            : ENUHeading createENUHeading(physics::Angle const &angle);
      71                 :            : 
      72                 :            : /**
      73                 :            :  * @brief create a ENUHeading from ECEFHeading value
      74                 :            :  *
      75                 :            :  * Heading in ENU coordinate system as angle measured from East to North axis (yaw) in radians
      76                 :            :  *
      77                 :            :  * @param[in] ecefHeading ECEFHeading value
      78                 :            :  *
      79                 :            :  * @returns heading with given yaw angle is normalized in the range -M_PI < heading <= M_PI
      80                 :            :  *
      81                 :            :  */
      82                 :            : ENUHeading createENUHeading(ECEFHeading const &ecefHeading);
      83                 :            : 
      84                 :            : /**
      85                 :            :  * @brief create a ENUHeading from ECEFHeading value
      86                 :            :  *
      87                 :            :  * Heading in ENU coordinate system as angle measured from East to North axis (yaw) in radians
      88                 :            :  *
      89                 :            :  * @param[in] ecefHeading ECEFHeading value
      90                 :            :  * @param[in] enuReferencePoint the reference point of the ENU coordinate system as GeoPoint
      91                 :            :  *
      92                 :            :  * @returns heading with given yaw angle is normalized in the range -M_PI < heading <= M_PI
      93                 :            :  *
      94                 :            :  */
      95                 :            : ENUHeading createENUHeading(ECEFHeading const &ecefHeading, GeoPoint const &enuReferencePoint);
      96                 :            : 
      97                 :            : /**
      98                 :            :  * @brief create a ENUHeading from ECEFHeading value
      99                 :            :  *
     100                 :            :  * Heading in ENU coordinate system as angle measured from East to North axis (yaw) in radians
     101                 :            :  *
     102                 :            :  * @param[in] ecefHeading ECEFHeading value
     103                 :            :  * @param[in] enuReferencePoint the reference point of the ENU coordinate system as ECEFPoint
     104                 :            :  *
     105                 :            :  * @returns heading with given yaw angle is normalized in the range -M_PI < heading <= M_PI
     106                 :            :  *
     107                 :            :  */
     108                 :            : ENUHeading createENUHeading(ECEFHeading const &ecefHeading, ECEFPoint const &enuReferencePoint);
     109                 :            : 
     110                 :            : /**
     111                 :            :  * @brief create a ENUHeading from a directional vector
     112                 :            :  *
     113                 :            :  * @param[in] start point defining the start of the directional vector
     114                 :            :  * @param[in] end point defining the end of the directional vector
     115                 :            :  *
     116                 :            :  * @returns ENU heading of the resulting directional vector
     117                 :            :  */
     118                 :            : ENUHeading createENUHeading(ENUPoint const &start, ENUPoint const &end);
     119                 :            : 
     120                 :            : /**
     121                 :            :  * @returns normalized ENU heading
     122                 :            :  *          heading with given yaw angle is normalized in the range -M_PI < heading <= M_PI
     123                 :            :  */
     124                 :            : ENUHeading normalizeENUHeading(ENUHeading const &heading);
     125                 :            : 
     126                 :            : /**
     127                 :            :  * @brief get a directional vector of the heading
     128                 :            :  *
     129                 :            :  * angle of zero vector will be along positive x - axis
     130                 :            :  *
     131                 :            :  * @param[in] heading the heading input
     132                 :            :  */
     133                 :        331 : inline ENUPoint getDirectionalVectorZPlane(ENUHeading const &heading)
     134                 :            : {
     135                 :        331 :   ENUPoint directionalVector;
     136                 :        331 :   directionalVector.x = ENUCoordinate(std::cos(static_cast<double>(heading)));
     137                 :        331 :   directionalVector.y = ENUCoordinate(std::sin(static_cast<double>(heading)));
     138                 :        331 :   directionalVector.z = ENUCoordinate(0.);
     139                 :        331 :   return directionalVector;
     140                 :            : }
     141                 :            : 
     142                 :            : /**
     143                 :            :  * @brief get a vector that is orthogonal to the vectors heading
     144                 :            :  *
     145                 :            :  * angle of zero vector will be along positive y - axis
     146                 :            :  *
     147                 :            :  * @param[in] heading the heading input
     148                 :            :  */
     149                 :            : inline ENUPoint getOrthogonalVectorZPlane(ENUHeading const &heading)
     150                 :            : {
     151                 :            :   ENUPoint orthogonalVector;
     152                 :            :   orthogonalVector.x = ENUCoordinate(-std::sin(static_cast<double>(heading)));
     153                 :            :   orthogonalVector.y = ENUCoordinate(std::cos(static_cast<double>(heading)));
     154                 :            :   orthogonalVector.z = ENUCoordinate(0.);
     155                 :            :   return orthogonalVector;
     156                 :            : }
     157                 :            : 
     158                 :            : /**
     159                 :            :  * @brief get a directional vector of the vectors heading and the orthogonal vector at once
     160                 :            :  *
     161                 :            :  * Combines getDirectionalVectorZPlane() and getOrthogonalVectorZPlane() in one call.
     162                 :            :  *
     163                 :            :  * @param[in] heading the heading input
     164                 :            :  * @param[out] directionalVector directional vector of the heading,
     165                 :            :  *    angle of zero vector will be along positive x - axis
     166                 :            :  * @param[out] orthogonalVector vector that is orthogonal to the vectors heading
     167                 :            :  *   angle of zero vector will be along positive y - axis
     168                 :            :  */
     169                 :            : inline void
     170                 :         17 : getDirectionVectorsZPlane(ENUHeading const &heading, ENUPoint &directionalVector, ENUPoint &orthogonalVector)
     171                 :            : {
     172                 :         17 :   directionalVector = getDirectionalVectorZPlane(heading);
     173                 :         17 :   orthogonalVector.x = -directionalVector.y;
     174                 :         17 :   orthogonalVector.y = directionalVector.x;
     175                 :         17 :   orthogonalVector.z = ENUCoordinate(0.);
     176                 :         17 : }
     177                 :            : 
     178                 :            : } // namespace point
     179                 :            : } // namespace map
     180                 :            : } // namespace ad
     181                 :            : 
     182                 :            : /**
     183                 :            :  * @brief add two ECEFHeading vectors
     184                 :            :  *
     185                 :            :  * @param[in] a vector a
     186                 :            :  * @param[in] b vector b
     187                 :            :  *
     188                 :            :  * @returns vector c = a + b
     189                 :            :  */
     190                 :            : inline ::ad::map::point::ECEFHeading operator+(::ad::map::point::ECEFHeading const &a,
     191                 :            :                                                ::ad::map::point::ECEFHeading const &b)
     192                 :            : {
     193                 :            :   return ::ad::map::point::vectorAdd(a, b);
     194                 :            : }
     195                 :            : 
     196                 :            : /**
     197                 :            :  * @brief subtract two ECEFHeading vectors from each right
     198                 :            :  *
     199                 :            :  * @param[in] a vector a
     200                 :            :  * @param[in] b vector b
     201                 :            :  *
     202                 :            :  * @returns c = a - b
     203                 :            :  */
     204                 :            : inline ::ad::map::point::ECEFHeading operator-(::ad::map::point::ECEFHeading const &a,
     205                 :            :                                                ::ad::map::point::ECEFHeading const &b)
     206                 :            : {
     207                 :            :   return ::ad::map::point::vectorSub(a, b);
     208                 :            : }
     209                 :            : 
     210                 :            : /**
     211                 :            :  * @brief calculate the dot product of two ECEFHeading vectors
     212                 :            :  *
     213                 :            :  * @param[in] a vector a
     214                 :            :  * @param[in] b vector b
     215                 :            :  *
     216                 :            :  * @returns value d = a * b
     217                 :            :  */
     218                 :     387409 : inline double operator*(::ad::map::point::ECEFHeading const &a, ::ad::map::point::ECEFHeading const &b)
     219                 :            : {
     220                 :     387409 :   return ::ad::map::point::vectorDotProduct(a, b);
     221                 :            : }
     222                 :            : 
     223                 :            : /**
     224                 :            :  * @brief multiplies a ECEFHeading vector with a scalar
     225                 :            :  * @param[in] a vector a
     226                 :            :  * @param[in] b scalar b
     227                 :            :  */
     228                 :            : inline ::ad::map::point::ECEFHeading operator*(::ad::map::point::ECEFHeading const &a, ::ad::physics::Distance const &b)
     229                 :            : {
     230                 :            :   return ::ad::map::point::vectorMultiplyScalar(a, b);
     231                 :            : }
     232                 :            : 
     233                 :            : /**
     234                 :            :  * @brief multiplies a ECEFHeading vector with a scalar
     235                 :            :  * @param[in] b scalar b
     236                 :            :  * @param[in] a vector a
     237                 :            :  */
     238                 :            : inline ::ad::map::point::ECEFHeading operator*(::ad::physics::Distance const &b, ::ad::map::point::ECEFHeading const &a)
     239                 :            : {
     240                 :            :   return ::ad::map::point::vectorMultiplyScalar(a, b);
     241                 :            : }
     242                 :            : 
     243                 :            : /**
     244                 :            :  * @brief multiplies a ECEFHeading vector with a scalar
     245                 :            :  * @param[in] a vector a
     246                 :            :  * @param[in] b scalar b
     247                 :            :  */
     248                 :            : inline ::ad::map::point::ECEFHeading operator*(::ad::map::point::ECEFHeading const &a, double const &b)
     249                 :            : {
     250                 :            :   return ::ad::map::point::vectorMultiplyScalar(a, b);
     251                 :            : }
     252                 :            : 
     253                 :            : /**
     254                 :            :  * @brief multiplies a ECEFHeading vector with a scalar
     255                 :            :  * @param[in] b scalar b
     256                 :            :  * @param[in] a vector a
     257                 :            :  */
     258                 :      90602 : inline ::ad::map::point::ECEFHeading operator*(double const &b, ::ad::map::point::ECEFHeading const &a)
     259                 :            : {
     260                 :      90602 :   return ::ad::map::point::vectorMultiplyScalar(a, b);
     261                 :            : }