APM:Libraries
vector3.cpp
Go to the documentation of this file.
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("O3")
20 
21 #include "AP_Math.h"
22 
23 #define HALF_SQRT_2 0.70710678118654757f
24 
25 // rotate a vector by a standard rotation, attempting
26 // to use the minimum number of floating point operations
27 template <typename T>
28 void Vector3<T>::rotate(enum Rotation rotation)
29 {
30  T tmp;
31  switch (rotation) {
32  case ROTATION_NONE:
33  case ROTATION_MAX:
34  return;
35  case ROTATION_YAW_45: {
36  tmp = HALF_SQRT_2*(float)(x - y);
37  y = HALF_SQRT_2*(float)(x + y);
38  x = tmp;
39  return;
40  }
41  case ROTATION_YAW_90: {
42  tmp = x; x = -y; y = tmp;
43  return;
44  }
45  case ROTATION_YAW_135: {
46  tmp = -HALF_SQRT_2*(float)(x + y);
47  y = HALF_SQRT_2*(float)(x - y);
48  x = tmp;
49  return;
50  }
51  case ROTATION_YAW_180:
52  x = -x; y = -y;
53  return;
54  case ROTATION_YAW_225: {
55  tmp = HALF_SQRT_2*(float)(y - x);
56  y = -HALF_SQRT_2*(float)(x + y);
57  x = tmp;
58  return;
59  }
60  case ROTATION_YAW_270: {
61  tmp = x; x = y; y = -tmp;
62  return;
63  }
64  case ROTATION_YAW_315: {
65  tmp = HALF_SQRT_2*(float)(x + y);
66  y = HALF_SQRT_2*(float)(y - x);
67  x = tmp;
68  return;
69  }
70  case ROTATION_ROLL_180: {
71  y = -y; z = -z;
72  return;
73  }
75  tmp = HALF_SQRT_2*(float)(x + y);
76  y = HALF_SQRT_2*(float)(x - y);
77  x = tmp; z = -z;
78  return;
79  }
81  tmp = x; x = y; y = tmp; z = -z;
82  return;
83  }
85  tmp = HALF_SQRT_2*(float)(y - x);
86  y = HALF_SQRT_2*(float)(y + x);
87  x = tmp; z = -z;
88  return;
89  }
90  case ROTATION_PITCH_180: {
91  x = -x; z = -z;
92  return;
93  }
95  tmp = -HALF_SQRT_2*(float)(x + y);
96  y = HALF_SQRT_2*(float)(y - x);
97  x = tmp; z = -z;
98  return;
99  }
101  tmp = x; x = -y; y = -tmp; z = -z;
102  return;
103  }
105  tmp = HALF_SQRT_2*(float)(x - y);
106  y = -HALF_SQRT_2*(float)(x + y);
107  x = tmp; z = -z;
108  return;
109  }
110  case ROTATION_ROLL_90: {
111  tmp = z; z = y; y = -tmp;
112  return;
113  }
115  tmp = z; z = y; y = -tmp;
116  tmp = HALF_SQRT_2*(float)(x - y);
117  y = HALF_SQRT_2*(float)(x + y);
118  x = tmp;
119  return;
120  }
122  tmp = z; z = y; y = -tmp;
123  tmp = x; x = -y; y = tmp;
124  return;
125  }
127  tmp = z; z = y; y = -tmp;
128  tmp = -HALF_SQRT_2*(float)(x + y);
129  y = HALF_SQRT_2*(float)(x - y);
130  x = tmp;
131  return;
132  }
133  case ROTATION_ROLL_270: {
134  tmp = z; z = -y; y = tmp;
135  return;
136  }
138  tmp = z; z = -y; y = tmp;
139  tmp = HALF_SQRT_2*(float)(x - y);
140  y = HALF_SQRT_2*(float)(x + y);
141  x = tmp;
142  return;
143  }
145  tmp = z; z = -y; y = tmp;
146  tmp = x; x = -y; y = tmp;
147  return;
148  }
150  tmp = z; z = -y; y = tmp;
151  tmp = -HALF_SQRT_2*(float)(x + y);
152  y = HALF_SQRT_2*(float)(x - y);
153  x = tmp;
154  return;
155  }
156  case ROTATION_PITCH_90: {
157  tmp = z; z = -x; x = tmp;
158  return;
159  }
160  case ROTATION_PITCH_270: {
161  tmp = z; z = x; x = -tmp;
162  return;
163  }
165  z = -z;
166  tmp = -x; x = -y; y = tmp;
167  return;
168  }
170  x = -x; z = -z;
171  tmp = x; x = y; y = -tmp;
172  return;
173  }
175  tmp = z; z = y; y = -tmp;
176  tmp = z; z = -x; x = tmp;
177  return;
178  }
180  y = -y; z = -z;
181  tmp = z; z = -x; x = tmp;
182  return;
183  }
185  tmp = z; z = -y; y = tmp;
186  tmp = z; z = -x; x = tmp;
187  return;
188  }
190  tmp = z; z = y; y = -tmp;
191  x = -x; z = -z;
192  return;
193  }
195  tmp = z; z = -y; y = tmp;
196  x = -x; z = -z;
197  return;
198  }
200  tmp = z; z = y; y = -tmp;
201  tmp = z; z = x; x = -tmp;
202  return;
203  }
205  y = -y; z = -z;
206  tmp = z; z = x; x = -tmp;
207  return;
208  }
210  tmp = z; z = -y; y = tmp;
211  tmp = z; z = x; x = -tmp;
212  return;
213  }
215  tmp = z; z = y; y = -tmp;
216  x = -x; z = -z;
217  tmp = x; x = -y; y = tmp;
218  return;
219  }
221  tmp = z; z = y; y = -tmp;
222  tmp = x; x = y; y = -tmp;
223  return;
224  }
226  float tmpx = x;
227  float tmpy = y;
228  float tmpz = z;
229  x = 0.143039f * tmpx + 0.368776f * tmpy + -0.918446f * tmpz;
230  y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz;
231  z = -0.932324f * tmpx + 0.361625f * tmpy + 0.000000f * tmpz;
232  return;
233  }
234  case ROTATION_PITCH_315: {
235  tmp = HALF_SQRT_2*(float)(x - z);
236  z = HALF_SQRT_2*(float)(x + z);
237  x = tmp;
238  return;
239  }
241  tmp = z; z = y; y = -tmp;
242  tmp = HALF_SQRT_2*(float)(x - z);
243  z = HALF_SQRT_2*(float)(x + z);
244  x = tmp;
245  return;
246  }
247  case ROTATION_CUSTOM: // no-op; caller should perform custom rotations via matrix multiplication
248  return;
249  }
250 }
251 
252 template <typename T>
254 {
255  Vector3<T> x_vec(1.0f,0.0f,0.0f);
256  Vector3<T> y_vec(0.0f,1.0f,0.0f);
257  Vector3<T> z_vec(0.0f,0.0f,1.0f);
258 
259  x_vec.rotate(rotation);
260  y_vec.rotate(rotation);
261  z_vec.rotate(rotation);
262 
263  Matrix3<T> M(
264  x_vec.x, y_vec.x, z_vec.x,
265  x_vec.y, y_vec.y, z_vec.y,
266  x_vec.z, y_vec.z, z_vec.z
267  );
268 
269  (*this) = M.mul_transpose(*this);
270 }
271 
272 // vector cross product
273 template <typename T>
275 {
276  Vector3<T> temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
277  return temp;
278 }
279 
280 // dot product
281 template <typename T>
283 {
284  return x*v.x + y*v.y + z*v.z;
285 }
286 
287 template <typename T>
288 float Vector3<T>::length(void) const
289 {
290  return norm(x, y, z);
291 }
292 
293 template <typename T>
295 {
296  x*=num; y*=num; z*=num;
297  return *this;
298 }
299 
300 template <typename T>
302 {
303  x /= num; y /= num; z /= num;
304  return *this;
305 }
306 
307 template <typename T>
309 {
310  x -= v.x; y -= v.y; z -= v.z;
311  return *this;
312 }
313 
314 template <typename T>
315 bool Vector3<T>::is_nan(void) const
316 {
317  return isnan(x) || isnan(y) || isnan(z);
318 }
319 
320 template <typename T>
321 bool Vector3<T>::is_inf(void) const
322 {
323  return isinf(x) || isinf(y) || isinf(z);
324 }
325 
326 template <typename T>
328 {
329  x+=v.x; y+=v.y; z+=v.z;
330  return *this;
331 }
332 
333 template <typename T>
335 {
336  return Vector3<T>(x/num, y/num, z/num);
337 }
338 
339 template <typename T>
341 {
342  return Vector3<T>(x*num, y*num, z*num);
343 }
344 
345 template <typename T>
347 {
348  return Vector3<T>(x-v.x, y-v.y, z-v.z);
349 }
350 
351 template <typename T>
353 {
354  return Vector3<T>(x+v.x, y+v.y, z+v.z);
355 }
356 
357 template <typename T>
359 {
360  return Vector3<T>(-x,-y,-z);
361 }
362 
363 template <typename T>
365 {
366  return (is_equal(x,v.x) && is_equal(y,v.y) && is_equal(z,v.z));
367 }
368 
369 template <typename T>
371 {
372  return (!is_equal(x,v.x) || !is_equal(y,v.y) || !is_equal(z,v.z));
373 }
374 
375 template <typename T>
376 float Vector3<T>::angle(const Vector3<T> &v2) const
377 {
378  float len = this->length() * v2.length();
379  if (len <= 0) {
380  return 0.0f;
381  }
382  float cosv = ((*this)*v2) / len;
383  if (fabsf(cosv) >= 1) {
384  return 0.0f;
385  }
386  return acosf(cosv);
387 }
388 
389 // multiplication of transpose by a vector
390 template <typename T>
392 {
393  return Vector3<T>(*this * m.colx(),
394  *this * m.coly(),
395  *this * m.colz());
396 }
397 
398 // multiply a column vector by a row vector, returning a 3x3 matrix
399 template <typename T>
401 {
402  const Vector3<T> v1 = *this;
403  return Matrix3<T>(v1.x * v2.x, v1.x * v2.y, v1.x * v2.z,
404  v1.y * v2.x, v1.y * v2.y, v1.y * v2.z,
405  v1.z * v2.x, v1.z * v2.y, v1.z * v2.z);
406 }
407 
408 // distance from the tip of this vector to a line segment specified by two vectors
409 template <typename T>
410 float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const
411 {
412  // triangle side lengths
413  float a = (*this-seg_start).length();
414  float b = (seg_start-seg_end).length();
415  float c = (seg_end-*this).length();
416 
417  // protect against divide by zero later
418  if (fabsf(b) < FLT_EPSILON) {
419  return 0.0f;
420  }
421 
422  // semiperimeter of triangle
423  float s = (a+b+c) * 0.5f;
424 
425  float area_squared = s*(s-a)*(s-b)*(s-c);
426  // area must be constrained above 0 because a triangle could have 3 points could be on a line and float rounding could push this under 0
427  if (area_squared < 0.0f) {
428  area_squared = 0.0f;
429  }
430  float area = safe_sqrt(area_squared);
431  return 2.0f*area/b;
432 }
433 
434 // define for float
435 template void Vector3<float>::rotate(enum Rotation);
436 template void Vector3<float>::rotate_inverse(enum Rotation);
437 template float Vector3<float>::length(void) const;
439 template float Vector3<float>::operator *(const Vector3<float> &v) const;
442 template Vector3<float> &Vector3<float>::operator *=(const float num);
443 template Vector3<float> &Vector3<float>::operator /=(const float num);
446 template Vector3<float> Vector3<float>::operator /(const float num) const;
447 template Vector3<float> Vector3<float>::operator *(const float num) const;
450 template Vector3<float> Vector3<float>::operator -(void) const;
451 template bool Vector3<float>::operator ==(const Vector3<float> &v) const;
452 template bool Vector3<float>::operator !=(const Vector3<float> &v) const;
453 template bool Vector3<float>::is_nan(void) const;
454 template bool Vector3<float>::is_inf(void) const;
455 template float Vector3<float>::angle(const Vector3<float> &v) const;
456 template float Vector3<float>::distance_to_segment(const Vector3<float> &seg_start, const Vector3<float> &seg_end) const;
457 
458 // define needed ops for Vector3l
460 
461 template void Vector3<double>::rotate(enum Rotation);
462 template void Vector3<double>::rotate_inverse(enum Rotation);
463 template float Vector3<double>::length(void) const;
465 template double Vector3<double>::operator *(const Vector3<double> &v) const;
468 template Vector3<double> &Vector3<double>::operator *=(const double num);
469 template Vector3<double> &Vector3<double>::operator /=(const double num);
472 template Vector3<double> Vector3<double>::operator /(const double num) const;
473 template Vector3<double> Vector3<double>::operator *(const double num) const;
476 template Vector3<double> Vector3<double>::operator -(void) const;
477 template bool Vector3<double>::operator ==(const Vector3<double> &v) const;
478 template bool Vector3<double>::operator !=(const Vector3<double> &v) const;
479 template bool Vector3<double>::is_nan(void) const;
480 template bool Vector3<double>::is_inf(void) const;
float norm(const T first, const U second, const Params... parameters)
Definition: AP_Math.h:190
float safe_sqrt(const T v)
Definition: AP_Math.cpp:64
bool is_inf(void) const
Definition: vector3.cpp:321
float angle(const Vector3< T > &v2) const
Definition: vector3.cpp:376
Vector3< T > operator-(void) const
Definition: vector3.cpp:358
void rotate_inverse(enum Rotation rotation)
Definition: vector3.cpp:253
bool is_nan(void) const
Definition: vector3.cpp:315
float distance_to_segment(const Vector3< T > &seg_start, const Vector3< T > &seg_end) const
Definition: vector3.cpp:410
Rotation
Definition: rotations.h:27
Vector3< T > colz(void) const
Definition: matrix3.h:169
bool operator!=(const Vector3< T > &v) const
Definition: vector3.cpp:370
#define x(i)
#define HALF_SQRT_2
Definition: vector3.cpp:23
Vector3< T > operator/(const T num) const
Definition: vector3.cpp:334
Vector3< T > coly(void) const
Definition: matrix3.h:163
#define f(i)
T y
Definition: vector3.h:67
Vector3< T > & operator+=(const Vector3< T > &v)
Definition: vector3.cpp:327
T z
Definition: vector3.h:67
Vector3< T > & operator/=(const T num)
Definition: vector3.cpp:301
float v
Definition: Printf.cpp:15
Vector3< T > operator%(const Vector3< T > &v) const
Definition: vector3.cpp:274
Vector3< T > operator+(const Vector3< T > &v) const
Definition: vector3.cpp:352
bool operator==(const Vector3< T > &v) const
Definition: vector3.cpp:364
float length(void) const
Definition: vector3.cpp:288
void rotate(enum Rotation rotation)
Definition: vector3.cpp:28
Vector3< T > mul_transpose(const Vector3< T > &v) const
Definition: matrix3.cpp:165
std::enable_if< std::is_integral< typename std::common_type< Arithmetic1, Arithmetic2 >::type >::value,bool >::type is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
Definition: AP_Math.cpp:12
Matrix3< T > mul_rowcol(const Vector3< T > &v) const
Definition: vector3.cpp:400
Vector3< T > & operator*=(const T num)
Definition: vector3.cpp:294
void uint32_t num
Definition: systick.h:80
Vector3< T > colx(void) const
Definition: matrix3.h:157
Vector3< T > & operator-=(const Vector3< T > &v)
Definition: vector3.cpp:308
T x
Definition: vector3.h:67
Vector3< T > operator*(const T num) const
Definition: vector3.cpp:340