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
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
|
#pragma once
#include "point2d.hpp"
#include "../base/matrix.hpp"
#include "../std/cmath.hpp"
namespace ang
{
template <typename T>
struct Angle
{
T m_val;
T m_sin;
T m_cos;
public:
Angle() : m_val(0), m_sin(0), m_cos(1) {}
Angle(T const & val) : m_val(val), m_sin(::sin(val)), m_cos(::cos(val)) {}
Angle(T const & sin, T const & cos) : m_val(::atan2(sin, cos)), m_sin(sin), m_cos(cos) {}
T const & val() const
{
return m_val;
}
T const & sin() const
{
return m_sin;
}
T const & cos() const
{
return m_cos;
}
Angle<T> const & operator*=(math::Matrix<T, 3, 3> const & m)
{
m2::Point<T> pt0(0, 0);
m2::Point<T> pt1(m_cos, m_sin);
pt1 *= m;
pt0 *= m;
m_val = atan2(pt1.y - pt0.y, pt1.x - pt0.x);
m_sin = sin(m_val);
m_cos = cos(m_val);
return this;
}
};
typedef Angle<double> AngleD;
typedef Angle<float> AngleF;
template <typename T>
Angle<T> const operator*(Angle<T> const & a, math::Matrix<T, 3, 3> const & m)
{
m2::Point<T> pt0(0, 0);
m2::Point<T> pt1(a.cos(), a.sin());
pt1 *= m;
pt0 *= m;
return Angle<T>(atan2(pt1.y - pt0.y, pt1.x - pt0.x));
}
/// Returns an angle of vector [p1, p2] from x-axis directed to y-axis.
/// Angle is in range [-pi, pi].
template <typename T>
inline T AngleTo(m2::Point<T> const & p1, m2::Point<T> const & p2)
{
return atan2(p2.y - p1.y, p2.x - p1.x);
}
inline double RadToDegree(double rad)
{
return rad / math::pi * 180.0;
}
inline double DegreeToRad(double degree)
{
return degree / 180.0 * math::pi;
}
inline double GetShortestDistance(double rad1, double rad2)
{
double period = 2 * math::pi;
rad1 = fmod(rad1, period);
rad2 = fmod(rad2, period);
double res = 0;
if (fabs(rad1 - rad2) > math::pi)
{
if (rad1 > rad2)
res = 2 * math::pi - (rad1 - rad2);
else
res = - 2 * math::pi + (rad2 - rad1);
}
else
res = rad2 - rad1;
return res;
}
inline 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;
}
/// Average angle calcker. Can't find any suitable solution, so decided to do like this:
/// Avg(i) = Avg(Avg(i-1), Ai);
class AverageCalc
{
double m_ang;
bool m_isEmpty;
public:
AverageCalc() : m_ang(0.0), m_isEmpty(true) {}
void Add(double a)
{
m_ang = (m_isEmpty ? a : GetMiddleAngle(m_ang, a));
m_isEmpty = false;
}
double GetAverage() const
{
return m_ang;
}
};
}
|
