Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : // Copyright 2010 Michael Smith, all rights reserved.
17 :
18 : // Derived closely from:
19 : /****************************************
20 : * 2D Vector Classes
21 : * By Bill Perone ([email protected])
22 : * Original: 9-16-2002
23 : * Revised: 19-11-2003
24 : * 18-12-2003
25 : * 06-06-2004
26 : *
27 : * Copyright 2003, This code is provided "as is" and you can use it freely as long as
28 : * credit is given to Bill Perone in the application it is used in
29 : ****************************************/
30 : #pragma once
31 :
32 : #ifndef MATH_CHECK_INDEXES
33 : #define MATH_CHECK_INDEXES 0
34 : #endif
35 : #if MATH_CHECK_INDEXES
36 : #include <assert.h>
37 : #endif
38 :
39 : #include <cmath>
40 : #include <float.h>
41 : #include <AP_Common/AP_Common.h>
42 : #include "ftype.h"
43 :
44 : template <typename T>
45 : struct Vector2
46 : {
47 : T x, y;
48 :
49 : // trivial ctor
50 541912594 : constexpr Vector2()
51 541912594 : : x(0)
52 541912594 : , y(0) {}
53 :
54 : // setting ctor
55 3926949426 : constexpr Vector2(const T x0, const T y0)
56 3926949426 : : x(x0)
57 3926949426 : , y(y0) {}
58 :
59 : // test for equality
60 : bool operator ==(const Vector2<T> &v) const;
61 :
62 : // test for inequality
63 : bool operator !=(const Vector2<T> &v) const;
64 :
65 : // negation
66 : Vector2<T> operator -(void) const;
67 :
68 : // addition
69 : Vector2<T> operator +(const Vector2<T> &v) const;
70 :
71 : // subtraction
72 : Vector2<T> operator -(const Vector2<T> &v) const;
73 :
74 : // uniform scaling
75 : Vector2<T> operator *(const T num) const;
76 :
77 : // uniform scaling
78 : Vector2<T> operator /(const T num) const;
79 :
80 : // addition
81 : Vector2<T> &operator +=(const Vector2<T> &v);
82 :
83 : // subtraction
84 : Vector2<T> &operator -=(const Vector2<T> &v);
85 :
86 : // uniform scaling
87 : Vector2<T> &operator *=(const T num);
88 :
89 : // uniform scaling
90 : Vector2<T> &operator /=(const T num);
91 :
92 : // dot product
93 : T operator *(const Vector2<T> &v) const;
94 :
95 : // dot product (same as above but a more easily understood name)
96 5115961 : T dot(const Vector2<T> &v) const {
97 5115961 : return *this * v;
98 : }
99 :
100 : // cross product
101 : T operator %(const Vector2<T> &v) const;
102 :
103 : // computes the angle between this vector and another vector
104 : // returns 0 if the vectors are parallel, and M_PI if they are antiparallel
105 : T angle(const Vector2<T> &v2) const;
106 :
107 : // computes the angle of this vector in radians, from -M_PI to M_PI,
108 : // from a unit vector(1,0); a (1,1) vector's angle is +M_PI/4
109 : T angle(void) const;
110 :
111 : // check if any elements are NAN
112 : bool is_nan(void) const WARN_IF_UNUSED;
113 :
114 : // check if any elements are infinity
115 : bool is_inf(void) const WARN_IF_UNUSED;
116 :
117 : // check if all elements are zero
118 42 : bool is_zero(void) const WARN_IF_UNUSED {
119 42 : return x == 0 && y == 0;
120 : }
121 :
122 : // allow a vector2 to be used as an array, 0 indexed
123 275810895 : T & operator[](uint8_t i) {
124 275810895 : T *_v = &x;
125 : #if MATH_CHECK_INDEXES
126 : assert(i >= 0 && i < 2);
127 : #endif
128 275810895 : return _v[i];
129 : }
130 :
131 21860046 : const T & operator[](uint8_t i) const {
132 21860046 : const T *_v = &x;
133 : #if MATH_CHECK_INDEXES
134 : assert(i >= 0 && i < 2);
135 : #endif
136 21860046 : return _v[i];
137 : }
138 :
139 : // zero the vector
140 163663848 : void zero()
141 : {
142 163663848 : x = y = 0;
143 163663848 : }
144 :
145 : // gets the length of this vector squared
146 : T length_squared() const;
147 :
148 : // gets the length of this vector
149 : T length(void) const;
150 :
151 : // limit vector to a given length. returns true if vector was limited
152 : bool limit_length(T max_length);
153 :
154 : // normalizes this vector
155 : void normalize();
156 :
157 : // returns the normalized vector
158 : Vector2<T> normalized() const;
159 :
160 : // reflects this vector about n
161 : void reflect(const Vector2<T> &n);
162 :
163 : // projects this vector onto v
164 : void project(const Vector2<T> &v);
165 :
166 : // returns this vector projected onto v
167 : Vector2<T> projected(const Vector2<T> &v) const;
168 :
169 : // adjust position by a given bearing (in degrees) and distance
170 : void offset_bearing(T bearing, T distance);
171 :
172 : // rotate vector by angle in radians
173 : void rotate(T angle_rad);
174 :
175 : /*
176 : conversion to/from double
177 : */
178 102258895 : Vector2<float> tofloat() const {
179 102258895 : return Vector2<float>{float(x),float(y)};
180 : }
181 143884589 : Vector2<double> todouble() const {
182 143884589 : return Vector2<double>{x,y};
183 : }
184 :
185 : // given a position p1 and a velocity v1 produce a vector
186 : // perpendicular to v1 maximising distance from p1
187 : static Vector2<T> perpendicular(const Vector2<T> &pos_delta, const Vector2<T> &v1);
188 :
189 : /*
190 : * Returns the point closest to p on the line segment (v,w).
191 : *
192 : * Comments and implementation taken from
193 : * http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment
194 : */
195 : static Vector2<T> closest_point(const Vector2<T> &p, const Vector2<T> &v, const Vector2<T> &w);
196 :
197 : /*
198 : * Returns the point closest to p on the line segment (0,w).
199 : *
200 : * this is a simplification of closest point with a general segment, with v=(0,0)
201 : */
202 : static Vector2<T> closest_point(const Vector2<T> &p, const Vector2<T> &w);
203 :
204 : // w1 and w2 define a line segment
205 : // p is a point
206 : // returns the square of the closest distance between the line segment and the point
207 : static T closest_distance_between_line_and_point_squared(const Vector2<T> &w1,
208 : const Vector2<T> &w2,
209 : const Vector2<T> &p);
210 :
211 : // w1 and w2 define a line segment
212 : // p is a point
213 : // returns the closest distance between the line segment and the point
214 : static T closest_distance_between_line_and_point(const Vector2<T> &w1,
215 : const Vector2<T> &w2,
216 : const Vector2<T> &p);
217 :
218 : // a1->a2 and b2->v2 define two line segments
219 : // returns the square of the closest distance between the two line segments
220 : static T closest_distance_between_lines_squared(const Vector2<T> &a1,
221 : const Vector2<T> &a2,
222 : const Vector2<T> &b1,
223 : const Vector2<T> &b2);
224 :
225 : // w defines a line segment from the origin
226 : // p is a point
227 : // returns the square of the closest distance between the radial and the point
228 : static T closest_distance_between_radial_and_point_squared(const Vector2<T> &w,
229 : const Vector2<T> &p);
230 :
231 : // w defines a line segment from the origin
232 : // p is a point
233 : // returns the closest distance between the radial and the point
234 : static T closest_distance_between_radial_and_point(const Vector2<T> &w,
235 : const Vector2<T> &p);
236 :
237 : // find the intersection between two line segments
238 : // returns true if they intersect, false if they do not
239 : // the point of intersection is returned in the intersection argument
240 : static bool 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) WARN_IF_UNUSED;
241 :
242 : // find the intersection between a line segment and a circle
243 : // returns true if they intersect and intersection argument is updated with intersection closest to seg_start
244 : static bool circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, T radius, Vector2<T>& intersection) WARN_IF_UNUSED;
245 :
246 : // check if a point falls on the line segment from seg_start to seg_end
247 22 : static bool point_on_segment(const Vector2<T>& point,
248 : const Vector2<T>& seg_start,
249 : const Vector2<T>& seg_end) WARN_IF_UNUSED {
250 22 : const T expected_run = seg_end.x-seg_start.x;
251 22 : const T intersection_run = point.x-seg_start.x;
252 : // check slopes are identical:
253 22 : if (::is_zero(expected_run)) {
254 8 : if (fabsF(intersection_run) > FLT_EPSILON) {
255 2 : return false;
256 : }
257 : } else {
258 14 : const T expected_slope = (seg_end.y-seg_start.y)/expected_run;
259 14 : const T intersection_slope = (point.y-seg_start.y)/intersection_run;
260 14 : if (fabsF(expected_slope - intersection_slope) > FLT_EPSILON) {
261 2 : return false;
262 : }
263 : }
264 : // check for presence in bounding box
265 18 : if (seg_start.x < seg_end.x) {
266 6 : if (point.x < seg_start.x || point.x > seg_end.x) {
267 2 : return false;
268 : }
269 : } else {
270 12 : if (point.x < seg_end.x || point.x > seg_start.x) {
271 2 : return false;
272 : }
273 : }
274 14 : if (seg_start.y < seg_end.y) {
275 12 : if (point.y < seg_start.y || point.y > seg_end.y) {
276 2 : return false;
277 : }
278 : } else {
279 2 : if (point.y < seg_end.y || point.y > seg_start.y) {
280 2 : return false;
281 : }
282 : }
283 10 : return true;
284 : }
285 : };
286 :
287 : // check if all elements are zero
288 242465821 : template<> inline bool Vector2<float>::is_zero(void) const {
289 242465821 : return ::is_zero(x) && ::is_zero(y);
290 : }
291 :
292 673880 : template<> inline bool Vector2<double>::is_zero(void) const {
293 673880 : return ::is_zero(x) && ::is_zero(y);
294 : }
295 :
296 : typedef Vector2<int16_t> Vector2i;
297 : typedef Vector2<uint16_t> Vector2ui;
298 : typedef Vector2<int32_t> Vector2l;
299 : typedef Vector2<uint32_t> Vector2ul;
300 : typedef Vector2<float> Vector2f;
301 : typedef Vector2<double> Vector2d;
|
