1 #include "geom_util.hpp"
2 #include <sstream>
3 #include <iomanip>
4 #include <cmath>
5 #include "util.hpp"
6
7 namespace horizon {
project_onto_perp_bisector(const Coordd & a,const Coordd & b,const Coordd & p)8 Coordd project_onto_perp_bisector(const Coordd &a, const Coordd &b, const Coordd &p)
9 {
10 const auto c = (a + b) / 2;
11 const auto d = b - a;
12 if (d.mag_sq() == 0)
13 return p;
14 const auto u = (d.dot(c) - d.dot(p)) / d.mag_sq();
15 return p + d * u;
16 }
17
18
coord_to_string(const Coordf & pos,bool delta)19 std::string coord_to_string(const Coordf &pos, bool delta)
20 {
21 std::ostringstream ss;
22 ss.imbue(get_locale());
23 if (delta) {
24 ss << "Δ";
25 }
26 ss << "X:";
27 if (pos.x >= 0) {
28 ss << "+";
29 }
30 else {
31 ss << "−"; // this is U+2212 MINUS SIGN, has same width as +
32 }
33 ss << std::fixed << std::setprecision(3) << std::setw(7) << std::setfill('0') << std::internal
34 << std::abs(pos.x / 1e6) << " mm "; // NBSP
35 if (delta) {
36 ss << "Δ";
37 }
38 ss << "Y:";
39 if (pos.y >= 0) {
40 ss << "+";
41 }
42 else {
43 ss << "−";
44 }
45 ss << std::setw(7) << std::abs(pos.y / 1e6) << " mm"; // NBSP
46 return ss.str();
47 }
48
dim_to_string(int64_t x,bool with_sign)49 std::string dim_to_string(int64_t x, bool with_sign)
50 {
51 std::ostringstream ss;
52 ss.imbue(get_locale());
53 if (with_sign) {
54 if (x >= 0) {
55 ss << "+";
56 }
57 else {
58 ss << "−"; // this is U+2212 MINUS SIGN, has same width as +
59 }
60 }
61 ss << std::fixed << std::setprecision(3) << std::setw(7) << std::setfill('0') << std::internal << std::abs(x / 1e6)
62 << " mm"; // NBSP
63 return ss.str();
64 }
65
angle_to_string(int x,bool pos_only)66 std::string angle_to_string(int x, bool pos_only)
67 {
68 x = wrap_angle(x);
69 if (!pos_only && x > 32768)
70 x -= 65536;
71
72 std::ostringstream ss;
73 ss.imbue(get_locale());
74 if (x >= 0) {
75 ss << "+";
76 }
77 else {
78 ss << "−"; // this is U+2212 MINUS SIGN, has same width as +
79 }
80 ss << std::fixed << std::setprecision(3) << std::setw(7) << std::setfill('0') << std::internal
81 << std::abs((x / 65536.0) * 360) << " °"; // NBSP
82 return ss.str();
83 }
84
orientation_to_angle(Orientation o)85 int orientation_to_angle(Orientation o)
86 {
87 int angle = 0;
88 switch (o) {
89 case Orientation::RIGHT:
90 angle = 0;
91 break;
92 case Orientation::LEFT:
93 angle = 32768;
94 break;
95 case Orientation::UP:
96 angle = 16384;
97 break;
98 case Orientation::DOWN:
99 angle = 49152;
100 break;
101 }
102 return angle;
103 }
104
round_multiple(int64_t x,int64_t mul)105 int64_t round_multiple(int64_t x, int64_t mul)
106 {
107 return ((x + sgn(x) * mul / 2) / mul) * mul;
108 }
109
angle_to_rad(int angle)110 double angle_to_rad(int angle)
111 {
112 return (angle / 32768.) * M_PI;
113 }
114
angle_from_rad(double rad)115 int angle_from_rad(double rad)
116 {
117 return std::round(rad / M_PI * 32768.);
118 }
119
wrap_angle(int angle)120 int wrap_angle(int angle)
121 {
122 while (angle < 0) {
123 angle += 65536;
124 }
125 angle %= 65536;
126 return angle;
127 }
128
c2pi(float x)129 float c2pi(float x)
130 {
131 while (x < 0)
132 x += 2 * M_PI;
133
134 while (x > 2 * M_PI)
135 x -= 2 * M_PI;
136 return x;
137 }
138
139 } // namespace horizon
140