Statistics
| Branch: | Revision:

adafruit_bno055 / utility / quaternion.h @ 5e9f001d

History | View | Annotate | Download (6.503 KB)

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

6
    Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
7

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

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

18
    You should have received a copy of the GNU General Public License
19
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
20
*/
21

    
22

    
23
#ifndef IMUMATH_QUATERNION_HPP
24
#define IMUMATH_QUATERNION_HPP
25

    
26
#include <stdlib.h>
27
#include <string.h>
28
#include <stdint.h>
29
#include <math.h>
30

    
31
#include "matrix.h"
32

    
33

    
34
namespace imu
35
{
36

    
37
class Quaternion
38
{
39
public:
40
    Quaternion(): _w(1.0), _x(0.0), _y(0.0), _z(0.0) {}
41

    
42
    Quaternion(double w, double x, double y, double z):
43
        _w(w), _x(x), _y(y), _z(z) {}
44

    
45
    Quaternion(double w, Vector<3> vec):
46
        _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {}
47

    
48
    double& w()
49
    {
50
        return _w;
51
    }
52
    double& x()
53
    {
54
        return _x;
55
    }
56
    double& y()
57
    {
58
        return _y;
59
    }
60
    double& z()
61
    {
62
        return _z;
63
    }
64

    
65
    double w() const
66
    {
67
        return _w;
68
    }
69
    double x() const
70
    {
71
        return _x;
72
    }
73
    double y() const
74
    {
75
        return _y;
76
    }
77
    double z() const
78
    {
79
        return _z;
80
    }
81

    
82
    double magnitude() const
83
    {
84
        return sqrt(_w*_w + _x*_x + _y*_y + _z*_z);
85
    }
86

    
87
    void normalize()
88
    {
89
        double mag = magnitude();
90
        *this = this->scale(1/mag);
91
    }
92

    
93
    Quaternion conjugate() const
94
    {
95
        return Quaternion(_w, -_x, -_y, -_z);
96
    }
97

    
98
    void fromAxisAngle(const Vector<3>& axis, double theta)
99
    {
100
        _w = cos(theta/2);
101
        //only need to calculate sine of half theta once
102
        double sht = sin(theta/2);
103
        _x = axis.x() * sht;
104
        _y = axis.y() * sht;
105
        _z = axis.z() * sht;
106
    }
107

    
108
    void fromMatrix(const Matrix<3>& m)
109
    {
110
        double tr = m.trace();
111

    
112
        double S;
113
        if (tr > 0)
114
        {
115
            S = sqrt(tr+1.0) * 2;
116
            _w = 0.25 * S;
117
            _x = (m(2, 1) - m(1, 2)) / S;
118
            _y = (m(0, 2) - m(2, 0)) / S;
119
            _z = (m(1, 0) - m(0, 1)) / S;
120
        }
121
        else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2))
122
        {
123
            S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
124
            _w = (m(2, 1) - m(1, 2)) / S;
125
            _x = 0.25 * S;
126
            _y = (m(0, 1) + m(1, 0)) / S;
127
            _z = (m(0, 2) + m(2, 0)) / S;
128
        }
129
        else if (m(1, 1) > m(2, 2))
130
        {
131
            S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
132
            _w = (m(0, 2) - m(2, 0)) / S;
133
            _x = (m(0, 1) + m(1, 0)) / S;
134
            _y = 0.25 * S;
135
            _z = (m(1, 2) + m(2, 1)) / S;
136
        }
137
        else
138
        {
139
            S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
140
            _w = (m(1, 0) - m(0, 1)) / S;
141
            _x = (m(0, 2) + m(2, 0)) / S;
142
            _y = (m(1, 2) + m(2, 1)) / S;
143
            _z = 0.25 * S;
144
        }
145
    }
146

    
147
    void toAxisAngle(Vector<3>& axis, double& angle) const
