LCOV - code coverage report
Current view: top level - src/common/autoware_auto_geometry/include/geometry - interval.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 64 68 94.1 %
Date: 2021-01-26 05:02:50 Functions: 23 25 92.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 70 114 61.4 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2020 Mapless AI, Inc.
       2                 :            : //
       3                 :            : // Permission is hereby granted, free of charge, to any person obtaining a copy
       4                 :            : // of this software and associated documentation files (the "Software"), to
       5                 :            : // deal in the Software without restriction, including without limitation the
       6                 :            : // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
       7                 :            : // sell copies of the Software, and to permit persons to whom the Software is
       8                 :            : // furnished to do so, subject to the following conditions:
       9                 :            : //
      10                 :            : // The above copyright notice and this permission notice shall be included in
      11                 :            : // all copies or substantial portions of the Software.
      12                 :            : //
      13                 :            : // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
      14                 :            : // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
      15                 :            : // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
      16                 :            : //  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
      17                 :            : //  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
      18                 :            : // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
      19                 :            : // IN THE SOFTWARE.
      20                 :            : 
      21                 :            : #ifndef GEOMETRY__INTERVAL_HPP_
      22                 :            : #define GEOMETRY__INTERVAL_HPP_
      23                 :            : 
      24                 :            : #include <algorithm>
      25                 :            : #include <cmath>
      26                 :            : #include <iostream>
      27                 :            : #include <limits>
      28                 :            : #include <sstream>
      29                 :            : #include <stdexcept>
      30                 :            : #include <string>
      31                 :            : #include <type_traits>
      32                 :            : #include <utility>
      33                 :            : 
      34                 :            : #include "common/types.hpp"
      35                 :            : #include "helper_functions/float_comparisons.hpp"
      36                 :            : 
      37                 :            : namespace autoware
      38                 :            : {
      39                 :            : namespace common
      40                 :            : {
      41                 :            : namespace geometry
      42                 :            : {
      43                 :            : 
      44                 :            : /**
      45                 :            :  * @brief Data structure to contain scalar interval bounds.
      46                 :            :  *
      47                 :            :  * @post The interval is guaranteed to be valid upon successful construction. An
      48                 :            :  * interval [min, max] is "valid" if it is empty (min/max = NaN) or its bounds
      49                 :            :  * are ordered (min <= max).
      50                 :            :  */
      51                 :            : template<typename T>
      52                 :            : class Interval
      53                 :            : {
      54                 :            :   static_assert(
      55                 :            :     std::is_floating_point<T>::value,
      56                 :            :     "Intervals only support floating point types.");
      57                 :            : 
      58                 :            : public:
      59                 :            :   /**
      60                 :            :    * @brief Compute equality.
      61                 :            :    *
      62                 :            :    * Two intervals compare equal iff they are both valid and they are both
      63                 :            :    * either empty or have equal bounds.
      64                 :            :    *
      65                 :            :    * @note This is defined inline because the class is templated.
      66                 :            :    *
      67                 :            :    * @return True iff the intervals compare equal.
      68                 :            :    */
      69                 :         12 :   friend bool operator==(const Interval & i1, const Interval & i2)
      70                 :            :   {
      71                 :         12 :     const auto min_eq = (Interval::min(i1) == Interval::min(i2));
      72                 :         12 :     const auto max_eq = (Interval::max(i1) == Interval::max(i2));
      73   [ +  +  +  + ]:         12 :     const auto bounds_equal = (min_eq && max_eq);
      74   [ +  +  +  - ]:         12 :     const auto both_empty = (Interval::empty(i1) && Interval::empty(i2));
      75   [ +  +  +  + ]:         12 :     return both_empty || bounds_equal;
      76                 :            :   }
      77                 :            : 
      78                 :            :   /**
      79                 :            :    * @brief Compute inequality and the logical negation of equality.
      80                 :            :    * @note This is defined inline because the class is templated.
      81                 :            :    */
      82                 :          6 :   friend bool operator!=(const Interval & i1, const Interval & i2)
      83                 :            :   {
      84                 :          6 :     return !(i1 == i2);
      85                 :            :   }
      86                 :            : 
      87                 :            :   /**
      88                 :            :    * @brief Stream overload for Interval types.
      89                 :            :    *
      90                 :            :    * @note Output precision is fixed inside the function definition, and the
      91                 :            :    * serialization is JSON compatible.
      92                 :            :    *
      93                 :            :    * @note The serialization is lossy. It is used for debugging and for
      94                 :            :    * generating exception strings.
      95                 :            :    */
      96                 :          2 :   friend std::ostream & operator<<(std::ostream & os, const Interval & i)
      97                 :            :   {
      98                 :          2 :     constexpr auto PRECISION = 5;
      99                 :            : 
     100                 :            :     // Internal helper to format the output.
     101                 :          4 :     const auto readable_extremum = [](const T & val) {
     102   [ -  +  #  # ]:          4 :         if (val == std::numeric_limits<T>::lowest()) {
     103   [ #  #  #  # ]:          0 :           return std::string("REAL_MIN");
     104                 :            :         }
     105   [ -  +  #  # ]:          4 :         if (val == std::numeric_limits<T>::max()) {
     106   [ #  #  #  # ]:          0 :           return std::string("REAL_MAX");
     107                 :            :         }
     108                 :            : 
     109   [ +  -  #  # ]:          8 :         std::stringstream ss;
     110                 :          4 :         ss.setf(std::ios::fixed, std::ios::floatfield);
     111                 :          4 :         ss.precision(PRECISION);
     112   [ +  -  #  # ]:          4 :         ss << val;
     113   [ +  -  #  # ]:          4 :         return ss.str();
     114                 :            :       };
     115                 :            : 
     116                 :            :     // Do stream output.
     117         [ -  + ]:          2 :     if (Interval::empty(i)) {
     118         [ #  # ]:          0 :       return os << "{}";
     119                 :            :     }
     120                 :          0 :     return os << "{\"min\": " << readable_extremum(Interval::min(i)) <<
     121   [ +  -  +  -  :          2 :            ", \"max\": " << readable_extremum(Interval::max(i)) << "}";
          +  -  +  -  +  
             -  +  -  +  
                      - ]
     122                 :            :   }
     123                 :            : 
     124                 :            :   /**
     125                 :            :    * @brief Test whether the two intervals have bounds within epsilon of each
     126                 :            :    * other.
     127                 :            :    *
     128                 :            :    * @note If both intervals are empty, this returns true. If only one is empty,
     129                 :            :    * this returns false.
     130                 :            :    */
     131                 :            :   static bool abs_eq(const Interval & i1, const Interval & i2, const T & eps);
     132                 :            : 
     133                 :            :   /** @brief The minimum bound of the interval. */
     134                 :        281 :   static T min(const Interval & i) {return i.min_;}
     135                 :            : 
     136                 :            :   /** @brief The maximum bound of the interval. */
     137                 :        169 :   static T max(const Interval & i) {return i.max_;}
     138                 :            : 
     139                 :            :   /**
     140                 :            :    * @brief Return the measure (length) of the interval.
     141                 :            :    *
     142                 :            :    * @note For empty or invalid intervals, NaN is returned. See Interval::empty
     143                 :            :    * for note on distinction between measure zero and the emptiness property.
     144                 :            :    */
     145                 :            :   static T measure(const Interval & i);
     146                 :            : 
     147                 :            :   /**
     148                 :            :    * @brief Utility for checking whether an interval has zero measure.
     149                 :            :    *
     150                 :            :    * @note For empty or invalid intervals, false is returned. See
     151                 :            :    * Interval::empty for note on distinction between measure zero and the
     152                 :            :    * emptiness property.
     153                 :            :    *
     154                 :            :    * @return True iff the interval has zero measure.
     155                 :            :    */
     156                 :            :   static bool zero_measure(const Interval & i);
     157                 :            : 
     158                 :            :   /**
     159                 :            :    * @brief Whether the interval is empty or not.
     160                 :            :    *
     161                 :            :    * @note Emptiness refers to whether the interval contains any points and is
     162                 :            :    * thus a distinct property from measure: an interval is non-empty if contains
     163                 :            :    * only a single point even though its measure in that case is zero.
     164                 :            :    *
     165                 :            :    * @return True iff the interval is empty.
     166                 :            :    */
     167                 :            :   static bool empty(const Interval & i);
     168                 :            : 
     169                 :            :   /**
     170                 :            :    * @brief Test for whether a given interval contains a given value.
     171                 :            :    * @return True iff 'interval' contains 'value'.
     172                 :            :    */
     173                 :            :   static bool contains(const Interval & i, const T & value);
     174                 :            : 
     175                 :            :   /**
     176                 :            :    * @brief Test for whether 'i1' subseteq 'i2'
     177                 :            :    * @return True iff i1 subseteq i2.
     178                 :            :    */
     179                 :            :   static bool is_subset_eq(const Interval & i1, const Interval & i2);
     180                 :            : 
     181                 :            :   /**
     182                 :            :    * @brief Compute the intersection of two intervals as a new interval.
     183                 :            :    */
     184                 :            :   static Interval intersect(const Interval & i1, const Interval & i2);
     185                 :            : 
     186                 :            :   /**
     187                 :            :    * @brief Clamp a scalar 'val' onto 'interval'.
     188                 :            :    * @return If 'val' in 'interval', return 'val'; otherwise return the nearer
     189                 :            :    * interval bound.
     190                 :            :    */
     191                 :            :   static T clamp_to(const Interval & i, T val);
     192                 :            : 
     193                 :            :   /**
     194                 :            :    * @brief Constructor: initialize an empty interval with members set to NaN.
     195                 :            :    */
     196                 :            :   Interval();
     197                 :            : 
     198                 :            :   /**
     199                 :            :    * @brief Constructor: specify exact interval bounds.
     200                 :            :    *
     201                 :            :    * @note An empty interval is specified by setting both bounds to NaN.
     202                 :            :    * @note An exception is thrown if the specified bounds are invalid.
     203                 :            :    *
     204                 :            :    * @post Construction is successful iff the interval is valid.
     205                 :            :    */
     206                 :            :   Interval(const T & min, const T & max);
     207                 :            : 
     208                 :            : private:
     209                 :            :   static constexpr T NaN = std::numeric_limits<T>::quiet_NaN();
     210                 :            : 
     211                 :            :   T min_;
     212                 :            :   T max_;
     213                 :            : 
     214                 :            :   /**
     215                 :            :    * @brief Verify that the bounds are valid in an interval.
     216                 :            :    * @note This method is private because it can only be used in the
     217                 :            :    * constructor. Once an interval has been constructed, its bounds are
     218                 :            :    * guaranteed to be valid.
     219                 :            :    */
     220                 :            :   static bool bounds_valid(const Interval & i);
     221                 :            : };  // class Interval
     222                 :            : 
     223                 :            : //------------------------------------------------------------------------------
     224                 :            : 
     225                 :            : typedef Interval<autoware::common::types::float64_t> Interval_d;
     226                 :            : typedef Interval<autoware::common::types::float32_t> Interval_f;
     227                 :            : 
     228                 :            : //------------------------------------------------------------------------------
     229                 :            : 
     230                 :            : template<typename T>
     231                 :            : constexpr T Interval<T>::NaN;
     232                 :            : 
     233                 :            : //------------------------------------------------------------------------------
     234                 :            : 
     235                 :            : template<typename T>
     236                 :         16 : Interval<T>::Interval()
     237                 :         16 : : min_(Interval::NaN), max_(Interval::NaN) {}
     238                 :            : 
     239                 :            : //------------------------------------------------------------------------------
     240                 :            : 
     241                 :            : template<typename T>
     242                 :         47 : Interval<T>::Interval(const T & min, const T & max)
     243                 :         47 : : min_(min), max_(max)
     244                 :            : {
     245         [ +  + ]:         47 :   if (!Interval::bounds_valid(*this)) {
     246         [ +  - ]:          4 :     std::stringstream ss;
     247   [ +  -  +  - ]:          2 :     ss << "Attempted to construct an invalid interval: " << *this;
     248   [ +  -  +  - ]:          2 :     throw std::runtime_error(ss.str());
     249                 :            :   }
     250                 :         45 : }
     251                 :            : 
     252                 :            : //------------------------------------------------------------------------------
     253                 :            : 
     254                 :            : template<typename T>
     255                 :          8 : bool Interval<T>::abs_eq(
     256                 :            :   const Interval & i1, const Interval & i2, const T & eps)
     257                 :            : {
     258                 :            :   namespace comp = helper_functions::comparisons;
     259                 :            : 
     260   [ +  +  +  + ]:          8 :   const auto both_empty = Interval::empty(i1) && Interval::empty(i2);
     261   [ +  +  +  + ]:          8 :   const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2);
     262                 :            : 
     263                 :          8 :   const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps);
     264                 :          8 :   const auto maxs_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps);
     265   [ +  +  +  +  :          8 :   const auto both_non_empty_equal = both_non_empty && mins_equal && maxs_equal;
                   +  - ]
     266                 :            : 
     267   [ +  +  +  + ]:          8 :   return both_empty || both_non_empty_equal;
     268                 :            : }
     269                 :            : 
     270                 :            : //------------------------------------------------------------------------------
     271                 :            : 
     272                 :            : template<typename T>
     273                 :          3 : bool Interval<T>::zero_measure(const Interval & i)
     274                 :            : {
     275                 :            :   // An empty interval will return false because (NaN == NaN) is false.
     276                 :          3 :   return Interval::min(i) == Interval::max(i);
     277                 :            : }
     278                 :            : 
     279                 :            : //------------------------------------------------------------------------------
     280                 :            : 
     281                 :            : template<typename T>
     282                 :        127 : bool Interval<T>::empty(const Interval & i)
     283                 :            : {
     284   [ +  +  +  - ]:        127 :   return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i));
     285                 :            : }
     286                 :            : 
     287                 :            : //------------------------------------------------------------------------------
     288                 :            : 
     289                 :            : template<typename T>
     290                 :         47 : bool Interval<T>::bounds_valid(const Interval & i)
     291                 :            : {
     292                 :         47 :   const auto is_ordered = (Interval::min(i) <= Interval::max(i));
     293                 :            : 
     294                 :            :   // Check for emptiness expicitly because it requires both bounds to be NaN
     295   [ +  -  +  + ]:         47 :   return Interval::empty(i) || is_ordered;
     296                 :            : }
     297                 :            : 
     298                 :            : //------------------------------------------------------------------------------
     299                 :            : 
     300                 :            : template<typename T>
     301                 :          5 : bool Interval<T>::is_subset_eq(const Interval & i1, const Interval & i2)
     302                 :            : {
     303                 :          5 :   const auto lower_contained = (Interval::min(i1) >= Interval::min(i2));
     304                 :          5 :   const auto upper_contained = (Interval::max(i1) <= Interval::max(i2));
     305   [ +  +  +  - ]:          5 :   return lower_contained && upper_contained;
     306                 :            : }
     307                 :            : 
     308                 :            : //------------------------------------------------------------------------------
     309                 :            : 
     310                 :            : template<typename T>
     311                 :          3 : bool Interval<T>::contains(const Interval & i, const T & value)
     312                 :            : {
     313   [ +  +  +  + ]:          3 :   return (value >= Interval::min(i)) && (value <= Interval::max(i));
     314                 :            : }
     315                 :            : 
     316                 :            : //------------------------------------------------------------------------------
     317                 :            : 
     318                 :            : template<typename T>
     319                 :          5 : T Interval<T>::measure(const Interval & i)
     320                 :            : {
     321                 :          5 :   return Interval::max(i) - Interval::min(i);
     322                 :            : }
     323                 :            : 
     324                 :            : //------------------------------------------------------------------------------
     325                 :            : 
     326                 :            : template<typename T>
     327                 :          4 : Interval<T> Interval<T>::intersect(const Interval & i1, const Interval & i2)
     328                 :            : {
     329                 :            :   // Construct intersection assuming an intersection exists.
     330                 :            :   try {
     331   [ +  +  -  + ]:          4 :     const auto either_empty = Interval::empty(i1) || Interval::empty(i2);
     332                 :          4 :     const auto min = std::max(Interval::min(i1), Interval::min(i2));
     333                 :          4 :     const auto max = std::min(Interval::max(i1), Interval::max(i2));
     334   [ +  +  +  + ]:          4 :     return either_empty ? Interval() : Interval(min, max);
     335                 :          1 :   } catch (...) {
     336                 :            :   }
     337                 :            : 
     338                 :            :   // Otherwise, the intersection is empty.
     339                 :          1 :   return Interval();
     340                 :            : }
     341                 :            : 
     342                 :            : //------------------------------------------------------------------------------
     343                 :            : 
     344                 :            : template<typename T>
     345                 :         30 : T Interval<T>::clamp_to(const Interval & i, T val)
     346                 :            : {
     347                 :            :   // clamp val to min
     348                 :         30 :   val = std::max(val, Interval::min(i));
     349                 :            : 
     350                 :            :   // clamp val to max
     351                 :         30 :   val = std::min(val, Interval::max(i));
     352                 :            : 
     353         [ +  + ]:         30 :   return Interval::empty(i) ? Interval::NaN : val;
     354                 :            : }
     355                 :            : 
     356                 :            : //------------------------------------------------------------------------------
     357                 :            : 
     358                 :            : }  // namespace geometry
     359                 :            : }  // namespace common
     360                 :            : }  // namespace autoware
     361                 :            : 
     362                 :            : #endif  // GEOMETRY__INTERVAL_HPP_

Generated by: LCOV version 1.14