forked from HongshiTan/RTIMULib2
-
Notifications
You must be signed in to change notification settings - Fork 0
/
RTMath.h
195 lines (150 loc) · 6.74 KB
/
RTMath.h
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
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib
//
// Copyright (c) 2014-2015, richards-tech, LLC
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#ifndef _RTMATH_H_
#define _RTMATH_H_
#include "RTIMUHal.h"
// The fundamental float type
#ifdef RTMATH_USE_DOUBLE
typedef double RTFLOAT;
#else
typedef float RTFLOAT;
#endif
// Useful constants
#define RTMATH_PI 3.1415926535
#define RTMATH_DEGREE_TO_RAD (RTMATH_PI / 180.0)
#define RTMATH_RAD_TO_DEGREE (180.0 / RTMATH_PI)
class RTVector3;
class RTMatrix4x4;
class RTQuaternion;
class RTMath
{
public:
// convenient display routines
static const char *displayRadians(const char *label, RTVector3& vec);
static const char *displayDegrees(const char *label, RTVector3& vec);
static const char *display(const char *label, RTQuaternion& quat);
static const char *display(const char *label, RTMatrix4x4& mat);
// currentUSecsSinceEpoch() is the source of all timestamps and
// is the number of uS since the standard epoch
static uint64_t currentUSecsSinceEpoch();
// poseFromAccelMag generates pose Euler angles from measured settings
static RTVector3 poseFromAccelMag(const RTVector3& accel, const RTVector3& mag);
// Takes signed 16 bit data from a char array and converts it to a vector of scaled RTFLOATs
static void convertToVector(unsigned char *rawData, RTVector3& vec, RTFLOAT scale, bool bigEndian);
// Takes a pressure in hPa and returns height above sea level in meters
static RTFLOAT convertPressureToHeight(RTFLOAT pressure, RTFLOAT staticPressure = 1013.25);
private:
static char m_string[1000]; // for the display routines
};
class RTVector3
{
public:
RTVector3();
RTVector3(RTFLOAT x, RTFLOAT y, RTFLOAT z);
const RTVector3& operator +=(RTVector3& vec);
const RTVector3& operator -=(RTVector3& vec);
RTVector3& operator =(const RTVector3& vec);
RTFLOAT length();
void normalize();
void zero();
const char *display();
const char *displayDegrees();
static float dotProduct(const RTVector3& a, const RTVector3& b);
static void crossProduct(const RTVector3& a, const RTVector3& b, RTVector3& d);
void accelToEuler(RTVector3& rollPitchYaw) const;
void accelToQuaternion(RTQuaternion& qPose) const;
inline RTFLOAT x() const { return m_data[0]; }
inline RTFLOAT y() const { return m_data[1]; }
inline RTFLOAT z() const { return m_data[2]; }
inline RTFLOAT data(const int i) const { return m_data[i]; }
inline void setX(const RTFLOAT val) { m_data[0] = val; }
inline void setY(const RTFLOAT val) { m_data[1] = val; }
inline void setZ(const RTFLOAT val) { m_data[2] = val; }
inline void setData(const int i, RTFLOAT val) { m_data[i] = val; }
inline void fromArray(RTFLOAT *val) { memcpy(m_data, val, 3 * sizeof(RTFLOAT)); }
inline void toArray(RTFLOAT *val) const { memcpy(val, m_data, 3 * sizeof(RTFLOAT)); }
private:
RTFLOAT m_data[3];
};
class RTQuaternion
{
public:
RTQuaternion();
RTQuaternion(RTFLOAT scalar, RTFLOAT x, RTFLOAT y, RTFLOAT z);
RTQuaternion& operator +=(const RTQuaternion& quat);
RTQuaternion& operator -=(const RTQuaternion& quat);
RTQuaternion& operator *=(const RTQuaternion& qb);
RTQuaternion& operator *=(const RTFLOAT val);
RTQuaternion& operator -=(const RTFLOAT val);
RTQuaternion& operator =(const RTQuaternion& quat);
const RTQuaternion operator *(const RTQuaternion& qb) const;
const RTQuaternion operator *(const RTFLOAT val) const;
const RTQuaternion operator -(const RTQuaternion& qb) const;
const RTQuaternion operator -(const RTFLOAT val) const;
void normalize();
void toEuler(RTVector3& vec);
void fromEuler(RTVector3& vec);
RTQuaternion conjugate() const;
void toAngleVector(RTFLOAT& angle, RTVector3& vec);
void fromAngleVector(const RTFLOAT& angle, const RTVector3& vec);
void zero();
const char *display();
inline RTFLOAT scalar() const { return m_data[0]; }
inline RTFLOAT x() const { return m_data[1]; }
inline RTFLOAT y() const { return m_data[2]; }
inline RTFLOAT z() const { return m_data[3]; }
inline RTFLOAT data(const int i) const { return m_data[i]; }
inline void setScalar(const RTFLOAT val) { m_data[0] = val; }
inline void setX(const RTFLOAT val) { m_data[1] = val; }
inline void setY(const RTFLOAT val) { m_data[2] = val; }
inline void setZ(const RTFLOAT val) { m_data[3] = val; }
inline void setData(const int i, RTFLOAT val) { m_data[i] = val; }
inline void fromArray(RTFLOAT *val) { memcpy(m_data, val, 4 * sizeof(RTFLOAT)); }
inline void toArray(RTFLOAT *val) const { memcpy(val, m_data, 4 * sizeof(RTFLOAT)); }
private:
RTFLOAT m_data[4];
};
class RTMatrix4x4
{
public:
RTMatrix4x4();
RTMatrix4x4& operator +=(const RTMatrix4x4& mat);
RTMatrix4x4& operator -=(const RTMatrix4x4& mat);
RTMatrix4x4& operator *=(const RTFLOAT val);
RTMatrix4x4& operator =(const RTMatrix4x4& vec);
const RTQuaternion operator *(const RTQuaternion& q) const;
const RTMatrix4x4 operator *(const RTFLOAT val) const;
const RTMatrix4x4 operator *(const RTMatrix4x4& mat) const;
const RTMatrix4x4 operator +(const RTMatrix4x4& mat) const;
inline RTFLOAT val(int row, int col) const { return m_data[row][col]; }
inline void setVal(int row, int col, RTFLOAT val) { m_data[row][col] = val; }
void fill(RTFLOAT val);
void setToIdentity();
RTMatrix4x4 inverted();
RTMatrix4x4 transposed();
private:
RTFLOAT m_data[4][4]; // row, column
RTFLOAT matDet();
RTFLOAT matMinor(const int row, const int col);
};
#endif /* _RTMATH_H_ */