148
    {
149
        double sqw = sqrt(1-_w*_w);
150
        if (sqw == 0) //it's a singularity and divide by zero, avoid
151
            return;
152

    
153
        angle = 2 * acos(_w);
154
        axis.x() = _x / sqw;
155
        axis.y() = _y / sqw;
156
        axis.z() = _z / sqw;
157
    }
158

    
159
    Matrix<3> toMatrix() const
160
    {
161
        Matrix<3> ret;
162
        ret.cell(0, 0) = 1 - 2*_y*_y - 2*_z*_z;
163
        ret.cell(0, 1) = 2*_x*_y - 2*_w*_z;
164
        ret.cell(0, 2) = 2*_x*_z + 2*_w*_y;
165

    
166
        ret.cell(1, 0) = 2*_x*_y + 2*_w*_z;
167
        ret.cell(1, 1) = 1 - 2*_x*_x - 2*_z*_z;
168
        ret.cell(1, 2) = 2*_y*_z - 2*_w*_x;
169

    
170
        ret.cell(2, 0) = 2*_x*_z - 2*_w*_y;
171
        ret.cell(2, 1) = 2*_y*_z + 2*_w*_x;
172
        ret.cell(2, 2) = 1 - 2*_x*_x - 2*_y*_y;
173
        return ret;
174
    }
175

    
176

    
177
    // Returns euler angles that represent the quaternion.  Angles are
178
    // returned in rotation order and right-handed about the specified
179
    // axes:
180
    //
181
    //   v[0] is applied 1st about z (ie, roll)
182
    //   v[1] is applied 2nd about y (ie, pitch)
183
    //   v[2] is applied 3rd about x (ie, yaw)
184
    //
185
    // Note that this means result.x() is not a rotation about x;
186
    // similarly for result.z().
187
    //
188
    Vector<3> toEuler() const
189
    {
190
        Vector<3> ret;
191
        double sqw = _w*_w;
192
        double sqx = _x*_x;
193
        double sqy = _y*_y;
194
        double sqz = _z*_z;
195

    
196
        ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));
197
        ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));
198
        ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));
199

    
200
        return ret;
201
    }
202

    
203
    Vector<3> toAngularVelocity(double dt) const
204
    {
205
        Vector<3> ret;
206
        Quaternion one(1.0, 0.0, 0.0, 0.0);
207
        Quaternion delta = one - *this;
208
        Quaternion r = (delta/dt);
209
        r = r * 2;
210
        r = r * one;
211

    
212
        ret.x() = r.x();
213
        ret.y() = r.y();
214
        ret.z() = r.z();
215
        return ret;
216
    }
217

    
218
    Vector<3> rotateVector(const Vector<2>& v) const
219
    {
220
        return rotateVector(Vector<3>(v.x(), v.y()));
221
    }
222

    
223
    Vector<3> rotateVector(const Vector<3>& v) const
224
    {
225
        Vector<3> qv(_x, _y, _z);
226
        Vector<3> t = qv.cross(v) * 2.0;
227
        return v + t*_w + qv.cross(t);
228
    }
229

    
230

    
231
    Quaternion operator*(const Quaternion& q) const
232
    {
233
        return Quaternion(
234
            _w*q._w - _x*q._x - _y*q._y - _z*q._z,
235
            _w*q._x + _x*q._w + _y*q._z - _z*q._y,
236
            _w*q._y - _x*q._z + _y*q._w + _z*q._x,
237
            _w*q._z + _x*q._y - _y*q._x + _z*q._w
238
        );
239
    }
240

    
241
    Quaternion operator+(const Quaternion& q) const
242
    {
243
        return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
244
    }
245

    
246
    Quaternion operator-(const Quaternion& q) const
247
    {
248
        return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
249
    }
250

    
251
    Quaternion operator/(double scalar) const
252
    {
253
        return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
254
    }
255

    
256
    Quaternion operator*(double scalar) const
257
    {
258
        return scale(scalar);
259
    }
260

    
261
    Quaternion scale(double scalar) const
262
    {
263
        return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
264
    }
265

    
266
private:
267
    double _w, _x, _y, _z;
268
};
269

    
270
} // namespace
271

    
272
#endif