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/BoundingSphereOperation.hpp"
      10                 :            : 
      11                 :            : namespace ad {
      12                 :            : namespace map {
      13                 :            : namespace point {
      14                 :            : 
      15                 :     810864 : void expandBounds(point::ECEFPoint &upperBound, point::ECEFPoint &lowerBound, point::ECEFPoint const &point)
      16                 :            : {
      17                 :     810864 :   upperBound.x = std::max(upperBound.x, point.x);
      18                 :     810864 :   upperBound.y = std::max(upperBound.y, point.y);
      19                 :     810864 :   upperBound.z = std::max(upperBound.z, point.z);
      20                 :     810864 :   lowerBound.x = std::min(lowerBound.x, point.x);
      21                 :     810864 :   lowerBound.y = std::min(lowerBound.y, point.y);
      22                 :     810864 :   lowerBound.z = std::min(lowerBound.z, point.z);
      23                 :     810864 : }
      24                 :            : 
      25                 :      47771 : point::BoundingSphere calcBoundingSphere(Geometry const &geometryLeft, Geometry const &geometryRight)
      26                 :            : {
      27         [ +  - ]:      47771 :   point::BoundingSphere boundingSphere;
      28                 :            : 
      29   [ +  +  +  +  :      47771 :   if (geometryLeft.ecefEdge.empty() && geometryRight.ecefEdge.empty())
                   +  + ]
      30                 :            :   {
      31                 :          1 :     return boundingSphere;
      32                 :            :   }
      33                 :            : 
      34         [ +  - ]:      47770 :   point::ECEFPoint upperBound;
      35         [ +  - ]:      47770 :   point::ECEFPoint lowerBound;
      36         [ +  + ]:      47770 :   if (geometryLeft.ecefEdge.empty())
      37                 :            :   {
      38                 :          1 :     upperBound = geometryRight.ecefEdge.front();
      39                 :            :   }
      40                 :            :   else
      41                 :            :   {
      42                 :      47769 :     upperBound = geometryLeft.ecefEdge.front();
      43                 :            :   }
      44                 :      47770 :   lowerBound = upperBound;
      45                 :            : 
      46         [ +  + ]:     453389 :   for (auto const &point : geometryLeft.ecefEdge)
      47                 :            :   {
      48         [ +  - ]:     405619 :     expandBounds(upperBound, lowerBound, point);
      49                 :            :   }
      50         [ +  + ]:     453015 :   for (auto const &point : geometryRight.ecefEdge)
      51                 :            :   {
      52         [ +  - ]:     405245 :     expandBounds(upperBound, lowerBound, point);
      53                 :            :   }
      54                 :            : 
      55         [ +  - ]:      47770 :   point::ECEFPoint const diagonalVector = upperBound - lowerBound;
      56         [ +  - ]:      47770 :   auto diagonalVectorLength = point::vectorLength(diagonalVector);
      57                 :            : 
      58         [ +  - ]:      47770 :   boundingSphere.radius = 0.5 * diagonalVectorLength;
      59   [ +  -  +  - ]:      47770 :   boundingSphere.center = lowerBound + 0.5 * diagonalVector;
      60                 :            : 
      61                 :      47770 :   return boundingSphere;
      62                 :            : }
      63                 :            : 
      64                 :            : } // namespace point
      65                 :            : } // namespace map
      66                 :            : } // namespace ad
      67                 :            : 
      68                 :       4823 : ::ad::map::point::BoundingSphere operator+(::ad::map::point::BoundingSphere const &a,
      69                 :            :                                            ::ad::map::point::BoundingSphere const &b)
      70                 :            : {
      71         [ +  - ]:       4823 :   ::ad::map::point::BoundingSphere result;
      72                 :            :   ::ad::map::point::BoundingSphere const *small;
      73                 :            :   ::ad::map::point::BoundingSphere const *large;
      74   [ +  -  +  + ]:       4823 :   if (a.radius < b.radius)
      75                 :            :   {
      76                 :       1438 :     small = &a;
      77                 :       1438 :     large = &b;
      78                 :            :   }
      79                 :            :   else
      80                 :            :   {
      81                 :       3385 :     small = &b;
      82                 :       3385 :     large = &a;
      83                 :            :   }
      84                 :            : 
      85         [ +  - ]:       4823 :   auto const fromLargeToSmallCenter = small->center - large->center;
      86         [ +  - ]:       4823 :   auto const sphereCenterDistance = ::ad::map::point::vectorLength(fromLargeToSmallCenter);
      87                 :            : 
      88                 :            :   // move the center of the larger sphere in direction of the smaller one
      89                 :            :   // and increase the larger radius by the moving distance
      90   [ +  -  +  -  :       4823 :   auto const displacement = 0.5 * (sphereCenterDistance - large->radius + small->radius);
                   +  - ]
      91   [ +  -  +  + ]:       4823 :   if (displacement <= ::ad::physics::Distance(0.))
      92                 :            :   {
      93                 :            :     // small is already within large
      94                 :        912 :     result = *large;
      95                 :            :   }
      96   [ +  -  -  + ]:       3911 :   else if (sphereCenterDistance == ::ad::physics::Distance(0.))
      97                 :            :   {
      98                 :            :     // tiny center distance
      99                 :          0 :     result = *large;
     100                 :            :   }
     101                 :            :   else
     102                 :            :   {
     103                 :            :     result.center
     104   [ +  -  +  -  :       3911 :       = large->center + ::ad::physics::Distance(displacement / sphereCenterDistance) * (fromLargeToSmallCenter);
                   +  - ]
     105         [ +  - ]:       3911 :     result.radius = large->radius + displacement;
     106                 :            :   }
     107                 :       9646 :   return result;
     108                 :            : }