LCOV - code coverage report
Current view: top level - include/ad/rss/unstructured - Geometry.hpp (source / functions) Hit Total Coverage
Test: ad_rss Lines: 28 40 70.0 %
Date: 2024-03-25 17:12:56 Functions: 7 9 77.8 %
Branches: 19 74 25.7 %

           Branch data     Line data    Source code
       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2020-2022 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : /**
      10                 :            :  * @file
      11                 :            :  */
      12                 :            : 
      13                 :            : #pragma once
      14                 :            : 
      15                 :            : #include <boost/geometry.hpp>
      16                 :            : #include <boost/geometry/geometries/geometries.hpp>
      17                 :            : #include <boost/geometry/geometries/point_xy.hpp>
      18                 :            : #include <boost/geometry/geometries/polygon.hpp>
      19                 :            : #include <sstream>
      20                 :            : #include "ad/physics/AngleOperation.hpp"
      21                 :            : #include "ad/physics/Distance.hpp"
      22                 :            : #include "ad/rss/state/HeadingRange.hpp"
      23                 :            : #include "ad/rss/world/UnstructuredTrajectorySet.hpp"
      24                 :            : 
      25                 :            : /*!
      26                 :            :  * @brief namespace ad
      27                 :            :  */
      28                 :            : namespace ad {
      29                 :            : /*!
      30                 :            :  * @brief namespace rss
      31                 :            :  */
      32                 :            : namespace rss {
      33                 :            : /*!
      34                 :            :  * @brief namespace unstructured
      35                 :            :  */
      36                 :            : namespace unstructured {
      37                 :            : 
      38                 :            : // Basic types
      39                 :            : typedef boost::geometry::model::d2::point_xy<double> Point;
      40                 :            : typedef boost::geometry::model::linestring<Point> Line;
      41                 :            : typedef boost::geometry::model::polygon<Point, false> Polygon; // counterclockwise
      42                 :            : typedef boost::geometry::model::multi_point<Point> MultiPoint;
      43                 :            : 
      44                 :            : /**
      45                 :            :  * @brief create a Point from a Distance2D
      46                 :            :  *
      47                 :            :  * @param[in] distance distance to convert
      48                 :            :  *
      49                 :            :  * @returns point
      50                 :            :  */
      51                 :       3815 : inline Point toPoint(ad::physics::Distance2D const &distance)
      52                 :            : {
      53         [ +  - ]:       3815 :   return Point(static_cast<double>(distance.x), static_cast<double>(distance.y));
      54                 :            : }
      55                 :            : 
      56                 :            : /**
      57                 :            :  * @brief create a Point from two Distance components
      58                 :            :  *
      59                 :            :  * @param[in] distanceX x component of distance
      60                 :            :  * @param[in] distanceY y component of distance
      61                 :            :  *
      62                 :            :  * @returns point
      63                 :            :  */
      64                 :       7467 : inline Point toPoint(ad::physics::Distance const &distanceX, ad::physics::Distance const &distanceY)
      65                 :            : {
      66         [ +  - ]:       7467 :   return Point(static_cast<double>(distanceX), static_cast<double>(distanceY));
      67                 :            : }
      68                 :            : 
      69                 :            : /**
      70                 :            :  * @brief convert a Point to a Distance2D
      71                 :            :  *
      72                 :            :  * @param[in] point point to convert
      73                 :            :  *
      74                 :            :  * @returns distance
      75                 :            :  */
      76                 :        996 : inline ad::physics::Distance2D toDistance(Point const &point)
      77                 :            : {
      78         [ #  # ]:        996 :   ad::physics::Distance2D distance;
      79         [ +  - ]:        996 :   distance.x = ad::physics::Distance(point.x());
      80         [ +  - ]:        996 :   distance.y = ad::physics::Distance(point.y());
      81                 :        996 :   return distance;
      82                 :            : }
      83                 :            : 
      84                 :            : /**
      85                 :            :  * @brief convert a trajectory set to a polygon
      86                 :            :  *
      87                 :            :  * @param[in] trajectorySet trajectory set to convert
      88                 :            :  * @param[out] polygon converted polygon
      89                 :            :  */
      90                 :            : void toPolygon(world::UnstructuredTrajectorySet const &trajectorySet, Polygon &polygon);
      91                 :            : 
      92                 :            : /**
      93                 :            :  * @brief convert a polygon to a trajectory set
      94                 :            :  *
      95                 :            :  * @param[in] polygon polygon to convert
      96                 :            :  * @param[out] trajectorySet converted trajectory set
      97                 :            :  */
      98                 :            : void toTrajectorySet(Polygon const &polygon, world::UnstructuredTrajectorySet &trajectorySet);
      99                 :            : 
     100                 :            : /**
     101                 :            :  * @brief check if an angle is within a range
     102                 :            :  *
     103                 :            :  * @param[in] angle angle to check
     104                 :            :  * @param[in] range heading range
     105                 :            :  *
     106                 :            :  * @returns true if inside angle range, otherwise false
     107                 :            :  */
     108                 :            : bool isInsideHeadingRange(ad::physics::Angle const &angle, state::HeadingRange const &range);
     109                 :            : 
     110                 :            : /**
     111                 :            :  * @brief get the overlap between two angle ranges
     112                 :            :  *
     113                 :            :  * @param[in]  a            first heading range
     114                 :            :  * @param[in]  b            second heading range
     115                 :            :  * @param[out] overlapRange overlapping heading ranges
     116                 :            :  *
     117                 :            :  * @returns true if overlap exists, otherwise false
     118                 :            :  */
     119                 :            : bool getHeadingOverlap(state::HeadingRange const &a,
     120                 :            :                        state::HeadingRange const &b,
     121                 :            :                        std::vector<state::HeadingRange> &overlapRanges);
     122                 :            : 
     123                 :            : /**
     124                 :            :  * @brief get the overlap between an angle range and a heading range
     125                 :            :  *
     126                 :            :  * @param[in]    headingRange angle range
     127                 :            :  * @param[inout] overlapRanges overlapping heading ranges
     128                 :            :  *
     129                 :            :  * @returns true if overlap exists, otherwise false
     130                 :            :  */
     131                 :            : bool getHeadingOverlap(state::HeadingRange const &headingRange, std::vector<state::HeadingRange> &overlapRanges);
     132                 :            : 
     133                 :            : /**
     134                 :            :  * @brief rotate a point around another point
     135                 :            :  *
     136                 :            :  * @param[in] origin         absolute origin
     137                 :            :  * @param[in] relativePoint  point to rotate, relative to origin
     138                 :            :  * @param[in] angle          angle to rotate the point
     139                 :            :  *
     140                 :            :  * @returns rotated point
     141                 :            :  */
     142                 :            : Point rotateAroundPoint(Point const &origin, Point const &relativePoint, ad::physics::Angle const &angle);
     143                 :            : 
     144                 :            : /**
     145                 :            :  * @brief calculate a point on a circle
     146                 :            :  *
     147                 :            :  * @param[in] origin absolute origin
     148                 :            :  * @param[in] radius radius of circle
     149                 :            :  * @param[in] angle  angle to rotate the point
     150                 :            :  *
     151                 :            :  * @returns resulting point
     152                 :            :  */
     153                 :            : Point getPointOnCircle(Point const &origin, ad::physics::Distance const &radius, ad::physics::Angle const &angle);
     154                 :            : 
     155                 :            : /**
     156                 :            :  * @brief calculate the circle origin
     157                 :            :  *
     158                 :            :  * @param[in] point  point on circle
     159                 :            :  * @param[in] radius radius of circle
     160                 :            :  * @param[in] angle  circle angle for point
     161                 :            :  *
     162                 :            :  * @returns circle origin
     163                 :            :  */
     164                 :            : Point getCircleOrigin(Point const &point, ad::physics::Distance const &radius, ad::physics::Angle const &angle);
     165                 :            : 
     166                 :            : /**
     167                 :            :  * @brief calculate points on a circle arc
     168                 :            :  *
     169                 :            :  * @param[in]  origin   absolute origin
     170                 :            :  * @param[in]  radius   radius of circle
     171                 :            :  * @param[in]  from     starting angle
     172                 :            :  * @param[in]  delta    angle of the arc
     173                 :            :  * @param[out] geometry geometry the calculated points are added to
     174                 :            :  */
     175                 :            : template <typename T>
     176                 :          1 : void calculateCircleArc(Point origin,
     177                 :            :                         ad::physics::Distance const &radius,
     178                 :            :                         ad::physics::Angle const &from,
     179                 :            :                         ad::physics::Angle const &delta,
     180                 :            :                         ad::physics::Angle const &stepWidth,
     181                 :            :                         T &geometry)
     182                 :            : {
     183                 :          1 :   ad::physics::Angle currentAngle = from;
     184         [ +  - ]:          1 :   ad::physics::Angle maxAngle = currentAngle + delta;
     185   [ +  -  +  + ]:          5 :   while (currentAngle <= maxAngle)
     186                 :            :   {
     187   [ +  -  +  - ]:          4 :     boost::geometry::append(geometry, getPointOnCircle(origin, radius, currentAngle));
     188         [ +  - ]:          4 :     currentAngle += stepWidth;
     189                 :            :   }
     190   [ +  -  +  -  :          1 :   if (currentAngle - stepWidth != maxAngle)
                   -  + ]
     191                 :            :   {
     192   [ #  #  #  # ]:          0 :     boost::geometry::append(geometry, getPointOnCircle(origin, radius, maxAngle));
     193                 :            :   }
     194                 :          1 : }
     195                 :            : 
     196                 :            : /**
     197                 :            :  * @brief check if two trajectory sets collide
     198                 :            :  *
     199                 :            :  * @param[in]  trajectorySet1 first trajectory set
     200                 :            :  * @param[in]  trajectorySet2 second trajectory set
     201                 :            :  *
     202                 :            :  * @returns true if trajectory sets collide, otherwise false
     203                 :            :  */
     204                 :            : bool collides(world::UnstructuredTrajectorySet const &trajectorySet1,
     205                 :            :               world::UnstructuredTrajectorySet const &trajectorySet2);
     206                 :            : 
     207                 :            : /**
     208                 :            :  * @brief Combine two polygons
     209                 :            :  *
     210                 :            :  * @param[in]  a       polygon a
     211                 :            :  * @param[in]  b       polygon b
     212                 :            :  * @param[out] result  resulting polygon
     213                 :            :  *
     214                 :            :  * @returns false if a failure occurred during calculations, true otherwise
     215                 :            :  */
     216                 :            : bool combinePolygon(Polygon const &a, Polygon const &b, Polygon &result);
     217                 :            : 
     218                 :            : } // namespace unstructured
     219                 :            : } // namespace rss
     220                 :            : } // namespace ad
     221                 :            : 
     222                 :            : /*!
     223                 :            :  * @brief Point operation: vector addition
     224                 :            :  *
     225                 :            :  * @param[in] a point a
     226                 :            :  * @param[in] b point b
     227                 :            :  *
     228                 :            :  * @returns c = a + b
     229                 :            :  */
     230                 :          5 : inline ad::rss::unstructured::Point operator+(ad::rss::unstructured::Point const &a,
     231                 :            :                                               ad::rss::unstructured::Point const &b)
     232                 :            : {
     233                 :          5 :   auto result = a;
     234         [ +  - ]:          5 :   boost::geometry::add_point(result, b);
     235                 :          5 :   return result;
     236                 :            : }
     237                 :            : 
     238                 :            : /*!
     239                 :            :  * @brief Point operation: vector subtraction
     240                 :            :  *
     241                 :            :  * @param[in] a point a
     242                 :            :  * @param[in] b point b
     243                 :            :  *
     244                 :            :  * @returns c = a - b
     245                 :            :  */
     246                 :       8853 : inline ad::rss::unstructured::Point operator-(ad::rss::unstructured::Point const &a,
     247                 :            :                                               ad::rss::unstructured::Point const &b)
     248                 :            : {
     249                 :       8853 :   auto result = a;
     250         [ +  - ]:       8853 :   boost::geometry::subtract_point(result, b);
     251                 :       8853 :   return result;
     252                 :            : }
     253                 :            : 
     254                 :            : /*!
     255                 :            :  * @brief comparison operation: Point
     256                 :            :  *
     257                 :            :  * @param[in] a point a
     258                 :            :  * @param[in] b point b
     259                 :            :  *
     260                 :            :  * @returns a == b
     261                 :            :  */
     262                 :        540 : inline bool operator==(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
     263                 :            : {
     264                 :        540 :   return (std::abs(a.x() - b.x()) <= std::numeric_limits<double>::epsilon())
     265   [ +  +  +  - ]:        540 :     && (std::abs(a.y() - b.y()) <= std::numeric_limits<double>::epsilon());
     266                 :            : }
     267                 :            : 
     268                 :            : /*!
     269                 :            :  * @brief comparison operation: Points not equal
     270                 :            :  *
     271                 :            :  * @param[in] a point a
     272                 :            :  * @param[in] b point b
     273                 :            :  *
     274                 :            :  * @returns a != b
     275                 :            :  */
     276                 :            : inline bool operator!=(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
     277                 :            : {
     278                 :            :   return !(a == b);
     279                 :            : }
     280                 :            : 
     281                 :            : namespace std {
     282                 :            : 
     283                 :            : /*!
     284                 :            :  * @brief to_string overload for Point
     285                 :            :  *
     286                 :            :  * @param[in] value a point
     287                 :            :  *
     288                 :            :  * @returns string describing point
     289                 :            :  */
     290                 :          0 : inline std::string to_string(ad::rss::unstructured::Point const &value)
     291                 :            : {
     292         [ #  # ]:          0 :   std::stringstream stream;
     293   [ #  #  #  #  :          0 :   stream << "[" << value.x() << "," << value.y() << "]";
          #  #  #  #  #  
             #  #  #  #  
                      # ]
     294         [ #  # ]:          0 :   return stream.str();
     295                 :            : }
     296                 :            : 
     297                 :            : /*!
     298                 :            :  * @brief to_string overload for Polygon
     299                 :            :  *
     300                 :            :  * @param[in] value a polygon
     301                 :            :  *
     302                 :            :  * @returns string describing polygon
     303                 :            :  */
     304                 :          0 : inline std::string to_string(ad::rss::unstructured::Polygon const &value)
     305                 :            : {
     306         [ #  # ]:          0 :   std::stringstream stream;
     307         [ #  # ]:          0 :   stream << "[";
     308         [ #  # ]:          0 :   for (auto pt : value.outer())
     309                 :            :   {
     310   [ #  #  #  #  :          0 :     stream << std::to_string(pt) << ",";
                   #  # ]
     311                 :            :   }
     312         [ #  # ]:          0 :   stream << "]";
     313         [ #  # ]:          0 :   return stream.str();
     314                 :            : }
     315                 :            : 
     316                 :            : /*!
     317                 :            :  * @brief to_string overload for Line
     318                 :            :  *
     319                 :            :  * @param[in] value a line
     320                 :            :  *
     321                 :            :  * @returns string describing line
     322                 :            :  */
     323                 :            : inline std::string to_string(ad::rss::unstructured::Line value)
     324                 :            : {
     325                 :            :   std::stringstream stream;
     326                 :            :   stream << "[";
     327                 :            :   for (auto pt : value)
     328                 :            :   {
     329                 :            :     stream << "[" << pt.x() << "," << pt.y() << "],";
     330                 :            :   }
     331                 :            :   stream << "]";
     332                 :            :   return stream.str();
     333                 :            : }
     334                 :            : } // namespace std

Generated by: LCOV version 1.14