AngleOperation.hpp

Go to the documentation of this file.

// ----------------- BEGIN LICENSE BLOCK ---------------------------------
//
// Copyright (C) 2020-2021 Intel Corporation
//
// SPDX-License-Identifier: MIT
//
// ----------------- END LICENSE BLOCK -----------------------------------
 
#pragma once
 
#include <cmath>
#include "ad/physics/Types.hpp"
 
inline ad::physics::Angle operator*(ad::physics::AngularVelocity const &v, ad::physics::Duration const &t)
{
  v.ensureValid();
  t.ensureValid();
  ad::physics::Angle const s(static_cast<double>(v) * static_cast<double>(t));
  s.ensureValid();
  return s;
}
 
inline ad::physics::Angle operator*(ad::physics::Duration const &t, ad::physics::AngularVelocity const &v)
{
  return operator*(v, t);
}
 
inline ad::physics::AngularVelocity operator*(ad::physics::AngularAcceleration const &a, ad::physics::Duration const &t)
{
  a.ensureValid();
  t.ensureValid();
  ad::physics::AngularVelocity const v(static_cast<double>(a) * static_cast<double>(t));
  v.ensureValid();
  return v;
}
 
inline ad::physics::AngularVelocity operator*(ad::physics::Duration const &t, ad::physics::AngularAcceleration const &a)
{
  return operator*(a, t);
}
 
namespace ad {
namespace physics {
 
static const Angle cPI(M_PI);
 
static const Angle c2PI(2. * M_PI);
 
static const Angle cPI_2(M_PI_2);
 
inline Angle normalizeAngle(Angle const &angle)
{
  angle.ensureValid();
  double normalizedAngle = std::fmod(static_cast<double>(angle), 2. * M_PI);
  if (normalizedAngle < 0.)
  {
    normalizedAngle += 2. * M_PI;
  }
  return Angle(normalizedAngle);
}
 
inline Angle normalizeAngleSigned(Angle const &angle)
{
  angle.ensureValid();
  double normalizedAngle = std::fmod(static_cast<double>(angle) + M_PI, 2. * M_PI);
  if (normalizedAngle <= 0.)
  {
    normalizedAngle += M_PI;
  }
  else
  {
    normalizedAngle -= M_PI;
  }
  return Angle(normalizedAngle);
}
 
} // namespace physics
} // namespace ad
 
namespace std {
 
inline double sin(ad::physics::Angle const &angle)
{
  angle.ensureValid();
  return sin(static_cast<double>(angle));
}
 
inline double cos(ad::physics::Angle const &angle)
{
  angle.ensureValid();
  return cos(static_cast<double>(angle));
}
 
inline double tan(ad::physics::Angle const &angle)
{
  angle.ensureValid();
  return tan(static_cast<double>(angle));
}
 
} // namespace std