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
|
#pragma once
#include "geometry/point2d.hpp"
#include "base/matrix.hpp"
#include <cmath>
#include <string>
namespace ang
{
template <typename T>
class Angle
{
public:
Angle() = default;
explicit Angle(T const & val) : m_val(val), m_sin(std::sin(val)), m_cos(std::cos(val)) {}
Angle(T const & sin, T const & cos) : m_val(std::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 = m2::Point<T>::Zero();
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 = std::sin(m_val);
m_cos = std::cos(m_val);
return *this;
}
friend std::string DebugPrint(Angle<T> const & ang) { return DebugPrint(ang.m_val); }
private:
T m_val = 0;
T m_sin = 0;
T m_cos = 1;
};
using AngleD = Angle<double>;
using AngleF = Angle<float>;
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>
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>
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>
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
{
public:
void Add(double a)
{
m_ang = (m_isEmpty ? a : GetMiddleAngle(m_ang, a));
m_isEmpty = false;
}
double GetAverage() const { return m_ang; }
private:
double m_ang = 0.0;
bool m_isEmpty = true;
};
} // namespace ang
|
