geometry/angles.cpp


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

#include "geometry/angles.hpp"


namespace ang
{

double AngleIn2PI(double ang)
{
  double const period = 2.0 * math::pi;
  ang = fmod(ang, period);
  if (ang < 0.0)
    ang += period;
  
  if (my::AlmostEqualULPs(period, ang))
    return 0.0;

  return ang;
}

double GetShortestDistance(double rad1, double rad2)
{
  double const period = 2.0 * math::pi;
  rad1 = fmod(rad1, period);
  rad2 = fmod(rad2, period);

  double res = rad2 - rad1;
  if (fabs(res) > math::pi)
  {
    if (res < 0.0)
      res = period + res;
    else
      res = - period + res;
  }
  return res;
}

double GetMiddleAngle(double a1, double a2)
{
  double ang = (a1 + a2) / 2.0;

  if (fabs(a1 - a2) > math::pi)
  {
    if (ang > 0.0)
      ang -= math::pi;
    else
      ang += math::pi;

  }
  return ang;
}

}