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/GeoOperation.hpp"
      10                 :            : #include "ad/map/point/EdgeOperation.hpp"
      11                 :            : #include "ad/map/point/Transform.hpp"
      12                 :            : 
      13                 :            : namespace ad {
      14                 :            : namespace map {
      15                 :            : namespace point {
      16                 :            : 
      17                 :            : Altitude const AltitudeUnknown = ::ad::map::point::Altitude::getMin();
      18                 :            : 
      19                 :            : struct CartesianCoordinates
      20                 :            : {
      21                 :            :   double x;
      22                 :            :   double y;
      23                 :            :   double z;
      24                 :            : };
      25                 :            : 
      26                 :            : /**
      27                 :            :  * @brief Independent conversion to the cartesian system.
      28                 :            :  */
      29                 :        100 : CartesianCoordinates toCartesianCoordinates(GeoPoint const &point)
      30                 :            : {
      31                 :        100 :   double wgs84_r = static_cast<double>(CoordinateTransform::WGS84_R(point.latitude));
      32                 :        100 :   double lat_rad = CoordinateTransform::geocentricLatitude(point.latitude);
      33                 :        100 :   double lon_rad = toRadians(point.longitude);
      34                 :            : 
      35                 :        100 :   double cos_lon = std::cos(lon_rad);
      36                 :        100 :   double sin_lon = std::sin(lon_rad);
      37                 :        100 :   double cos_lat = std::cos(lat_rad);
      38                 :        100 :   double sin_lat = std::sin(lat_rad);
      39                 :            :   CartesianCoordinates result;
      40                 :        100 :   result.x = wgs84_r * cos_lon * cos_lat;
      41                 :        100 :   result.y = wgs84_r * sin_lon * cos_lat;
      42                 :        100 :   result.z = wgs84_r * sin_lat;
      43                 :            : 
      44                 :        100 :   double cos_glat = std::cos(toRadians(point.latitude));
      45                 :        100 :   double sin_glat = std::sin(toRadians(point.latitude));
      46                 :            : 
      47                 :        100 :   result.x += static_cast<double>(point.altitude) * cos_glat * cos_lon;
      48                 :        100 :   result.y += static_cast<double>(point.altitude) * cos_glat * sin_lon;
      49                 :        100 :   result.z += static_cast<double>(point.altitude) * sin_glat;
      50                 :            : 
      51                 :        100 :   return result;
      52                 :            : }
      53                 :            : 
      54                 :         50 : physics::Distance distance(GeoPoint const &point, GeoPoint const &other)
      55                 :            : {
      56         [ +  - ]:         50 :   auto const pointCartesian = toCartesianCoordinates(point);
      57         [ +  - ]:         50 :   auto const otherCartesian = toCartesianCoordinates(other);
      58                 :         50 :   auto const delta = vectorSub(pointCartesian, otherCartesian);
      59         [ +  - ]:        100 :   return vectorLength(delta);
      60                 :            : }
      61                 :            : 
      62                 :         57 : GeoPoint zeroAltitude(GeoPoint const &point)
      63                 :            : {
      64                 :         57 :   GeoPoint pt(point);
      65                 :         57 :   pt.altitude = Altitude(0);
      66                 :         57 :   return pt;
      67                 :            : }
      68                 :            : 
      69                 :         24 : physics::Distance flatDistance(GeoPoint const &point, GeoPoint const &other)
      70                 :            : {
      71   [ +  -  +  - ]:         48 :   return distance(zeroAltitude(point), zeroAltitude(other));
      72                 :            : }
      73                 :            : 
      74                 :          7 : GeoPoint approxAltitude(GeoPoint const &point, const GeoEdge &pts)
      75                 :            : {
      76                 :          7 :   GeoPoint pt(zeroAltitude(point));
      77         [ +  - ]:          7 :   if (pts.size() == 0)
      78                 :            :   {
      79                 :            :   }
      80         [ +  + ]:          7 :   else if (pts.size() == 1)
      81                 :            :   {
      82                 :          1 :     pt.altitude = pts[0].altitude;
      83                 :            :   }
      84                 :            :   else
      85                 :            :   {
      86                 :          6 :     GeoPoint pt0(pts[0]);
      87                 :          6 :     GeoPoint pt1(pts[1]);
      88         [ +  - ]:          6 :     auto d0 = flatDistance(pt, pt0);
      89         [ +  - ]:          6 :     auto d1 = flatDistance(pt, pt1);
      90   [ +  -  +  + ]:          6 :     if (d0 > d1)
      91                 :            :     {
      92                 :          3 :       std::swap(pt0, pt1);
      93                 :          3 :       std::swap(d0, d1);
      94                 :            :     }
      95         [ +  + ]:         18 :     for (size_t i = 2; i < pts.size(); i++)
      96                 :            :     {
      97         [ +  - ]:         12 :       auto const d = flatDistance(pt, pts[i]);
      98   [ +  -  +  + ]:         12 :       if (d < d0)
      99                 :            :       {
     100                 :          3 :         pt1 = pt0;
     101                 :          3 :         pt0 = pts[i];
     102                 :          3 :         d1 = d0;
     103                 :          3 :         d0 = d;
     104                 :            :       }
     105   [ +  -  +  + ]:          9 :       else if (d < d1)
     106                 :            :       {
     107                 :          4 :         pt1 = pts[i];
     108                 :          4 :         d1 = d;
     109                 :            :       }
     110                 :            :     }
     111         [ +  - ]:          6 :     auto const d01 = d0 + d1;
     112   [ +  -  +  - ]:          6 :     if (d01 > physics::Distance(0.))
     113                 :            :     {
     114         [ +  - ]:          6 :       double t = d0 / d01;
     115   [ +  -  +  -  :          6 :       pt.altitude = (1 - t) * pt0.altitude + t * pt1.altitude;
                   +  - ]
     116                 :            :     }
     117                 :            :     else
     118                 :            :     {
     119                 :          0 :       pt.altitude = pt0.altitude;
     120                 :            :     }
     121                 :            :   }
     122                 :          7 :   return pt;
     123                 :            : }
     124                 :            : 
     125                 :          6 : bool isOnTheLeft(GeoPoint const &point, const GeoPoint &pt0, const GeoPoint &pt1)
     126                 :            : {
     127   [ +  -  +  -  :          6 :   if ((pt1.longitude - pt0.longitude) == Longitude(0.))
                   +  + ]
     128                 :            :   {
     129         [ +  + ]:          2 :     if (point.longitude < pt1.longitude)
     130                 :            :     {
     131                 :          1 :       return pt1.latitude > pt0.latitude;
     132                 :            :     }
     133         [ +  - ]:          1 :     else if (point.longitude > pt1.longitude)
     134                 :            :     {
     135                 :          1 :       return pt1.latitude < pt0.latitude;
     136                 :            :     }
     137                 :            :   }
     138   [ +  -  +  -  :          4 :   else if ((pt1.latitude - pt0.latitude) == Latitude(0.))
                   +  + ]
     139                 :            :   {
     140         [ +  + ]:          2 :     if (point.latitude < pt1.latitude)
     141                 :            :     {
     142                 :          1 :       return pt1.longitude < pt0.longitude;
     143                 :            :     }
     144         [ +  - ]:          1 :     else if (point.latitude > pt1.latitude)
     145                 :            :     {
     146                 :          1 :       return pt1.longitude > pt0.longitude;
     147                 :            :     }
     148                 :            :   }
     149                 :            :   else
     150                 :            :   {
     151         [ +  - ]:          2 :     auto const slope = physics::RatioValue(static_cast<double>(pt1.latitude - pt0.latitude)
     152         [ +  - ]:          2 :                                            / static_cast<double>(pt1.longitude - pt0.longitude));
     153   [ +  -  +  - ]:          2 :     if (slope != physics::RatioValue(0.))
     154                 :            :     {
     155                 :          2 :       double yi = static_cast<double>(pt0.latitude) - static_cast<double>(pt0.longitude) * static_cast<double>(slope);
     156                 :          2 :       double cs = (static_cast<double>(slope) * static_cast<double>(point.longitude)) + yi;
     157         [ +  + ]:          2 :       if (static_cast<double>(point.latitude) > cs)
     158                 :            :       {
     159         [ +  - ]:          2 :         return pt1.longitude > pt0.longitude;
     160                 :            :       }
     161         [ +  - ]:          1 :       if (static_cast<double>(point.latitude) < cs)
     162                 :            :       {
     163         [ +  - ]:          1 :         return pt1.longitude < pt0.longitude;
     164                 :            :       }
     165                 :            :     }
     166                 :            :   }
     167                 :          0 :   return false;
     168                 :            : }
     169                 :            : 
     170                 :          1 : bool haveSameOrientation(const GeoEdge &pts0, const GeoEdge &pts1)
     171                 :            : {
     172   [ +  -  +  -  :          1 :   if (pts0.size() > 1 && pts1.size() > 1)
                   +  - ]
     173                 :            :   {
     174         [ +  - ]:          1 :     auto const front_front = distance(pts0.front(), pts1.front());
     175         [ +  - ]:          1 :     auto const front_back = distance(pts0.front(), pts1.back());
     176         [ +  - ]:          1 :     return front_front < front_back;
     177                 :            :   }
     178                 :            :   else
     179                 :            :   {
     180                 :          0 :     return false;
     181                 :            :   }
     182                 :            : }
     183                 :            : 
     184                 :          6 : bool isOnTheLeft(const GeoEdge &pts0, const GeoEdge &pts1)
     185                 :            : {
     186   [ +  -  +  -  :          6 :   if (pts0.size() > 1 && pts1.size() > 1)
                   +  - ]
     187                 :            :   {
     188                 :          6 :     return isOnTheLeft(pts0.front(), pts1[0], pts1[1]);
     189                 :            :   }
     190                 :            :   else
     191                 :            :   {
     192                 :          0 :     return false;
     193                 :            :   }
     194                 :            : }
     195                 :            : 
     196                 :          2 : physics::Distance calcLength(GeoEdge const &edge)
     197                 :            : {
     198                 :          2 :   return calculateEdgeLength(edge);
     199                 :            : }
     200                 :            : 
     201                 :            : } // namespace point
     202                 :            : } // namespace map
     203                 :            : } // namespace ad