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 <cmath>
      15                 :            : 
      16                 :            : #include "ad/map/point/ECEFCoordinateOperation.hpp"
      17                 :            : #include "ad/map/point/ENUCoordinateOperation.hpp"
      18                 :            : #include "ad/physics/Distance.hpp"
      19                 :            : #include "ad/physics/ParametricValue.hpp"
      20                 :            : #include "ad/physics/RatioValue.hpp"
      21                 :            : 
      22                 :            : /** @brief namespace ad */
      23                 :            : namespace ad {
      24                 :            : /** @brief namespace map */
      25                 :            : namespace map {
      26                 :            : /** @brief namespace point */
      27                 :            : namespace point {
      28                 :            : 
      29                 :            : /**
      30                 :            :  * @brief calculate the cross product of two vectors
      31                 :            :  *
      32                 :            :  * @param[in] a vector a
      33                 :            :  * @param[in] b vector b
      34                 :            :  *
      35                 :            :  * @returns vector c = a x b
      36                 :            :  */
      37                 :       1144 : template <typename PointType> PointType vectorCrossProduct(PointType const &a, PointType const &b)
      38                 :            : {
      39                 :       1144 :   PointType result;
      40         [ +  - ]:       1144 :   result.x = a.y * b.z - a.z * b.y;
      41         [ +  - ]:       1144 :   result.y = a.z * b.x - a.x * b.z;
      42         [ +  - ]:       1144 :   result.z = a.x * b.y - a.y * b.x;
      43                 :       1144 :   return result;
      44                 :            : }
      45                 :            : 
      46                 :            : /**
      47                 :            :  * @brief calculate the dot product of two vectors
      48                 :            :  *
      49                 :            :  * @param[in] a vector a
      50                 :            :  * @param[in] b vector b
      51                 :            :  *
      52                 :            :  * @returns value d = a * b
      53                 :            :  */
      54                 :  131575226 : template <typename PointType> double vectorDotProduct(PointType const &a, PointType const &b)
      55                 :            : {
      56                 :  131575226 :   return static_cast<double>(a.x) * static_cast<double>(b.x) + static_cast<double>(a.y) * static_cast<double>(b.y)
      57                 :  131575226 :     + static_cast<double>(a.z) * static_cast<double>(b.z);
      58                 :            : }
      59                 :            : 
      60                 :            : /**
      61                 :            :  * @brief multiplies a vector with a scalar
      62                 :            :  * @param[in] a vector a
      63                 :            :  * @param[in] b scalar b
      64                 :            :  */
      65                 :     142392 : template <typename PointType> PointType vectorMultiplyScalar(PointType const &a, double const &b)
      66                 :            : {
      67                 :     142392 :   PointType result;
      68                 :     142392 :   result.x = a.x * b;
      69                 :     142392 :   result.y = a.y * b;
      70                 :     142392 :   result.z = a.z * b;
      71                 :     142392 :   return result;
      72                 :            : }
      73                 :            : 
      74                 :            : /**
      75                 :            :  * @brief multiplies a vector with a scalar
      76                 :            :  * @param[in] a vector a
      77                 :            :  * @param[in] b scalar b
      78                 :            :  */
      79                 :       3979 : template <typename PointType> PointType vectorMultiplyScalar(PointType const &a, physics::Distance const &b)
      80                 :            : {
      81         [ +  - ]:       3979 :   return vectorMultiplyScalar(a, static_cast<double>(b));
      82                 :            : }
      83                 :            : 
      84                 :            : /**
      85                 :            :  * @brief calculate the length of a vector
      86                 :            :  *
      87                 :            :  * @param[in] a vector a
      88                 :            :  *
      89                 :            :  * @returns value d = |a|
      90                 :            :  */
      91                 :   84603807 : template <typename PointType> physics::Distance vectorLength(PointType const &a)
      92                 :            : {
      93                 :   84603807 :   physics::Distance const length(std::sqrt(static_cast<double>(vectorDotProduct(a, a))));
      94                 :   84603807 :   return length;
      95                 :            : }
      96                 :            : 
      97                 :            : /**
      98                 :            :  * @brief normalizes a vector
      99                 :            :  *
     100                 :            :  * @param[in] a vector a
     101                 :            :  *
     102                 :            :  * @returns vector c = a  / |a|
     103                 :            :  */
     104                 :     127619 : template <typename PointType> PointType vectorNorm(PointType const &a)
     105                 :            : {
     106         [ +  - ]:     127619 :   physics::Distance const length = vectorLength(a);
     107   [ +  -  +  - ]:     127619 :   if (length > physics::Distance(0.))
     108                 :            :   {
     109         [ +  - ]:     127619 :     PointType result;
     110         [ +  - ]:     127619 :     result.x = a.x / length;
     111         [ +  - ]:     127619 :     result.y = a.y / length;
     112         [ +  - ]:     127619 :     result.z = a.z / length;
     113                 :     127619 :     return result;
     114                 :            :   }
     115                 :          0 :   return a;
     116                 :            : }
     117                 :            : 
     118                 :            : /**
     119                 :            :  * @brief add two vectors
     120                 :            :  *
     121                 :            :  * @param[in] a vector a
     122                 :            :  * @param[in] b vector b
     123                 :            :  *
     124                 :            :  * @returns vector c = a + b
     125                 :            :  */
     126                 :      57190 : template <typename PointType> PointType vectorAdd(PointType const &a, PointType const &b)
     127                 :            : {
     128                 :      57190 :   PointType result;
     129                 :      57190 :   result.x = a.x + b.x;
     130                 :      57190 :   result.y = a.y + b.y;
     131                 :      57190 :   result.z = a.z + b.z;
     132                 :      57190 :   return result;
     133                 :            : }
     134                 :            : 
     135                 :            : /**
     136                 :            :  * @brief subtract two vectors from each right
     137                 :            :  *
     138                 :            :  * @param[in] a vector a
     139                 :            :  * @param[in] b vector b
     140                 :            :  *
     141                 :            :  * @returns c = a - b
     142                 :            :  */
     143                 :  131451361 : template <typename PointType> PointType vectorSub(PointType const &a, PointType const &b)
     144                 :            : {
     145                 :  131451311 :   PointType result;
     146                 :  131451361 :   result.x = a.x - b.x;
     147                 :  131451361 :   result.y = a.y - b.y;
     148                 :  131451361 :   result.z = a.z - b.z;
     149                 :  131451361 :   return result;
     150                 :            : }
     151                 :            : 
     152                 :            : /**
     153                 :            :  * @brief Interpolates point between two points.
     154                 :            :  *
     155                 :            :  * @param[in] a vector a
     156                 :            :  * @param[in] b vector b
     157                 :            :  * @param[in] tparam Between 0 and 1.
     158                 :            :  *
     159                 :            :  * @returns Point between point a and b.
     160                 :            :  *          with tparam==0, it will return point a;
     161                 :            :  *          with tparam==1, it will return point b.
     162                 :            :  */
     163                 :            : template <typename PointType>
     164                 :   29822670 : PointType vectorInterpolate(PointType const &a, PointType const &b, physics::ParametricValue const &tparam)
     165                 :            : {
     166         [ +  - ]:   29822670 :   return vectorExtrapolate(a, b, static_cast<double>(tparam));
     167                 :            : }
     168                 :            : 
     169                 :            : /**
     170                 :            :  * @brief Extrapolate point based on line defined by two points.
     171                 :            :  *
     172                 :            :  * @param[in] a vector a
     173                 :            :  * @param[in] b vector b
     174                 :            :  * @param[in] scalar scalar value
     175                 :            :  *
     176                 :            :  * @returns Points extrapolated using point a and b.
     177                 :            :  *          with scalar==0, it will return point a;
     178                 :            :  *          with scalar==1, it will return point b.
     179                 :            :  */
     180                 :   29822668 : template <typename PointType> PointType vectorExtrapolate(PointType const &a, PointType const &b, double const &scalar)
     181                 :            : {
     182                 :   29822668 :   PointType result;
     183   [ +  -  +  - ]:   29822668 :   result.x = (1 - scalar) * a.x + scalar * b.x;
     184   [ +  -  +  - ]:   29822668 :   result.y = (1 - scalar) * a.y + scalar * b.y;
     185   [ +  -  +  - ]:   29822668 :   result.z = (1 - scalar) * a.z + scalar * b.z;
     186                 :   29822668 :   return result;
     187                 :            : }
     188                 :            : 
     189                 :            : } // namespace point
     190                 :            : } // namespace map
     191                 :            : } // namespace ad