Statistics
| Branch: | Revision:

adafruit_bno055 / utility / quaternion.h @ f12bf4b5

History | View | Annotate | Download (7.279 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 0695bf91 Paul Du Bois (laptop)
    double w() const
80
    {
81
        return _w;
82
    }
83
    double x() const
84
    {
85
        return _x;
86
    }
87
    double y() const
88
    {
89
        return _y;
90
    }
91
    double z() const
92
    {
93
        return _z;
94
    }
95
96
    double magnitude() const
97 4bc1c0c1 Kevin Townsend
    {
98
        double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);
99
        return sqrt(res);
100
    }
101
102
    void normalize()
103
    {
104 0695bf91 Paul Du Bois (laptop)
        double mag = magnitude();
105 4bc1c0c1 Kevin Townsend
        *this = this->scale(1/mag);
106
    }
107
108
109 0695bf91 Paul Du Bois (laptop)
    Quaternion conjugate() const
110 4bc1c0c1 Kevin Townsend
    {
111
        Quaternion q;
112
        q.w() = _w;
113
        q.x() = -_x;
114
        q.y() = -_y;
115
        q.z() = -_z;
116
        return q;
117
    }
118
119
    void fromAxisAngle(Vector<3> axis, double theta)
120
    {
121
        _w = cos(theta/2);
122
        //only need to calculate sine of half theta once
123
        double sht = sin(theta/2);
124
        _x = axis.x() * sht;
125
        _y = axis.y() * sht;
126
        _z = axis.z() * sht;
127
    }
128
129
    void fromMatrix(Matrix<3> m)
130
    {
131
        float tr = m(0, 0) + m(1, 1) + m(2, 2);
132
133
        float S = 0.0;
134
        if (tr > 0)
135
        {
136
            S = sqrt(tr+1.0) * 2;
137
            _w = 0.25 * S;
138
            _x = (m(2, 1) - m(1, 2)) / S;
139
            _y = (m(0, 2) - m(2, 0)) / S;
140
            _z = (m(1, 0) - m(0, 1)) / S;
141
        }
142
        else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2)))
143
        {
144
            S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
145
            _w = (m(2, 1) - m(1, 2)) / S;
146
            _x = 0.25 * S;
147
            _y = (m(0, 1) + m(1, 0)) / S;
148
            _z = (m(0, 2) + m(2, 0)) / S;
149
        }
150
        else if (m(1, 1) < m(2, 2))
151
        {
152
            S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
153
            _w = (m(0, 2) - m(2, 0)) / S;
154
            _x = (m(0, 1) + m(1, 0)) / S;
155
            _y = 0.25 * S;
156
            _z = (m(1, 2) + m(2, 1)) / S;
157
        }
158
        else
159
        {
160
            S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
161
            _w = (m(1, 0) - m(0, 1)) / S;
162
            _x = (m(0, 2) + m(2, 0)) / S;
163
            _y = (m(1, 2) + m(2, 1)) / S;
164
            _z = 0.25 * S;
165
        }
166
    }
167
168 0695bf91 Paul Du Bois (laptop)
    void toAxisAngle(Vector<3>& axis, float& angle) const
169 4bc1c0c1 Kevin Townsend
    {
170
        float sqw = sqrt(1-_w*_w);
171
        if(sqw == 0) //it's a singularity and divide by zero, avoid
172
            return;
173
174
        angle = 2 * acos(_w);
175
        axis.x() = _x / sqw;
176
        axis.y() = _y / sqw;
177
        axis.z() = _z / sqw;
178
    }
179
180 0695bf91 Paul Du Bois (laptop)
    Matrix<3> toMatrix() const
181 4bc1c0c1 Kevin Townsend
    {
182
        Matrix<3> ret;
183
        ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z));
184
        ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z);
185
        ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y);
186
187
        ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z);
188
        ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z));
189
        ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x));
190
191
        ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y);
192
        ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x);
193
        ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y));
194
        return ret;
195
    }
196
197
198 0695bf91 Paul Du Bois (laptop)
    // Returns euler angles that represent the quaternion.  Angles are
199
    // returned in rotation order and right-handed about the specified
200
    // axes:
201
    //
202
    //   v[0] is applied 1st about z (ie, roll)
203
    //   v[1] is applied 2nd about y (ie, pitch)
