Line data Source code
1 : /*
2 : * vector3.cpp
3 : * Copyright (C) Andrew Tridgell 2012
4 : *
5 : * This file is free software: you can redistribute it and/or modify it
6 : * under the terms of the GNU General Public License as published by the
7 : * Free Software Foundation, either version 3 of the License, or
8 : * (at your option) any later version.
9 : *
10 : * This file is distributed in the hope that it will be useful, but
11 : * WITHOUT ANY WARRANTY; without even the implied warranty of
12 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 : * See the GNU General Public License for more details.
14 : *
15 : * You should have received a copy of the GNU General Public License along
16 : * with this program. If not, see <http://www.gnu.org/licenses/>.
17 : */
18 :
19 : #pragma GCC optimize("O2")
20 :
21 : #include "AP_Math.h"
22 :
23 : template <typename T>
24 148694938 : T Vector2<T>::length_squared() const
25 : {
26 148694938 : return (T)(x*x + y*y);
27 : }
28 :
29 : template <typename T>
30 802474674 : T Vector2<T>::length(void) const
31 : {
32 802474674 : return norm(x, y);
33 : }
34 :
35 : // limit vector to a given length. returns true if vector was limited
36 : template <typename T>
37 135901983 : bool Vector2<T>::limit_length(T max_length)
38 : {
39 135901983 : const T len = length();
40 135901983 : if ((len > max_length) && is_positive(len)) {
41 1634779 : x *= (max_length / len);
42 1634779 : y *= (max_length / len);
43 1634779 : return true;
44 : }
45 : return false;
46 : }
47 :
48 : // dot product
49 : template <typename T>
50 171840333 : T Vector2<T>::operator *(const Vector2<T> &v) const
51 : {
52 171840333 : return x*v.x + y*v.y;
53 : }
54 :
55 : // cross product
56 : template <typename T>
57 5386791 : T Vector2<T>::operator %(const Vector2<T> &v) const
58 : {
59 5386791 : return x*v.y - y*v.x;
60 : }
61 :
62 : template <typename T>
63 81589125 : Vector2<T> &Vector2<T>::operator *=(const T num)
64 : {
65 81589125 : x*=num; y*=num;
66 81589125 : return *this;
67 : }
68 :
69 : template <typename T>
70 85345359 : Vector2<T> &Vector2<T>::operator /=(const T num)
71 : {
72 85345359 : x /= num; y /= num;
73 85345359 : return *this;
74 : }
75 :
76 : template <typename T>
77 3957575 : Vector2<T> &Vector2<T>::operator -=(const Vector2<T> &v)
78 : {
79 3957575 : x -= v.x; y -= v.y;
80 3957575 : return *this;
81 : }
82 :
83 : template <typename T>
84 119670676 : bool Vector2<T>::is_nan(void) const
85 : {
86 119670676 : return isnan(x) || isnan(y);
87 : }
88 :
89 : template <typename T>
90 121159198 : bool Vector2<T>::is_inf(void) const
91 : {
92 121159198 : return isinf(x) || isinf(y);
93 : }
94 :
95 : template <typename T>
96 693548880 : Vector2<T> &Vector2<T>::operator +=(const Vector2<T> &v)
97 : {
98 693548880 : x+=v.x; y+=v.y;
99 693548880 : return *this;
100 : }
101 :
102 : template <typename T>
103 55711712 : Vector2<T> Vector2<T>::operator /(const T num) const
104 : {
105 55711712 : return Vector2<T>(x/num, y/num);
106 : }
107 :
108 : template <typename T>
109 541023477 : Vector2<T> Vector2<T>::operator *(const T num) const
110 : {
111 541023477 : return Vector2<T>(x*num, y*num);
112 : }
113 :
114 : template <typename T>
115 980220953 : Vector2<T> Vector2<T>::operator -(const Vector2<T> &v) const
116 : {
117 980220953 : return Vector2<T>(x-v.x, y-v.y);
118 : }
119 :
120 : template <typename T>
121 357453827 : Vector2<T> Vector2<T>::operator +(const Vector2<T> &v) const
122 : {
123 357453827 : return Vector2<T>(x+v.x, y+v.y);
124 : }
125 :
126 : template <typename T>
127 557164 : Vector2<T> Vector2<T>::operator -(void) const
128 : {
129 557164 : return Vector2<T>(-x,-y);
130 : }
131 :
132 : template <typename T>
133 7109295 : bool Vector2<T>::operator ==(const Vector2<T> &v) const
134 : {
135 7109295 : return (is_equal(x,v.x) && is_equal(y,v.y));
136 : }
137 :
138 : template <typename T>
139 14 : bool Vector2<T>::operator !=(const Vector2<T> &v) const
140 : {
141 14 : return (!is_equal(x,v.x) || !is_equal(y,v.y));
142 : }
143 :
144 : template <typename T>
145 121859116 : T Vector2<T>::angle(const Vector2<T> &v2) const
146 : {
147 121859116 : const T len = this->length() * v2.length();
148 121859116 : if (len <= 0) {
149 : return 0.0f;
150 : }
151 121859114 : const T cosv = ((*this)*v2) / len;
152 121859114 : if (cosv >= 1) {
153 : return 0.0f;
154 : }
155 32793728 : if (cosv <= -1) {
156 : return M_PI;
157 : }
158 32766606 : return acosF(cosv);
159 : }
160 :
161 : template <typename T>
162 6830627 : T Vector2<T>::angle(void) const
163 : {
164 6830627 : return atan2F(y, x);
165 : }
166 :
167 : // find the intersection between two line segments
168 : // returns true if they intersect, false if they do not
169 : // the point of intersection is returned in the intersection argument
170 : template <typename T>
171 439918 : bool Vector2<T>::segment_intersection(const Vector2<T>& seg1_start, const Vector2<T>& seg1_end, const Vector2<T>& seg2_start, const Vector2<T>& seg2_end, Vector2<T>& intersection)
172 : {
173 : // implementation borrowed from http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
174 439918 : const Vector2<T> r1 = seg1_end - seg1_start;
175 439918 : const Vector2<T> r2 = seg2_end - seg2_start;
176 439918 : const Vector2<T> ss2_ss1 = seg2_start - seg1_start;
177 439920 : const T r1xr2 = r1 % r2;
178 439918 : const T q_pxr = ss2_ss1 % r1;
179 439918 : if (::is_zero(r1xr2)) {
180 : // either collinear or parallel and non-intersecting
181 : return false;
182 : } else {
183 : // t = (q - p) * s / (r * s)
184 : // u = (q - p) * r / (r * s)
185 439908 : const T t = (ss2_ss1 % r2) / r1xr2;
186 439909 : const T u = q_pxr / r1xr2;
187 439909 : if ((u >= 0) && (u <= 1) && (t >= 0) && (t <= 1)) {
188 : // lines intersect
189 : // t can be any non-negative value because (p, p + r) is a ray
190 : // u must be between 0 and 1 because (q, q + s) is a line segment
191 3447 : intersection = seg1_start + (r1*t);
192 3447 : return true;
193 : } else {
194 : // non-parallel and non-intersecting
195 : return false;
196 : }
197 : }
198 : }
199 :
200 : // find the intersection between a line segment and a circle
201 : // returns true if they intersect and intersection argument is updated with intersection closest to seg_start
202 : // solution adapted from http://stackoverflow.com/questions/1073336/circle-line-segment-collision-detection-algorithm
203 : template <typename T>
204 269096666 : bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, T radius, Vector2<T>& intersection)
205 : {
206 : // calculate segment start and end as offsets from circle's center
207 269096666 : const Vector2<T> seg_start_local = seg_start - circle_center;
208 :
209 : // calculate vector from start to end
210 269096666 : const Vector2<T> seg_end_minus_start = seg_end - seg_start;
211 :
212 269096666 : const T a = sq(seg_end_minus_start.x) + sq(seg_end_minus_start.y);
213 269096666 : const T b = 2 * ((seg_end_minus_start.x * seg_start_local.x) + (seg_end_minus_start.y * seg_start_local.y));
214 269096666 : const T c = sq(seg_start_local.x) + sq(seg_start_local.y) - sq(radius);
215 :
216 : // check for invalid data
217 269096666 : if (::is_zero(a) || isnan(a) || isnan(b) || isnan(c)) {
218 : return false;
219 : }
220 :
221 269096664 : const T delta = sq(b) - (4.0f * a * c);
222 :
223 269096664 : if (isnan(delta)) {
224 : return false;
225 : }
226 :
227 : // check for invalid delta (i.e. discriminant)
228 269096664 : if (delta < 0.0f) {
229 : return false;
230 : }
231 :
232 2860129 : const T delta_sqrt = sqrtF(delta);
233 2860129 : const T t1 = (-b + delta_sqrt) / (2.0f * a);
234 2860129 : const T t2 = (-b - delta_sqrt) / (2.0f * a);
235 :
236 : // Three hit cases:
237 : // -o-> --|--> | | --|->
238 : // Impale(t1 hit,t2 hit), Poke(t1 hit,t2>1), ExitWound(t1<0, t2 hit),
239 :
240 : // Three miss cases:
241 : // -> o o -> | -> |
242 : // FallShort (t1>1,t2>1), Past (t1<0,t2<0), CompletelyInside(t1<0, t2>1)
243 :
244 : // intersection = NEW_NOTHROW Vector3(E.x + t1 * d.x, secondPoint.y, E.y + t1 * d.y);
245 : // intersection.x = seg_start.x + t1 * seg_end_minus_start.x;
246 : // intersection.y = seg_start.y + t1 * seg_end_minus_start.y;
247 :
248 2860129 : if ((t1 >= 0.0f) && (t1 <= 1.0f)) {
249 : // t1 is the intersection, and it is closer than t2 (since t1 uses -b - discriminant)
250 : // Impale, Poke
251 1412203 : intersection = seg_start + (seg_end_minus_start * t1);
252 1412203 : return true;
253 : }
254 :
255 : // here t1 did not intersect so we are either started inside the sphere or completely past it
256 1447926 : if ((t2 >= 0.0f) && (t2 <= 1.0f)) {
257 : // ExitWound
258 0 : intersection = seg_start + (seg_end_minus_start * t2);
259 0 : return true;
260 : }
261 :
262 : // no intersection: FallShort, Past or CompletelyInside
263 : return false;
264 : }
265 :
266 : // normalizes this vector
267 : template <typename T>
268 84141806 : void Vector2<T>::normalize()
269 : {
270 84141806 : *this /= length();
271 84141806 : }
272 :
273 : // returns the normalized vector
274 : template <typename T>
275 4335891 : Vector2<T> Vector2<T>::normalized() const
276 : {
277 4335891 : return *this/length();
278 : }
279 :
280 : // reflects this vector about n
281 : template <typename T>
282 6 : void Vector2<T>::reflect(const Vector2<T> &n)
283 : {
284 6 : const Vector2<T> orig(*this);
285 6 : project(n);
286 6 : *this = *this*2 - orig;
287 6 : }
288 :
289 : // projects this vector onto v
290 : template <typename T>
291 8 : void Vector2<T>::project(const Vector2<T> &v)
292 : {
293 8 : *this= v * (*this * v)/(v*v);
294 8 : }
295 :
296 : // returns this vector projected onto v
297 : template <typename T>
298 178460 : Vector2<T> Vector2<T>::projected(const Vector2<T> &v) const
299 : {
300 178460 : return v * (*this * v)/(v*v);
301 : }
302 :
303 : // extrapolate position given bearing (in degrees) and distance
304 : template <typename T>
305 1980 : void Vector2<T>::offset_bearing(T bearing, T distance)
306 : {
307 1980 : x += cosF(radians(bearing)) * distance;
308 1980 : y += sinF(radians(bearing)) * distance;
309 1980 : }
310 :
311 : // given a position pos_delta and a velocity v1 produce a vector
312 : // perpendicular to v1 maximising distance from p1
313 : template <typename T>
314 16 : Vector2<T> Vector2<T>::perpendicular(const Vector2<T> &pos_delta, const Vector2<T> &v1)
315 : {
316 16 : const Vector2<T> perpendicular1 = Vector2<T>(-v1[1], v1[0]);
317 16 : const Vector2<T> perpendicular2 = Vector2<T>(v1[1], -v1[0]);
318 16 : const T d1 = perpendicular1 * pos_delta;
319 16 : const T d2 = perpendicular2 * pos_delta;
320 16 : if (d1 > d2) {
321 6 : return perpendicular1;
322 : }
323 10 : return perpendicular2;
324 : }
325 :
326 : /*
327 : * Returns the point closest to p on the line segment (v,w).
328 : *
329 : * Comments and implementation taken from
330 : * http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment
331 : */
332 : template <typename T>
333 18767416 : Vector2<T> Vector2<T>::closest_point(const Vector2<T> &p, const Vector2<T> &v, const Vector2<T> &w)
334 : {
335 : // length squared of line segment
336 18767416 : const T l2 = (v - w).length_squared();
337 18767416 : if (l2 < FLT_EPSILON) {
338 : // v == w case
339 2 : return v;
340 : }
341 : // Consider the line extending the segment, parameterized as v + t (w - v).
342 : // We find projection of point p onto the line.
343 : // It falls where t = [(p-v) . (w-v)] / |w-v|^2
344 : // We clamp t from [0,1] to handle points outside the segment vw.
345 18767414 : const T t = ((p - v) * (w - v)) / l2;
346 18767414 : if (t <= 0) {
347 4891954 : return v;
348 13875460 : } else if (t >= 1) {
349 4682618 : return w;
350 : } else {
351 9192842 : return v + (w - v)*t;
352 : }
353 : }
354 :
355 : /*
356 : * Returns the point closest to p on the line segment (0,w).
357 : *
358 : * this is a simplification of closest point with a general segment, with v=(0,0)
359 : */
360 : template <typename T>
361 147342 : Vector2<T> Vector2<T>::closest_point(const Vector2<T> &p, const Vector2<T> &w)
362 : {
363 : // length squared of line segment
364 147342 : const T l2 = w.length_squared();
365 147342 : if (l2 < FLT_EPSILON) {
366 : // v == w case
367 12 : return w;
368 : }
369 147330 : const T t = (p * w) / l2;
370 147330 : if (t <= 0) {
371 32796 : return Vector2<T>(0,0);
372 114534 : } else if (t >= 1) {
373 55422 : return w;
374 : } else {
375 59112 : return w*t;
376 : }
377 : }
378 :
379 : // closest distance between a line segment and a point
380 : // https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments
381 : template <typename T>
382 147103 : T Vector2<T>::closest_distance_between_line_and_point_squared(const Vector2<T> &w1,
383 : const Vector2<T> &w2,
384 : const Vector2<T> &p)
385 : {
386 147103 : return closest_distance_between_radial_and_point_squared(w2-w1, p-w1);
387 : }
388 :
389 : // w1 and w2 define a line segment
390 : // p is a point
391 : // returns the closest distance between the line segment and the point
392 : template <typename T>
393 13717 : T Vector2<T>::closest_distance_between_line_and_point(const Vector2<T> &w1,
394 : const Vector2<T> &w2,
395 : const Vector2<T> &p)
396 : {
397 13717 : return sqrtF(closest_distance_between_line_and_point_squared(w1, w2, p));
398 : }
399 :
400 : // a1->a2 and b2->v2 define two line segments
401 : // returns the square of the closest distance between the two line segments
402 : // see https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments
403 : template <typename T>
404 33346 : T Vector2<T>::closest_distance_between_lines_squared(const Vector2<T> &a1,
405 : const Vector2<T> &a2,
406 : const Vector2<T> &b1,
407 : const Vector2<T> &b2)
408 : {
409 33346 : const T dist1 = Vector2<T>::closest_distance_between_line_and_point_squared(b1,b2,a1);
410 33346 : const T dist2 = Vector2<T>::closest_distance_between_line_and_point_squared(b1,b2,a2);
411 33346 : const T dist3 = Vector2<T>::closest_distance_between_line_and_point_squared(a1,a2,b1);
412 33346 : const T dist4 = Vector2<T>::closest_distance_between_line_and_point_squared(a1,a2,b2);
413 33346 : const T m1 = MIN(dist1,dist2);
414 33346 : const T m2 = MIN(dist3,dist4);
415 33346 : return MIN(m1,m2);
416 : }
417 :
418 : // w defines a line segment from the origin
419 : // p is a point
420 : // returns the square of the closest distance between the radial and the point
421 : template <typename T>
422 147330 : T Vector2<T>::closest_distance_between_radial_and_point_squared(const Vector2<T> &w,
423 : const Vector2<T> &p)
424 : {
425 147330 : const Vector2<T> closest = closest_point(p, w);
426 147330 : return (closest - p).length_squared();
427 : }
428 :
429 : // w defines a line segment from the origin
430 : // p is a point
431 : // returns the closest distance between the radial and the point
432 : template <typename T>
433 225 : T Vector2<T>::closest_distance_between_radial_and_point(const Vector2<T> &w,
434 : const Vector2<T> &p)
435 : {
436 225 : return sqrtF(closest_distance_between_radial_and_point_squared(w,p));
437 : }
438 :
439 : // rotate vector by angle in radians
440 : template <typename T>
441 1240345 : void Vector2<T>::rotate(T angle_rad)
442 : {
443 1240345 : const T cs = cosF(angle_rad);
444 1240345 : const T sn = sinF(angle_rad);
445 1240345 : T rx = x * cs - y * sn;
446 1240345 : T ry = x * sn + y * cs;
447 1240345 : x = rx;
448 1240345 : y = ry;
449 1240345 : }
450 :
451 : // define for float and double
452 : template class Vector2<float>;
453 : template class Vector2<double>;
454 :
455 : // define some ops for int and long
456 : template bool Vector2<long>::operator ==(const Vector2<long> &v) const;
457 : template bool Vector2<long>::operator !=(const Vector2<long> &v) const;
458 : template bool Vector2<int>::operator ==(const Vector2<int> &v) const;
459 : template bool Vector2<int>::operator !=(const Vector2<int> &v) const;
460 : template int32_t Vector2l::closest_distance_between_line_and_point_squared(Vector2l const&, Vector2l const&, Vector2l const&);
|
