Statistics
| Branch: | Revision:

adafruit_bno055 / utility / quaternion.h @ 67f3cff5

History | View | Annotate | Download (6.572 KB)

1 4bc1c0c1 Kevin Townsend
/*
2
    Inertial Measurement Unit Maths Library
3
    Copyright (C) 2013-2014  Samuel Cowen
4
        www.camelsoftware.com
5

6
    This program is free software: you can redistribute it and/or modify
7
    it under the terms of the GNU General Public License as published by
8
    the Free Software Foundation, either version 3 of the License, or
9
    (at your option) any later version.
10

11
    This program is distributed in the hope that it will be useful,
12
    but WITHOUT ANY WARRANTY; without even the implied warranty of
13
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14
    GNU General Public License for more details.
15

16
    You should have received a copy of the GNU General Public License
17
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
18
*/
19
20
21
#ifndef IMUMATH_QUATERNION_HPP
22
#define IMUMATH_QUATERNION_HPP
23
24
#include <stdlib.h>
25
#include <string.h>
26
#include <stdint.h>
27
#include <math.h>
28
29
#include "vector.h"
30
31
32
namespace imu
33
{
34
35
36
37
class Quaternion
38
{
39
public:
40
    Quaternion()
41
    {
42
        _w = 1.0;
43
        _x = _y = _z = 0.0;
44
    }
45
46
    Quaternion(double iw, double ix, double iy, double iz)
47
    {
48
        _w = iw;
49
        _x = ix;
50
        _y = iy;
51
        _z = iz;
52
    }
53
54
    Quaternion(double w, Vector<3> vec)
55
    {
56
        _w = w;
57
        _x = vec.x();
58
        _y = vec.y();
59
        _z = vec.z();
60
    }
61
62
    double& w()
63
    {
64
        return _w;
65
    }
66
    double& x()
67
    {
68
        return _x;
69
    }
70
    double& y()
71
    {
72
        return _y;
73
    }
74
    double& z()
75
    {
76
        return _z;
77
    }
78
79
    double magnitude()
80
    {
81
        double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);
82
        return sqrt(res);
83
    }
84
85
    void normalize()
86
    {
87
                double mag = magnitude();
88
        *this = this->scale(1/mag);
89
    }
90
91
92
    Quaternion conjugate()
93
    {
94
        Quaternion q;
95
        q.w() = _w;
96
        q.x() = -_x;
97
        q.y() = -_y;
98
        q.z() = -_z;
99
        return q;
100
    }
101
102
    void fromAxisAngle(Vector<3> axis, double theta)
103
    {
104
        _w = cos(theta/2);
105
        //only need to calculate sine of half theta once
106
        double sht = sin(theta/2);
107
        _x = axis.x() * sht;
108
        _y = axis.y() * sht;
109
        _z = axis.z() * sht;
110
    }
111
112
    void fromMatrix(Matrix<3> m)
113
    {
114
        float tr = m(0, 0) + m(1, 1) + m(2, 2);
115
116
        float S = 0.0;
117
        if (tr > 0)
118
        {
119
            S = sqrt(tr+1.0) * 2;
120
            _w = 0.25 * S;
121
            _x = (m(2, 1) - m(1, 2)) / S;
122
            _y = (m(0, 2) - m(2, 0)) / S;
123
            _z = (m(1, 0) - m(0, 1)) / S;
124
        }
125
        else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2)))
126
        {
127
            S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
128
            _w = (m(2, 1) - m(1, 2)) / S;
129
            _x = 0.25 * S;
130
            _y = (m(0, 1) + m(1, 0)) / S;
131
            _z = (m(0, 2) + m(2, 0)) / S;
132
        }
133
        else if (m(1, 1) < m(2, 2))
134
        {
135
            S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
136
            _w = (m(0, 2) - m(2, 0)) / S;
137
            _x = (m(0, 1) + m(1, 0)) / S;
138
            _y = 0.25 * S;
139
            _z = (m(1, 2) + m(2, 1)) / S;
140
        }
141
        else
