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/lane/Types.hpp"
      15                 :            : #include "ad/map/point/ECEFOperation.hpp"
      16                 :            : #include "ad/map/point/ENUOperation.hpp"
      17                 :            : #include "ad/map/point/GeoOperation.hpp"
      18                 :            : #include "ad/map/point/HeadingOperation.hpp"
      19                 :            : 
      20                 :            : /** @brief namespace ad */
      21                 :            : namespace ad {
      22                 :            : /** @brief namespace map */
      23                 :            : namespace map {
      24                 :            : /** @brief namespace lane */
      25                 :            : namespace lane {
      26                 :            : 
      27                 :            : /**
      28                 :            :  * @brief Get the ENUEdge between the given border with corresponding lateralAlignment
      29                 :            :  *
      30                 :            :  * @param[in] border the ENU border, the edge is calculated from
      31                 :            :  * @param[in] lateralAlignment the lateral alignment as TParam [0.;1.] used to calculate the resulting edge.
      32                 :            :  *   The lateral alignment is relative to the left edge. If lateralAlignment is 1., the left edge is returned,
      33                 :            :  *   if lateralAlignment is 0., the right edge is returned
      34                 :            :  *
      35                 :            :  * @throws std::invalid_argument if the lateralAlignment parameter is smaller than 0. or larger than 1.
      36                 :            :  */
      37                 :            : point::ENUEdge getLateralAlignmentEdge(ENUBorder const &border, physics::ParametricValue const lateralAlignment);
      38                 :            : 
      39                 :            : /**
      40                 :            :  * @brief Get the distance between an ENU point and the lateral alignment edge
      41                 :            :  *
      42                 :            :  * @param[in] enuPoint is the point for which the distance should be calculated
      43                 :            :  * @param[in] lateralAlignmentEdge the lateral alignment in ENU form
      44                 :            :  *
      45                 :            :  * @return calculated Distance
      46                 :            :  */
      47                 :            : physics::Distance getDistanceEnuPointToLateralAlignmentEdge(point::ENUPoint const &enuPoint,
      48                 :            :                                                             point::ENUEdge const &lateralAlignmentEdge);
      49                 :            : 
      50                 :            : /**
      51                 :            :  * @brief normalizes the border
      52                 :            :  *
      53                 :            :  * At first the left and right edges of the border are checked for irregular directional vectors
      54                 :            :  * (scalarproduct of the vectors of three consecutive edge points has to be positive)
      55                 :            :  * Irregular points are dropped.
      56                 :            :  *
      57                 :            :  * Then, the number of points of the left and right edge of the border are made equal by extending the smaller edge.
      58                 :            :  * If an edge has less than 2 points, nothing is done.
      59                 :            :  *
      60                 :            :  * The previousBorder (optional parameter) is used to extend the irregular directional vector check to the beginning
      61                 :            :  * of the border edges; here leading to an potential overwrite of the first edge point by the end of the previousBorder.
      62                 :            :  */
      63                 :            : void normalizeBorder(ENUBorder &border, ENUBorder const *previousBorder = nullptr);
      64                 :            : 
      65                 :            : /**
      66                 :            :  * @brief operation to make the transition between two edges continuous
      67                 :            :  *
      68                 :            :  * If the end point of \c first is near (<=0.1m) to the end point of \c second or one of the edges has less than two
      69                 :            :  * points, nothing is done.
      70                 :            :  * Otherwise the first point of the second edge will exchanged by the last point of the first edge.
      71                 :            :  * The second point of the second edge is placed at some distance along the edge which is calculated using:
      72                 :            :  * - the distance of the displacement of the two edges
      73                 :            :  * - the direction of the displacement of the two edges compared to the the corresponding direction of the edges
      74                 :            :  *
      75                 :            :  * @param[in] first the first edge (is untouched by the algorithm)
      76                 :            :  * @param[in] second the second edge to be adapted if required to make the transition continuous
      77                 :            :  */
      78                 :            : void makeTransitionToSecondEdgeContinuous(point::ENUEdge const &first, point::ENUEdge &second);
      79                 :            : 
      80                 :            : /**
      81                 :            :  * @brief operation to make the transition between two borders continuous
      82                 :            :  *
      83                 :            :  * This executes the makeTransitionToSecondEdgeContinuous() for left and right edges.
      84                 :            :  * In addition, adds interpolation points to the respective other edge if required.
      85                 :            :  *
      86                 :            :  * @param[in] first the first border (is untouched by the algorithm)
      87                 :            :  * @param[in] second the second border to be adapted if required to make the transition continuous
      88                 :            :  */
      89                 :            : void makeTransitionToSecondBorderContinuous(ENUBorder const &first, ENUBorder &second);
      90                 :            : 
      91                 :            : /**
      92                 :            :  * @brief operation to make the transition between two edges continuous
      93                 :            :  *
      94                 :            :  * If the end point of \c first is near (<=0.1m) to the end point of \c second or one of the edges has less than two
      95                 :            :  * points, nothing is done.
      96                 :            :  * Otherwise the first point of the second edge will exchanged by the last point of the first edge.
      97                 :            :  * The second point of the second edge is placed at some distance along the edge which is calculated using:
      98                 :            :  * - the distance of the displacement of the two edges
      99                 :            :  * - the direction of the displacement of the two edges compared to the the corresponding direction of the edges
     100                 :            :  *
     101                 :            :  * @param[in] first the first edge to be adapted if required to make the transition continuous
     102                 :            :  * @param[in] second the second edge (is untouched by the algorithm)
     103                 :            :  */
     104                 :            : void makeTransitionFromFirstEdgeContinuous(point::ENUEdge &first, point::ENUEdge const &second);
     105                 :            : 
     106                 :            : /**
     107                 :            :  * @brief operation to make the transition between two borders continuous
     108                 :            :  *
     109                 :            :  * This executes the makeTransitionToSecondEdgeContinuous() for left and right edges.
     110                 :            :  * In addition, adds interpolation points to the respective other edge if required.
     111                 :            :  *
     112                 :            :  * @param[in] first the first border to be adapted if required to make the transition continuous
     113                 :            :  * @param[in] second the second border (is untouched by the algorithm)
     114                 :            :  */
     115                 :            : void makeTransitionFromFirstBorderContinuous(ENUBorder &first, ENUBorder const &second);
     116                 :            : 
     117                 :            : /** @brief calculate the length of the provided border as distance value
     118                 :            :  *
     119                 :            :  * For length calculation the average between left and right edge of the border is returned.
     120                 :            :  * Length calculation is performed within Cartesian ENU coordinate frame.
     121                 :            :  */
     122                 :          4 : inline physics::Distance calcLength(ENUBorder const &border)
     123                 :            : {
     124   [ +  -  +  -  :          8 :   return (calcLength(border.left) + calcLength(border.right)) / 2.;
             +  -  +  - ]
     125                 :            : }
     126                 :            : /** @brief calculate the length of the provided border as distance value
     127                 :            :  *
     128                 :            :  * For length calculation the average between left and right edge of the border is returned.
     129                 :            :  * Length calculation is performed within Cartesian ECEF coordinate frame.
     130                 :            :  */
     131                 :          0 : inline physics::Distance calcLength(ECEFBorder const &border)
     132                 :            : {
     133   [ #  #  #  #  :          0 :   return (calcLength(border.left) + calcLength(border.right)) / 2.;
             #  #  #  # ]
     134                 :            : }
     135                 :            : 
     136                 :            : /** @brief calculate the length of the provided border as distance value
     137                 :            :  *
     138                 :            :  * For length calculation the average between left and right edge of the border is returned.
     139                 :            :  * Length calculation is performed within Cartesian ECEF coordinate frame.
     140                 :            :  */
     141                 :          0 : inline physics::Distance calcLength(GeoBorder const &border)
     142                 :            : {
     143   [ #  #  #  #  :          0 :   return (calcLength(border.left) + calcLength(border.right)) / 2.;
             #  #  #  # ]
     144                 :            : }
     145                 :            : 
     146                 :            : /** @brief calculate the length out of the provided ENU border List as distance value
     147                 :            :  *
     148                 :            :  * For length calculation the average between left and right edge of the border is returned.
     149                 :            :  * Length calculation is performed within Cartesian ENU coordinate frame.
     150                 :            :  */
     151                 :            : physics::Distance calcLength(ENUBorderList const &borderList);
     152                 :            : 
     153                 :            : /** @brief calculate the length out of the provided ECEF border List as distance value
     154                 :            :  *
     155                 :            :  * For length calculation the average between left and right edge of the border is returned.
     156                 :            :  * Length calculation is performed within Cartesian ECEF coordinate frame.
     157                 :            :  */
     158                 :            : physics::Distance calcLength(ECEFBorderList const &borderList);
     159                 :            : 
     160                 :            : /** @brief calculate the length out of the provided GEO border List as distance value
     161                 :            :  *
     162                 :            :  * For length calculation the average between left and right edge of the border is returned.
     163                 :            :  * Length calculation is performed within GEO coordinate frame.
     164                 :            :  */
     165                 :            : physics::Distance calcLength(GeoBorderList const &borderList);
     166                 :            : 
     167                 :            : /** @brief calculate the ENUHeading of the vector<ENUBorder> at the given ENUPoint
     168                 :            :  *
     169                 :            :  *  If the given ENUPoint is not within the given borders,
     170                 :            :  *  an ENUHeading(2*M_PI) is returned.
     171                 :            :  */
     172                 :            : point::ENUHeading getENUHeading(ENUBorderList const &borderList, point::ENUPoint const &enuPoint);
     173                 :            : 
     174                 :            : } // namespace lane
     175                 :            : } // namespace map
     176                 :            : } // namespace ad