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                 :            : #include "ad/map/point/HeadingOperation.hpp"
      10                 :            : 
      11                 :            : #include <cmath>
      12                 :            : 
      13                 :            : #include "ad/map/access/Operation.hpp"
      14                 :            : #include "ad/map/point/ECEFOperation.hpp"
      15                 :            : #include "ad/map/point/ENUOperation.hpp"
      16                 :            : #include "ad/map/point/Transform.hpp"
      17                 :            : #include "ad/physics/AngleOperation.hpp"
      18                 :            : 
      19                 :            : namespace ad {
      20                 :            : namespace map {
      21                 :            : namespace point {
      22                 :            : 
      23                 :     127561 : ECEFHeading createECEFHeading(ECEFPoint const &start, ECEFPoint const &end)
      24                 :            : {
      25   [ +  -  +  - ]:     127561 :   ECEFPoint const heading = vectorNorm(end - start);
      26         [ +  - ]:     127561 :   ECEFHeading result;
      27                 :     127561 :   result.x = heading.x;
      28                 :     127561 :   result.y = heading.y;
      29                 :     127561 :   result.z = heading.z;
      30                 :     255122 :   return result;
      31                 :            : }
      32                 :            : 
      33                 :        314 : ECEFHeading createECEFHeading(ENUHeading const &yaw, GeoPoint const &enuReferencePoint)
      34                 :            : {
      35         [ +  - ]:        314 :   ECEFPoint const start = toECEF(enuReferencePoint);
      36         [ +  - ]:        314 :   ENUPoint const enuEndPoint = getDirectionalVectorZPlane(yaw);
      37         [ +  - ]:        314 :   ECEFPoint const end = toECEF(enuEndPoint, enuReferencePoint);
      38         [ +  - ]:        628 :   return createECEFHeading(start, end);
      39                 :            : }
      40                 :            : 
      41                 :          1 : ENUHeading normalizeENUHeading(ENUHeading const &heading)
      42                 :            : {
      43                 :          1 :   return createENUHeading(static_cast<double>(heading));
      44                 :            : }
      45                 :            : 
      46                 :        400 : ENUHeading createENUHeading(physics::Angle const &angle)
      47                 :            : {
      48                 :        400 :   return ENUHeading(static_cast<double>(physics::normalizeAngleSigned(angle)));
      49                 :            : }
      50                 :            : 
      51                 :        400 : ENUHeading createENUHeading(double yawAngleRadian)
      52                 :            : {
      53         [ +  - ]:        800 :   return createENUHeading(physics::Angle(yawAngleRadian));
      54                 :            : }
      55                 :            : 
      56                 :         34 : ENUHeading createENUHeading(ECEFHeading const &ecefHeading)
      57                 :            : {
      58         [ +  - ]:         68 :   return createENUHeading(ecefHeading, access::getENUReferencePoint());
      59                 :            : }
      60                 :            : 
      61                 :       1042 : ENUHeading createENUHeading(ECEFHeading const &ecefHeading, ECEFPoint const &enuReferencePoint)
      62                 :            : {
      63         [ +  - ]:       2084 :   return createENUHeading(ecefHeading, toGeo(enuReferencePoint));
      64                 :            : }
      65                 :            : 
      66                 :       1130 : ENUHeading createENUHeading(ECEFHeading const &ecefHeading, GeoPoint const &enuReferencePoint)
      67                 :            : {
      68         [ +  - ]:       1130 :   ECEFPoint const enuZero = toECEF(enuReferencePoint);
      69   [ +  -  +  - ]:       1130 :   ECEFPoint const enuEastAxis = toECEF(getEnuEastAxis(), enuReferencePoint);
      70   [ +  -  +  - ]:       1130 :   ECEFPoint const enuUpAxis = toECEF(getEnuUpAxis(), enuReferencePoint);
      71                 :            : 
      72         [ +  - ]:       1130 :   ECEFHeading const toEast = createECEFHeading(enuZero, enuEastAxis);
      73         [ +  - ]:       1130 :   ECEFHeading const toUp = createECEFHeading(enuZero, enuUpAxis);
      74                 :            : 
      75                 :       1130 :   double const cosHeading = vectorDotProduct(toEast, ecefHeading);
      76                 :            : 
      77         [ +  - ]:       1130 :   ECEFHeading const crossProductVector(vectorCrossProduct(toEast, ecefHeading));
      78                 :       1130 :   double const sinHeadingAbs = static_cast<double>(vectorLength(crossProductVector));
      79                 :            : 
      80                 :            :   // determine the sign of sine
      81                 :       1130 :   ENUHeading result;
      82         [ +  + ]:       1130 :   if (std::signbit(vectorDotProduct(crossProductVector, toUp)))
      83                 :            :   {
      84                 :        562 :     result = ENUHeading(std::atan2(-sinHeadingAbs, cosHeading));
      85                 :            :   }
      86                 :            :   else
      87                 :            :   {
      88                 :        568 :     result = ENUHeading(std::atan2(sinHeadingAbs, cosHeading));
      89                 :            :   }
      90                 :            :   // atan2 seems to allow -M_PI, so normalize that one
      91   [ +  -  -  + ]:       1130 :   if (result == ENUHeading(-M_PI))
      92                 :            :   {
      93                 :          0 :     result = ENUHeading(M_PI);
      94                 :            :   }
      95                 :       1130 :   return result;
      96                 :            : }
      97                 :            : 
      98                 :          0 : ENUHeading createENUHeading(ENUPoint const &start, ENUPoint const &end)
      99                 :            : {
     100         [ #  # ]:          0 :   ENUPoint const direction = end - start;
     101                 :          0 :   ENUHeading result{std::atan2(static_cast<double>(direction.y), static_cast<double>(direction.x))};
     102                 :            :   // atan2 seems to allow -M_PI, so normalize that one
     103   [ #  #  #  # ]:          0 :   if (result == ENUHeading(-M_PI))
     104                 :            :   {
     105                 :          0 :     result = ENUHeading(M_PI);
     106                 :            :   }
     107                 :          0 :   return result;
     108                 :            : }
     109                 :            : 
     110                 :            : } // namespace point
     111                 :            : } // namespace map
     112                 :            : } // namespace ad