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 : * 3D Vector Classes
21 : * By Bill Perone ([email protected])
22 : * Original: 9-16-2002
23 : * Revised: 19-11-2003
24 : * 11-12-2003
25 : * 18-12-2003
26 : * 06-06-2004
27 : *
28 : * Copyright 2003, This code is provided "as is" and you can use it freely as long as
29 : * credit is given to Bill Perone in the application it is used in
30 : *
31 : * Notes:
32 : * if a*b = 0 then a & b are orthogonal
33 : * a%b = -b%a
34 : * a*(b%c) = (a%b)*c
35 : * a%b = a(cast to matrix)*b
36 : * (a%b).length() = area of parallelogram formed by a & b
37 : * (a%b).length() = a.length()*b.length() * sin(angle between a & b)
38 : * (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0
39 : * a * (b%c) = volume of parallelpiped formed by a, b, c
40 : * vector triple product: a%(b%c) = b*(a*c) - c*(a*b)
41 : * scalar triple product: a*(b%c) = c*(a%b) = b*(c%a)
42 : * vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c)
43 : * if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0
44 : * vectors a1...an are linearly dependent if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0
45 : * or if the matrix (A) * b = 0
46 : *
47 : ****************************************/
48 : #pragma once
49 :
50 : #ifndef MATH_CHECK_INDEXES
51 : #define MATH_CHECK_INDEXES 0
52 : #endif
53 :
54 : #include <cmath>
55 : #include <float.h>
56 : #include <string.h>
57 : #if MATH_CHECK_INDEXES
58 : #include <assert.h>
59 : #endif
60 :
61 : #include "rotations.h"
62 :
63 : #include "ftype.h"
64 :
65 : template <typename T>
66 : class Matrix3;
67 :
68 : template <typename T>
69 : class Vector2;
70 :
71 : template <typename T>
72 : class Vector3
73 : {
74 :
75 : public:
76 : T x, y, z;
77 :
78 : // trivial ctor
79 2643380513 : constexpr Vector3()
80 2643380513 : : x(0)
81 2643380513 : , y(0)
82 2643380513 : , z(0) {}
83 :
84 : // setting ctor
85 58708779299 : constexpr Vector3(const T x0, const T y0, const T z0)
86 58708779299 : : x(x0)
87 58708779299 : , y(y0)
88 58708779299 : , z(z0) {}
89 :
90 : //Create a Vector3 from a Vector2 with z
91 535236 : constexpr Vector3(const Vector2<T> &v0, const T z0)
92 535236 : : x(v0.x)
93 535236 : , y(v0.y)
94 535236 : , z(z0) {}
95 :
96 : // test for equality
97 : bool operator ==(const Vector3<T> &v) const;
98 :
99 : // test for inequality
100 : bool operator !=(const Vector3<T> &v) const;
101 :
102 : // negation
103 : Vector3<T> operator -(void) const;
104 :
105 : // addition
106 : Vector3<T> operator +(const Vector3<T> &v) const;
107 :
108 : // subtraction
109 : Vector3<T> operator -(const Vector3<T> &v) const;
110 :
111 : // uniform scaling
112 : Vector3<T> operator *(const T num) const;
113 :
114 : // uniform scaling
115 : Vector3<T> operator /(const T num) const;
116 :
117 : // addition
118 : Vector3<T> &operator +=(const Vector3<T> &v);
119 :
120 : // subtraction
121 : Vector3<T> &operator -=(const Vector3<T> &v);
122 :
123 : // uniform scaling
124 : Vector3<T> &operator *=(const T num);
125 :
126 : // uniform scaling
127 : Vector3<T> &operator /=(const T num);
128 :
129 : // non-uniform scaling
130 74836022 : Vector3<T> &operator *=(const Vector3<T> &v) {
131 74836022 : x *= v.x; y *= v.y; z *= v.z;
132 74836022 : return *this;
133 : }
134 :
135 : // allow a vector3 to be used as an array, 0 indexed
136 3326216565 : T & operator[](uint8_t i) {
137 3326216565 : T *_v = &x;
138 : #if MATH_CHECK_INDEXES
139 : assert(i >= 0 && i < 3);
140 : #endif
141 3326216565 : return _v[i];
142 : }
143 :
144 4585814316 : const T & operator[](uint8_t i) const {
145 4585814316 : const T *_v = &x;
146 : #if MATH_CHECK_INDEXES
147 : assert(i >= 0 && i < 3);
148 : #endif
149 4585814316 : return _v[i];
150 : }
151 :
152 : // dot product
153 : T operator *(const Vector3<T> &v) const;
154 :
155 : // dot product for Lua
156 7678302 : T dot(const Vector3<T> &v) const {
157 7678302 : return *this * v;
158 : }
159 :
160 : // multiply a row vector by a matrix, to give a row vector
161 : Vector3<T> row_times_mat(const Matrix3<T> &m) const;
162 :
163 : // multiply a column vector by a row vector, returning a 3x3 matrix
164 : Matrix3<T> mul_rowcol(const Vector3<T> &v) const;
165 :
166 : // cross product
167 : Vector3<T> operator %(const Vector3<T> &v) const;
168 :
169 : // cross product for Lua
170 82491 : Vector3<T> cross(const Vector3<T> &v) const {
171 82491 : return *this % v;
172 : }
173 :
174 : // scale a vector3
175 79250 : Vector3<T> scale(const T v) const {
176 79250 : return *this * v;
177 : }
178 :
179 : // computes the angle between this vector and another vector
180 : T angle(const Vector3<T> &v2) const;
181 :
182 : // check if any elements are NAN
183 : bool is_nan(void) const WARN_IF_UNUSED;
184 :
185 : // check if any elements are infinity
186 : bool is_inf(void) const WARN_IF_UNUSED;
187 :
188 : // check if all elements are zero
189 174917 : bool is_zero(void) const WARN_IF_UNUSED {
190 174917 : return x == 0 && y == 0 && z == 0;
191 : }
192 :
193 :
194 : // rotate by a standard rotation
195 : void rotate(enum Rotation rotation);
196 : void rotate_inverse(enum Rotation rotation);
197 :
198 : // rotate vector by angle in radians in xy plane leaving z untouched
199 : void rotate_xy(T rotation_rad);
200 :
201 : // return xy components of a vector3 as a vector2.
202 : // this returns a reference to the original vector3 xy data
203 373016246 : const Vector2<T> &xy() const {
204 373016246 : return *(const Vector2<T> *)this;
205 : }
206 1608325225 : Vector2<T> &xy() {
207 1608325225 : return *(Vector2<T> *)this;
208 : }
209 :
210 : // gets the length of this vector squared
211 140132636 : T length_squared() const
212 : {
213 140132636 : return (T)(*this * *this);
214 : }
215 :
216 : // gets the length of this vector
217 : T length(void) const;
218 :
219 : // limit xy component vector to a given length. returns true if vector was limited
220 : bool limit_length_xy(T max_length);
221 :
222 : // normalizes this vector
223 20218772 : void normalize()
224 : {
225 20218772 : *this /= length();
226 20218772 : }
227 :
228 : // zero the vector
229 619444581 : void zero()
230 : {
231 619444581 : x = y = z = 0;
232 619444581 : }
233 :
234 : // returns the normalized version of this vector
235 14828298 : Vector3<T> normalized() const
236 : {
237 14828298 : return *this/length();
238 : }
239 :
240 : // reflects this vector about n
241 0 : void reflect(const Vector3<T> &n)
242 : {
243 0 : Vector3<T> orig(*this);
244 0 : project(n);
245 0 : *this = *this*2 - orig;
246 0 : }
247 :
248 : // projects this vector onto v
249 0 : void project(const Vector3<T> &v)
250 : {
251 0 : *this= v * (*this * v)/(v*v);
252 0 : }
253 :
254 : // returns this vector projected onto v
255 453956990 : Vector3<T> projected(const Vector3<T> &v) const
256 : {
257 453956990 : return v * (*this * v)/(v*v);
258 : }
259 :
260 : // distance from the tip of this vector to another vector squared (so as to avoid the sqrt calculation)
261 186482 : T distance_squared(const Vector3<T> &v) const {
262 186482 : const T dist_x = x-v.x;
263 186482 : const T dist_y = y-v.y;
264 186482 : const T dist_z = z-v.z;
265 186482 : return (dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
266 : }
267 :
268 : // distance from the tip of this vector to a line segment specified by two vectors
269 : T distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const;
270 :
271 : // extrapolate position given bearing and pitch (in degrees) and distance
272 : void offset_bearing(T bearing, T pitch, T distance);
273 :
274 : /*
275 : conversion to/from double
276 : */
277 430872209 : Vector3<float> tofloat() const {
278 430872209 : return Vector3<float>{float(x),float(y),float(z)};
279 : }
280 993084351 : Vector3<double> todouble() const {
281 993084351 : return Vector3<double>{x,y,z};
282 : }
283 :
284 : // convert from right-front-up to front-right-down
285 : // or ENU to NED
286 212004 : Vector3<T> rfu_to_frd() const {
287 212004 : return Vector3<T>{y,x,-z};
288 : }
289 :
290 : // Returns the component of p1 perpendicular to v1 (i.e., p1 projected
291 : // onto the plane orthogonal to v1). Returns zero if p1 is zero.
292 22 : static Vector3<T> perpendicular(const Vector3<T> &p1, const Vector3<T> &v1)
293 : {
294 22 : const T d = p1 * v1;
295 22 : if (::is_zero(d)) {
296 8 : return p1;
297 : }
298 14 : const Vector3<T> parallel = (v1 * d) / v1.length_squared();
299 14 : Vector3<T> perpendicular = p1 - parallel;
300 :
301 14 : return perpendicular;
302 : }
303 :
304 : // Shortest distance between point(p) to a point contained in the line segment defined by w1,w2
305 : static T closest_distance_between_line_and_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p);
306 :
307 : // Point in the line segment defined by w1,w2 which is closest to point(p)
308 : static Vector3<T> point_on_line_closest_to_other_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p);
309 :
310 : // This implementation is borrowed from: http://geomalgorithms.com/a07-_distance.html
311 : // INPUT: 4 points corresponding to start and end of two line segments
312 :
313 : // OUTPUT: closest point on segment 2, from segment 1, gets passed on reference as "closest_point"
314 : static void segment_to_segment_closest_point(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& closest_point);
315 :
316 : // Returns true if the passed 3D segment passes through a plane defined by plane normal, and a point on the plane
317 : static bool segment_plane_intersect(const Vector3<T>& seg_start, const Vector3<T>& seg_end, const Vector3<T>& plane_normal, const Vector3<T>& plane_point);
318 : };
319 :
320 : // check if all elements are zero
321 8988613961 : template<> inline bool Vector3<float>::is_zero(void) const {
322 8988613961 : return ::is_zero(x) && ::is_zero(y) && ::is_zero(z);
323 : }
324 :
325 84281839 : template<> inline bool Vector3<double>::is_zero(void) const {
326 84281839 : return ::is_zero(x) && ::is_zero(y) && ::is_zero(z);
327 : }
328 :
329 : // The creation of temporary vector objects as return types creates a significant overhead in certain hot
330 : // code paths. This allows callers to select the inline versions where profiling shows a significant benefit
331 : #if defined(AP_INLINE_VECTOR_OPS) && !defined(HAL_DEBUG_BUILD)
332 :
333 : // vector cross product
334 : template <typename T>
335 : inline Vector3<T> Vector3<T>::operator %(const Vector3<T> &v) const
336 : {
337 : return Vector3<T>(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
338 : }
339 :
340 : // dot product
341 : template <typename T>
342 : inline T Vector3<T>::operator *(const Vector3<T> &v) const
343 : {
344 : return x*v.x + y*v.y + z*v.z;
345 : }
346 :
347 : template <typename T>
348 : inline Vector3<T> &Vector3<T>::operator *=(const T num)
349 : {
350 : x*=num; y*=num; z*=num;
351 : return *this;
352 : }
353 :
354 : template <typename T>
355 : inline Vector3<T> &Vector3<T>::operator /=(const T num)
356 : {
357 : x /= num; y /= num; z /= num;
358 : return *this;
359 : }
360 :
361 : template <typename T>
362 : inline Vector3<T> &Vector3<T>::operator -=(const Vector3<T> &v)
363 : {
364 : x -= v.x; y -= v.y; z -= v.z;
365 : return *this;
366 : }
367 :
368 : template <typename T>
369 : inline Vector3<T> &Vector3<T>::operator +=(const Vector3<T> &v)
370 : {
371 : x+=v.x; y+=v.y; z+=v.z;
372 : return *this;
373 : }
374 :
375 : template <typename T>
376 : inline Vector3<T> Vector3<T>::operator /(const T num) const
377 : {
378 : return Vector3<T>(x/num, y/num, z/num);
379 : }
380 :
381 : template <typename T>
382 : inline Vector3<T> Vector3<T>::operator *(const T num) const
383 : {
384 : return Vector3<T>(x*num, y*num, z*num);
385 : }
386 :
387 : template <typename T>
388 : inline Vector3<T> Vector3<T>::operator -(const Vector3<T> &v) const
389 : {
390 : return Vector3<T>(x-v.x, y-v.y, z-v.z);
391 : }
392 :
393 : template <typename T>
394 : inline Vector3<T> Vector3<T>::operator +(const Vector3<T> &v) const
395 : {
396 : return Vector3<T>(x+v.x, y+v.y, z+v.z);
397 : }
398 :
399 : template <typename T>
400 : inline Vector3<T> Vector3<T>::operator -(void) const
401 : {
402 : return Vector3<T>(-x,-y,-z);
403 : }
404 : #endif
405 :
406 : typedef Vector3<int16_t> Vector3i;
407 : typedef Vector3<uint16_t> Vector3ui;
408 : typedef Vector3<int32_t> Vector3l;
409 : typedef Vector3<uint32_t> Vector3ul;
410 : typedef Vector3<float> Vector3f;
411 : typedef Vector3<double> Vector3d;
|