204
    //   v[2] is applied 3rd about x (ie, yaw)
205
    //
206
    // Note that this means result.x() is not a rotation about x;
207
    // similarly for result.z().
208
    //
209
    Vector<3> toEuler() const
210 4bc1c0c1 Kevin Townsend
    {
211
        Vector<3> ret;
212
        double sqw = _w*_w;
213
        double sqx = _x*_x;
214
        double sqy = _y*_y;
215
        double sqz = _z*_z;
216
217
        ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));
218
        ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));
219
        ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));
220
221
        return ret;
222
    }
223
224 0695bf91 Paul Du Bois (laptop)
    Vector<3> toAngularVelocity(float dt) const
225 4bc1c0c1 Kevin Townsend
    {
226
        Vector<3> ret;
227
        Quaternion one(1.0, 0.0, 0.0, 0.0);
228
        Quaternion delta = one - *this;
229
        Quaternion r = (delta/dt);
230
        r = r * 2;
231
        r = r * one;
232
233
        ret.x() = r.x();
234
        ret.y() = r.y();
235
        ret.z() = r.z();
236
        return ret;
237
    }
238
239 0695bf91 Paul Du Bois (laptop)
    Vector<3> rotateVector(Vector<2> v) const
240 4bc1c0c1 Kevin Townsend
    {
241
        Vector<3> ret(v.x(), v.y(), 0.0);
242
        return rotateVector(ret);
243
    }
244
245 0695bf91 Paul Du Bois (laptop)
    Vector<3> rotateVector(Vector<3> v) const
246 4bc1c0c1 Kevin Townsend
    {
247
        Vector<3> qv(this->x(), this->y(), this->z());
248
        Vector<3> t;
249
        t = qv.cross(v) * 2.0;
250
        return v + (t * _w) + qv.cross(t);
251
    }
252
253
254 0695bf91 Paul Du Bois (laptop)
    Quaternion operator * (Quaternion q) const
255 4bc1c0c1 Kevin Townsend
    {
256
        Quaternion ret;
257
        ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z));
258
        ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y));
259
        ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x));
260
        ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w));
261
        return ret;
262
    }
263
264 0695bf91 Paul Du Bois (laptop)
    Quaternion operator + (Quaternion q) const
265 4bc1c0c1 Kevin Townsend
    {
266
        Quaternion ret;
267
        ret._w = _w + q._w;
268
        ret._x = _x + q._x;
269
        ret._y = _y + q._y;
270
        ret._z = _z + q._z;
271
        return ret;
272
    }
273
274 0695bf91 Paul Du Bois (laptop)
    Quaternion operator - (Quaternion q) const
275 4bc1c0c1 Kevin Townsend
    {
276
        Quaternion ret;
277
        ret._w = _w - q._w;
278
        ret._x = _x - q._x;
279
        ret._y = _y - q._y;
280
        ret._z = _z - q._z;
281
        return ret;
282
    }
283
284 0695bf91 Paul Du Bois (laptop)
    Quaternion operator / (float scalar) const
285 4bc1c0c1 Kevin Townsend
    {
286
        Quaternion ret;
287
        ret._w = this->_w/scalar;
288
        ret._x = this->_x/scalar;
289
        ret._y = this->_y/scalar;
290
        ret._z = this->_z/scalar;
291
        return ret;
292
    }
293
294 0695bf91 Paul Du Bois (laptop)
    Quaternion operator * (float scalar) const
295 4bc1c0c1 Kevin Townsend
    {
296
        Quaternion ret;
297
        ret._w = this->_w*scalar;
298
        ret._x = this->_x*scalar;
299
        ret._y = this->_y*scalar;
300
        ret._z = this->_z*scalar;
301
        return ret;
302
    }
303
304 0695bf91 Paul Du Bois (laptop)
    Quaternion scale(double scalar) const
305
    {
306 4bc1c0c1 Kevin Townsend
        Quaternion ret;
307
        ret._w = this->_w*scalar;
308
        ret._x = this->_x*scalar;
309
        ret._y = this->_y*scalar;
310
        ret._z = this->_z*scalar;
311
        return ret;
312
    }
313
314
private:
315
    double _w, _x, _y, _z;
316
};
317
318
319
};
320
321
#endif