142
        {
143
            S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
144
            _w = (m(1, 0) - m(0, 1)) / S;
145
            _x = (m(0, 2) + m(2, 0)) / S;
146
            _y = (m(1, 2) + m(2, 1)) / S;
147
            _z = 0.25 * S;
148
        }
149
    }
150
151
    void toAxisAngle(Vector<3>& axis, float& angle)
152
    {
153
        float sqw = sqrt(1-_w*_w);
154
        if(sqw == 0) //it's a singularity and divide by zero, avoid
155
            return;
156
157
        angle = 2 * acos(_w);
158
        axis.x() = _x / sqw;
159
        axis.y() = _y / sqw;
160
        axis.z() = _z / sqw;
161
    }
162
163
    Matrix<3> toMatrix()
164
    {
165
        Matrix<3> ret;
166
        ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z));
167
        ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z);
168
        ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y);
169
170
        ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z);
171
        ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z));
172
        ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x));
173
174
        ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y);
175
        ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x);
176
        ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y));
177
        return ret;
178
    }
179
180
181
    Vector<3> toEuler()
182
    {
183
        Vector<3> ret;
184
        double sqw = _w*_w;
185
        double sqx = _x*_x;
186
        double sqy = _y*_y;
187
        double sqz = _z*_z;
188
189
        ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));
190
        ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));
191
        ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));
192
193
        return ret;
194
    }
195
196
    Vector<3> toAngularVelocity(float dt)
197
    {
198
        Vector<3> ret;
199
        Quaternion one(1.0, 0.0, 0.0, 0.0);
200
        Quaternion delta = one - *this;
201
        Quaternion r = (delta/dt);
202
        r = r * 2;
203
        r = r * one;
204
205
        ret.x() = r.x();
206
        ret.y() = r.y();
207
        ret.z() = r.z();
208
        return ret;
209
    }
210
211
    Vector<3> rotateVector(Vector<2> v)
212
    {
213
        Vector<3> ret(v.x(), v.y(), 0.0);
214
        return rotateVector(ret);
215
    }
216
217
    Vector<3> rotateVector(Vector<3> v)
218
    {
219
        Vector<3> qv(this->x(), this->y(), this->z());
220
        Vector<3> t;
221
        t = qv.cross(v) * 2.0;
222
        return v + (t * _w) + qv.cross(t);
223
    }
224
225
226
    Quaternion operator * (Quaternion q)
227
    {
228
        Quaternion ret;
229
        ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z));
230
        ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y));
231
        ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x));
232
        ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w));
233
        return ret;
234
    }
235
236
    Quaternion operator + (Quaternion q)
237
    {
238
        Quaternion ret;
239
        ret._w = _w + q._w;
240
        ret._x = _x + q._x;
241
        ret._y = _y + q._y;
242
        ret._z = _z + q._z;
243
        return ret;
244
    }
245
246
    Quaternion operator - (Quaternion q)
247
    {
248
        Quaternion ret;
249
        ret._w = _w - q._w;
250
        ret._x = _x - q._x;
251
        ret._y = _y - q._y;
252
        ret._z = _z - q._z;
253
        return ret;
254
    }
255
256
    Quaternion operator / (float scalar)
257
    {
258
        Quaternion ret;
259
        ret._w = this->_w/scalar;
260
        ret._x = this->_x/scalar;
261
        ret._y = this->_y/scalar;
262
        ret._z = this->_z/scalar;
263
        return ret;
264
    }
265
266
    Quaternion operator * (float scalar)
267
    {
268
        Quaternion ret;
269
        ret._w = this->_w*scalar;
270
        ret._x = this->_x*scalar;
271
        ret._y = this->_y*scalar;
272
        ret._z = this->_z*scalar;
273
        return ret;
274
    }
275
276
        Quaternion scale(double scalar)
277
        {
278
        Quaternion ret;
279
        ret._w = this->_w*scalar;
280
        ret._x = this->_x*scalar;
281
        ret._y = this->_y*scalar;
282
        ret._z = this->_z*scalar;
283
        return ret;
284
    }
285
286
private:
287
    double _w, _x, _y, _z;
288
};
289
290
291
};
292
293
#endif