1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
| #pragma once
|
| #include "math.hpp"
|
| // grody hack - this should go once C++11 is supported
| // on all platforms.
| #if defined (__PX4_NUTTX) || defined (__PX4_QURT)
| #include <math.h>
| #else
| #include <cmath>
| #endif
|
| namespace matrix
| {
|
| template<typename Type>
| Type wrap_pi(Type x)
| {
| #if defined (__PX4_NUTTX) || defined (__PX4_QURT)
| if (!isfinite(x)) {
| #else
| if (!std::isfinite(x)) {
| #endif
| return x;
| }
|
| while (x >= (Type)M_PI) {
| x -= (Type)(2.0 * M_PI);
|
| }
|
| while (x < (Type)(-M_PI)) {
| x += (Type)(2.0 * M_PI);
|
| }
|
| return x;
| }
|
|
| };
|
|
