Line data Source code
1 : #include "Calc.hpp"
2 :
3 : #include <math.h>
4 :
5 : #include <cmath>
6 :
7 : namespace Soldank::Calc
8 : {
9 3 : float Distance(const glm::vec2& p1, const glm::vec2& p2)
10 : {
11 3 : return glm::length(p2 - p1);
12 : }
13 :
14 9 : float SquareDistance(const glm::vec2& p1, const glm::vec2& p2)
15 : {
16 9 : glm::vec2 diff = p1 - p2;
17 18 : return glm::dot(diff, diff);
18 : }
19 :
20 2 : float Vec2Length(const glm::vec2& v)
21 : {
22 2 : return glm::length(v);
23 : }
24 :
25 20 : glm::vec2 Vec2Normalize(const glm::vec2& v)
26 : {
27 20 : auto magnitude = glm::length(v);
28 20 : if (magnitude < 0.001) {
29 9 : return { 0.0F, 0.0F };
30 : }
31 :
32 11 : return v / magnitude;
33 : }
34 :
35 4 : float Vec2Angle(const glm::vec2& v)
36 : {
37 4 : return (float)(-atan2(0.0, 1.0) - atan2(v.y, v.x));
38 : }
39 2 : glm::vec2 Vec2Scale(const glm::vec2& v, float scale)
40 : {
41 2 : return v * scale;
42 : }
43 :
44 3 : float PointLineDistance(const glm::vec2& p1, const glm::vec2& p2, const glm::vec2& p3)
45 : {
46 3 : auto u = ((p3.x - p1.x) * (p2.x - p1.x) + (p3.y - p1.y) * (p2.y - p1.y)) /
47 3 : (std::pow(p2.x - p1.x, 2) + std::pow(p2.y - p1.y, 2));
48 :
49 3 : auto x = p1.x + u * (p2.x - p1.x);
50 3 : auto y = p1.y + u * (p2.y - p1.y);
51 :
52 3 : return (float)std::sqrt(std::pow(x - p3.x, 2) + std::pow(y - p3.y, 2));
53 : }
54 :
55 5 : glm::vec2 Lerp(glm::vec2 a, glm::vec2 b, float t)
56 : {
57 5 : return glm::mix(a, b, t);
58 : }
59 :
60 3 : float Lerp(float a, float b, float t)
61 : {
62 3 : return std::lerp(a, b, t);
63 : }
64 :
65 1 : std::vector<glm::vec2> LineCircleCollisionPoints(glm::vec2 line1,
66 : glm::vec2 line2,
67 : glm::vec2 circle_center,
68 : float radius)
69 : {
70 1 : float diff_x = line2.x - line1.x;
71 1 : float diff_y = line2.y - line1.y;
72 :
73 1 : if (std::abs(diff_x) < 0.00001F && std::abs(diff_y) < 0.00001F) {
74 : // Floating point inaccuracy
75 0 : return {};
76 : }
77 :
78 : // If the angle of the bullet is bigger than 45 degrees,
79 : // flip the coordinate system.
80 : // This algorithm deals with lines being nearly horizontal just fine,
81 : // but nearly vertical would cause a havoc, as vertical line is not a function.
82 1 : bool flipped = false;
83 1 : if (std::abs(diff_y) > std::abs(diff_x)) {
84 0 : flipped = true;
85 0 : std::swap(line1.x, line1.y);
86 0 : std::swap(line2.x, line2.y);
87 0 : std::swap(circle_center.x, circle_center.y);
88 0 : std::swap(diff_x, diff_x);
89 : }
90 :
91 : // Line equation: ax + b - y = 0. given x1, y1, x2, y2, let's calculate a and b
92 : // a = (y1 - y2)/(x1 - x2)
93 1 : float a = diff_y / diff_x;
94 : // b := y - ax
95 1 : float b = line1.y - a * line1.x;
96 : // Circle equation: (x - x1)^2 + (y - y1)^2 - r^2 = 0
97 : // Now we need to solve: (x - x1)^2 + (y - y1)^2 - r^2 = ax + b - y
98 : // Simplyfing above: (a^2 + 1)x^2 + 2(ab − ay1 − x1)x + (y1^2 − r^2 + x1^2 − 2by1b^2)=0
99 : // now, since this is a standard Ax^2 + Bx + C equation, we find x and y using
100 : // x = (-B +/- sqrt(B^2 - 4ac))/(2A)
101 : // A = (a^2 + 1)
102 1 : float a1 = (a * a) + 1;
103 : // B = 2(ab - ay1 - x1)
104 1 : float b1 = 2 * (a * b - a * circle_center.y - circle_center.x);
105 : // C = y1^2 − r^2 + x1^2 − 2by1 + b^2
106 1 : float c1 = (circle_center.y * circle_center.y) - (radius * radius) +
107 1 : (circle_center.x * circle_center.x) - 2 * b * circle_center.y + (b * b);
108 : // delta = B^2 - 4AC;
109 1 : float delta = (b1 * b1) - 4 * a1 * c1;
110 : // having x1 and x2 result, we can calculate y1 and y2 from y = a * x + b
111 :
112 : // if delta < 0, no intersection
113 1 : if (delta < 0) {
114 1 : return {};
115 : }
116 :
117 0 : float minx = NAN;
118 0 : float miny = NAN;
119 0 : float maxx = NAN;
120 0 : float maxy = NAN;
121 :
122 0 : if (line1.x < line2.x) {
123 0 : minx = line1.x;
124 0 : maxx = line2.x;
125 : } else {
126 0 : minx = line2.x;
127 0 : maxx = line1.x;
128 : }
129 :
130 0 : if (line1.y < line2.y) {
131 0 : miny = line1.y;
132 0 : maxy = line2.y;
133 : } else {
134 0 : miny = line2.y;
135 0 : maxy = line1.y;
136 : }
137 :
138 0 : std::vector<glm::vec2> result;
139 : glm::vec2 intersect;
140 : // we don't care about a case of delta = 0 as it's extremaly rare,
141 : // also this will handle it fine, just less effecient
142 0 : float sqrtdelta = std::sqrt(delta);
143 0 : float a2 = 2 * a1;
144 0 : intersect.x = (-b1 - sqrtdelta) / a2;
145 0 : intersect.y = a * intersect.x + b;
146 : // we know that infinite line does intersect the circle, now let's see if our part does
147 0 : auto in_range = [](float a, float b, float c) { return a >= b && a <= c; };
148 0 : if (in_range(intersect.x, minx, maxx) && in_range(intersect.y, miny, maxy)) {
149 0 : if (flipped) {
150 0 : std::swap(intersect.x, intersect.y);
151 : }
152 :
153 0 : result.push_back(intersect);
154 : }
155 :
156 0 : intersect.x = (-b1 + sqrtdelta) / a2;
157 0 : intersect.y = a * intersect.x + b;
158 0 : if (in_range(intersect.x, minx, maxx) && in_range(intersect.y, miny, maxy)) {
159 0 : if (flipped) {
160 0 : std::swap(intersect.x, intersect.y);
161 : }
162 :
163 0 : result.push_back(intersect);
164 : }
165 :
166 0 : return result;
167 0 : }
168 :
169 3 : std::optional<glm::vec2> LineCircleCollision(const glm::vec2& start_point,
170 : const glm::vec2& end_point,
171 : const glm::vec2& circle_center,
172 : float radius)
173 : {
174 3 : float r2 = radius * radius;
175 :
176 3 : if (SquareDistance(start_point, circle_center) <= r2) {
177 0 : return start_point;
178 : }
179 :
180 3 : if (SquareDistance(end_point, circle_center) <= r2) {
181 2 : return end_point;
182 : }
183 :
184 : auto intersection_result =
185 1 : LineCircleCollisionPoints(start_point, end_point, circle_center, radius);
186 1 : if (!intersection_result.empty()) {
187 0 : if (intersection_result.size() == 2 &&
188 0 : SquareDistance(intersection_result[0], start_point) >
189 0 : SquareDistance(intersection_result[1], start_point)) {
190 :
191 0 : return intersection_result[1];
192 : }
193 :
194 0 : return intersection_result[0];
195 : }
196 1 : return std::nullopt;
197 1 : }
198 :
199 0 : bool SegmentsIntersect(glm::vec2 a, glm::vec2 b, glm::vec2 c, glm::vec2 d)
200 : {
201 0 : float segment_ab_center_x = b.x - a.x;
202 0 : float segment_ab_center_y = b.y - a.y;
203 0 : float segment_cd_center_x = d.x - c.x;
204 0 : float segment_cd_center_y = d.y - c.y;
205 :
206 0 : float s = NAN;
207 0 : float t = NAN;
208 0 : s = (-segment_ab_center_y * (a.x - c.x) + segment_ab_center_x * (a.y - c.y)) /
209 0 : (-segment_cd_center_x * segment_ab_center_y + segment_ab_center_x * segment_cd_center_y);
210 0 : t = (segment_cd_center_x * (a.y - c.y) - segment_cd_center_y * (a.x - c.x)) /
211 0 : (-segment_cd_center_x * segment_ab_center_y + segment_ab_center_x * segment_cd_center_y);
212 :
213 0 : return s >= 0 && s <= 1 && t >= 0 && t <= 1;
214 : }
215 :
216 0 : float Det(glm::vec2 a, glm::vec2 b, glm::vec2 c)
217 : {
218 0 : return a.x * b.y + a.y * c.x + b.x * c.y - c.x * b.y - c.y * a.x - b.x * a.y;
219 : }
220 :
221 0 : float GetAngle(const glm::vec2& p1, const glm::vec2& p2)
222 : {
223 0 : return std::atan2(p1.y - p2.y, p1.x - p2.x);
224 : }
225 :
226 0 : glm::vec2 RotatePoint(const glm::vec2& point, const glm::vec2& origin, float rotation)
227 : {
228 0 : glm::vec4 origin4 = { origin.x, origin.y, 1.0F, 1.0F };
229 0 : glm::vec4 point4 = { point.x, point.y, 1.0F, 1.0F };
230 0 : glm::vec4 new_position = point4 - origin4;
231 0 : glm::mat4 transform(1);
232 0 : transform = glm::rotate(transform, rotation, glm::vec3(0.0, 0.0, 1.0));
233 0 : new_position = transform * new_position;
234 0 : new_position += origin4;
235 : glm::vec2 result;
236 0 : result.x = new_position.x;
237 0 : result.y = new_position.y;
238 0 : return result;
239 : }
240 :
241 0 : glm::vec2 ScalePoint(const glm::vec2& point, const glm::vec2& origin, const glm::vec2& scale_factor)
242 : {
243 0 : glm::vec2 result = point - origin;
244 0 : result.x *= scale_factor.x;
245 0 : result.y *= scale_factor.y;
246 0 : result += origin;
247 0 : return result;
248 : }
249 : } // namespace Soldank::Calc
|
