Branch data     Line data    Source code

       1                 :            : // Copyright 2020 Apex.AI, Inc.
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : #ifndef HELPER_FUNCTIONS__ANGLE_UTILS_HPP_
      18                 :            : #define HELPER_FUNCTIONS__ANGLE_UTILS_HPP_
      19                 :            : 
      20                 :            : #include <cmath>
      21                 :            : #include <type_traits>
      22                 :            : 
      23                 :            : namespace autoware
      24                 :            : {
      25                 :            : namespace common
      26                 :            : {
      27                 :            : namespace helper_functions
      28                 :            : {
      29                 :            : 
      30                 :            : namespace detail
      31                 :            : {
      32                 :            : constexpr auto kDoublePi = 2.0 * M_PI;
      33                 :            : }  // namespace detail
      34                 :            : 
      35                 :            : ///
      36                 :            : /// @brief      Wrap angle to the [-pi, pi] range.
      37                 :            : ///
      38                 :            : /// @details    This method uses the formula suggested in the paper [On wrapping the Kalman filter
      39                 :            : ///             and estimating with the SO(2) group](https://arxiv.org/pdf/1708.05551.pdf) and
      40                 :            : ///             implements the following formula:
      41                 :            : ///             \f$\mathrm{mod}(\alpha + \pi, 2 \pi) - \pi\f$.
      42                 :            : ///
      43                 :            : /// @param[in]  angle  The input angle
      44                 :            : ///
      45                 :            : /// @tparam     T      Type of scalar
      46                 :            : ///
      47                 :            : /// @return     Angle wrapped to the chosen range.
      48                 :            : ///
      49                 :            : template<typename T>
      50                 :          5 : constexpr T wrap_angle(T angle) noexcept
      51                 :            : {
      52                 :          5 :   auto help_angle = angle + T(M_PI);
      53         [ +  + ]:          6 :   while (help_angle < T{}) {help_angle += T(detail::kDoublePi);}
      54         [ +  + ]:          7 :   while (help_angle >= T(detail::kDoublePi)) {help_angle -= T(detail::kDoublePi);}
      55                 :          5 :   return help_angle - T(M_PI);
      56                 :            : }
      57                 :            : 
      58                 :            : }  // namespace helper_functions
      59                 :            : }  // namespace common
      60                 :            : }  // namespace autoware
      61                 :            : 
      62                 :            : #endif  // HELPER_FUNCTIONS__ANGLE_UTILS_HPP_