Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2020-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: MIT
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : /**
      10                 :            :  * @file
      11                 :            :  */
      12                 :            : 
      13                 :            : #pragma once
      14                 :            : 
      15                 :            : #include <cmath>
      16                 :            : #include "ad/physics/Types.hpp"
      17                 :            : 
      18                 :            : /*!
      19                 :            :  * @brief Arithmetic physics operation s = v * t
      20                 :            :  *
      21                 :            :  * @param[in] v angular speed value
      22                 :            :  * @param[in] t duration value
      23                 :            :  *
      24                 :            :  * @returns s = v * t as angle value
      25                 :            :  *
      26                 :            :  * \note throws a std::out_of_range exception if one of the two operands or the result of
      27                 :            :  *   the operation is not valid
      28                 :            :  */
      29                 :            : inline ad::physics::Angle operator*(ad::physics::AngularVelocity const &v, ad::physics::Duration const &t)
      30                 :            : {
      31                 :            :   v.ensureValid();
      32                 :            :   t.ensureValid();
      33                 :            :   ad::physics::Angle const s(static_cast<double>(v) * static_cast<double>(t));
      34                 :            :   s.ensureValid();
      35                 :            :   return s;
      36                 :            : }
      37                 :            : 
      38                 :            : /*!
      39                 :            :  * @brief Arithmetic physics operation s = t * v
      40                 :            :  *
      41                 :            :  * @param[in] t duration value
      42                 :            :  * @param[in] v angular speed value
      43                 :            :  *
      44                 :            :  * @returns s = v * t as angle value
      45                 :            :  *
      46                 :            :  * \note throws a std::out_of_range exception if one of the two operands or the result of
      47                 :            :  *   the operation is not valid
      48                 :            :  */
      49                 :            : inline ad::physics::Angle operator*(ad::physics::Duration const &t, ad::physics::AngularVelocity const &v)
      50                 :            : {
      51                 :            :   return operator*(v, t);
      52                 :            : }
      53                 :            : 
      54                 :            : /*!
      55                 :            :  * @brief Arithmetic physics operation v = a * t
      56                 :            :  *
      57                 :            :  * @param[in] a angular acceleration value
      58                 :            :  * @param[in] t duration value
      59                 :            :  *
      60                 :            :  * @returns v = a * t as angular velocity value
      61                 :            :  *
      62                 :            :  * \note throws a std::out_of_range exception if one of the two operands or the result of
      63                 :            :  *   the operation is not valid
      64                 :            :  */
      65                 :            : inline ad::physics::AngularVelocity operator*(ad::physics::AngularAcceleration const &a, ad::physics::Duration const &t)
      66                 :            : {
      67                 :            :   a.ensureValid();
      68                 :            :   t.ensureValid();
      69                 :            :   ad::physics::AngularVelocity const v(static_cast<double>(a) * static_cast<double>(t));
      70                 :            :   v.ensureValid();
      71                 :            :   return v;
      72                 :            : }
      73                 :            : 
      74                 :            : /*!
      75                 :            :  * @brief Arithmetic physics operation v = t * a
      76                 :            :  *
      77                 :            :  * @param[in] t duration value
      78                 :            :  * @param[in] a angular acceleration value
      79                 :            :  *
      80                 :            :  * @returns v = t * a as angular velocity value
      81                 :            :  *
      82                 :            :  * \note throws a std::out_of_range exception if one of the two operands or the result of
      83                 :            :  *   the operation is not valid
      84                 :            :  */
      85                 :            : inline ad::physics::AngularVelocity operator*(ad::physics::Duration const &t, ad::physics::AngularAcceleration const &a)
      86                 :            : {
      87                 :            :   return operator*(a, t);
      88                 :            : }
      89                 :            : 
      90                 :            : /*!
      91                 :            :  * @brief namespace ad
      92                 :            :  */
      93                 :            : namespace ad {
      94                 :            : /*!
      95                 :            :  * @brief namespace for RSS physics datatypes and operations
      96                 :            :  */
      97                 :            : namespace physics {
      98                 :            : 
      99                 :            : /*!
     100                 :            :  * @brief constant PI
     101                 :            :  */
     102                 :            : static const Angle cPI(M_PI);
     103                 :            : 
     104                 :            : /*!
     105                 :            :  * @brief constant 2*PI
     106                 :            :  */
     107                 :            : static const Angle c2PI(2. * M_PI);
     108                 :            : 
     109                 :            : /*!
     110                 :            :  * @brief constant PI/2
     111                 :            :  */
     112                 :            : static const Angle cPI_2(M_PI_2);
     113                 :            : 
     114                 :            : /*!
     115                 :            :  * @returns normalized angle in the range of [0; 2*PI)
     116                 :            :  *
     117                 :            :  * \note throws a std::out_of_range exception if the operand is not valid
     118                 :            :  */
     119                 :          4 : inline Angle normalizeAngle(Angle const &angle)
     120                 :            : {
     121                 :          4 :   angle.ensureValid();
     122                 :          4 :   double normalizedAngle = std::fmod(static_cast<double>(angle), 2. * M_PI);
     123         [ +  + ]:          4 :   if (normalizedAngle < 0.)
     124                 :            :   {
     125                 :          1 :     normalizedAngle += 2. * M_PI;
     126                 :            :   }
     127                 :          4 :   return Angle(normalizedAngle);
     128                 :            : }
     129                 :            : 
     130                 :            : /*!
     131                 :            :  * @returns normalized angle in the range of (-PI; PI]
     132                 :            :  *
     133                 :            :  * \note throws a std::out_of_range exception if the operand is not valid
     134                 :            :  */
     135                 :          5 : inline Angle normalizeAngleSigned(Angle const &angle)
     136                 :            : {
     137                 :          5 :   angle.ensureValid();
     138                 :          5 :   double normalizedAngle = std::fmod(static_cast<double>(angle) + M_PI, 2. * M_PI);
     139         [ +  + ]:          5 :   if (normalizedAngle <= 0.)
     140                 :            :   {
     141                 :          3 :     normalizedAngle += M_PI;
     142                 :            :   }
     143                 :            :   else
     144                 :            :   {
     145                 :          2 :     normalizedAngle -= M_PI;
     146                 :            :   }
     147                 :          5 :   return Angle(normalizedAngle);
     148                 :            : }
     149                 :            : 
     150                 :            : } // namespace physics
     151                 :            : } // namespace ad
     152                 :            : 
     153                 :            : namespace std {
     154                 :            : 
     155                 :            : /*!
     156                 :            :  * @returns sine of the angle
     157                 :            :  *
     158                 :            :  * \note throws a std::out_of_range exception if the operand is not valid
     159                 :            :  */
     160                 :            : inline double sin(ad::physics::Angle const &angle)
     161                 :            : {
     162                 :            :   angle.ensureValid();
     163                 :            :   return sin(static_cast<double>(angle));
     164                 :            : }
     165                 :            : 
     166                 :            : /*!
     167                 :            :  * @returns cosine of the angle
     168                 :            :  *
     169                 :            :  * \note throws a std::out_of_range exception if the operand is not valid
     170                 :            :  */
     171                 :            : inline double cos(ad::physics::Angle const &angle)
     172                 :            : {
     173                 :            :   angle.ensureValid();
     174                 :            :   return cos(static_cast<double>(angle));
     175                 :            : }
     176                 :            : 
     177                 :            : /*!
     178                 :            :  * @returns Tangent of the angle
     179                 :            :  *
     180                 :            :  * \note throws a std::out_of_range exception if the operand is not valid
     181                 :            :  */
     182                 :            : inline double tan(ad::physics::Angle const &angle)
     183                 :            : {
     184                 :            :   angle.ensureValid();
     185                 :            :   return tan(static_cast<double>(angle));
     186                 :            : }
     187                 :            : 
     188                 :            : } // namespace std