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
|
#pragma once
#include "geometry/point2d.hpp"
#include "base/matrix.hpp"
#include "std/cmath.hpp"
namespace ang
{
template <typename T>
class 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;
}
friend string DebugPrint(Angle<T> const & ang)
{
return DebugPrint(ang.m_val);
}
};
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)
{
Angle<T> ret(a);
ret *= m;
return ret;
}
/// 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);
}
/// Returns an angle from vector [p, p1] to vector [p, p2]. A counterclockwise rotation.
/// Angle is in range [0, 2 * pi]
template <typename T>
inline T TwoVectorsAngle(m2::Point<T> const & p, m2::Point<T> const & p1, m2::Point<T> const & p2)
{
T a = ang::AngleTo(p, p2) - ang::AngleTo(p, p1);
while (a < 0)
a += math::twicePi;
return a;
}
double AngleIn2PI(double ang);
/// @return Oriented angle (<= PI) from rad1 to rad2.
/// >0 - clockwise, <0 - counterclockwise
double GetShortestDistance(double rad1, double rad2);
double GetMiddleAngle(double a1, double a2);
/// @return If north is zero - azimuth between geographic north and [p1, p2] vector is returned.
/// If north is not zero - it is treated as azimuth of some custom direction(eg magnetic north or some other direction),
/// and azimuth between that direction and [p1, p2] is returned.
/// Azimuth is in range [0, 2 * pi]
template <typename T>
inline T Azimuth(m2::Point<T> const & p1, m2::Point<T> const & p2, T north = 0)
{
T azimuth = math::pi2 - (AngleTo(p1, p2) + north);
if (azimuth < 0)
azimuth += math::twicePi;
return azimuth;
}
/// 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;
}
};
}
|
