Statistics
| Branch: | Revision:

adafruit_bno055 / utility / quaternion.h @ 3cae40b9

History | View | Annotate | Download (6.44 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 4a94251b Gé Vissers
#include "matrix.h"
30 4bc1c0c1 Kevin Townsend
31
32
namespace imu
33
{
34
35
class Quaternion
36
{
37
public:
38 88b09bb5 Gé Vissers
    Quaternion(): _w(1.0), _x(0.0), _y(0.0), _z(0.0) {}
39 4bc1c0c1 Kevin Townsend
40 88b09bb5 Gé Vissers
    Quaternion(double w, double x, double y, double z):
41
        _w(w), _x(x), _y(y), _z(z) {}
42 4bc1c0c1 Kevin Townsend
43 88b09bb5 Gé Vissers
    Quaternion(double w, Vector<3> vec):
44
        _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {}
45 4bc1c0c1 Kevin Townsend
46
    double& w()
47
    {
48
        return _w;
49
    }
50
    double& x()
51
    {
52
        return _x;
53
    }
54
    double& y()
55
    {
56
        return _y;
57
    }
58
    double& z()
59
    {
60
        return _z;
61
    }
62
63 0695bf91 Paul Du Bois (laptop)
    double w() const
64
    {
65
        return _w;
66
    }
67
    double x() const
68
    {
69
        return _x;
70
    }
71
    double y() const
72
    {
73
        return _y;
74
    }
75
    double z() const
76
    {
77
        return _z;
78
    }
79
80
    double magnitude() const
81 4bc1c0c1 Kevin Townsend
    {
82 2b07acc9 Gé Vissers
        return sqrt(_w*_w + _x*_x + _y*_y + _z*_z);
83 4bc1c0c1 Kevin Townsend
    }
84
85
    void normalize()
86
    {
87 0695bf91 Paul Du Bois (laptop)
        double mag = magnitude();
88 4bc1c0c1 Kevin Townsend
        *this = this->scale(1/mag);
89
    }
90
91 2b07acc9 Gé Vissers
    Quaternion conjugate() const
92 4bc1c0c1 Kevin Townsend
    {
93 0ecc7129 Gé Vissers
        return Quaternion(_w, -_x, -_y, -_z);
94 4bc1c0c1 Kevin Townsend
    }
95
96 2b07acc9 Gé Vissers
    void fromAxisAngle(const Vector<3>& axis, double theta)
97 4bc1c0c1 Kevin Townsend
    {
98
        _w = cos(theta/2);
99
        //only need to calculate sine of half theta once
100
        double sht = sin(theta/2);
101
        _x = axis.x() * sht;
102
        _y = axis.y() * sht;
103
        _z = axis.z() * sht;
104
    }
105
106 e8e79779 Gé Vissers
    void fromMatrix(const Matrix<3>& m)
107 4bc1c0c1 Kevin Townsend
    {
108 e8e79779 Gé Vissers
        double tr = m.trace();
109 4bc1c0c1 Kevin Townsend
110 e8e79779 Gé Vissers
        double S;
111 4bc1c0c1 Kevin Townsend
        if (tr > 0)
112
        {
113
            S = sqrt(tr+1.0) * 2;
114
            _w = 0.25 * S;
115
            _x = (m(2, 1) - m(1, 2)) / S;
116
            _y = (m(0, 2) - m(2, 0)) / S;
117
            _z = (m(1, 0) - m(0, 1)) / S;
118
        }
119 e8e79779 Gé Vissers
        else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2))
120 4bc1c0c1 Kevin Townsend
        {
121
            S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
122
            _w = (m(2, 1) - m(1, 2)) / S;
123
            _x = 0.25 * S;
124
            _y = (m(0, 1) + m(1, 0)) / S;
125
            _z = (m(0, 2) + m(2, 0)) / S;
126
        }
127 e8e79779 Gé Vissers
        else if (m(1, 1) > m(2, 2))
128 4bc1c0c1 Kevin Townsend
        {
129
            S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
130
            _w = (m(0, 2) - m(2, 0)) / S;
131
            _x = (m(0, 1) + m(1, 0)) / S;
132
            _y = 0.25 * S;
133
            _z = (m(1, 2) + m(2, 1)) / S;
134
        }
135
        else
136
        {
137
            S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
138
            _w = (m(1, 0) - m(0, 1)) / S;
139
            _x = (m(0, 2) + m(2, 0)) / S;
140
            _y = (m(1, 2) + m(2, 1)) / S;
141
            _z = 0.25 * S;
142
        }
143
    }
144
145 2dd31024 Gé Vissers
    void toAxisAngle(Vector<3>& axis, double& angle) const
146 4bc1c0c1 Kevin Townsend
    {
147 2dd31024 Gé Vissers
        double sqw = sqrt(1-_w*_w);
148
        if (sqw == 0) //it's a singularity and divide by zero, avoid
149 4bc1c0c1 Kevin Townsend
            return;
150
151
        angle = 2 * acos(_w);
152
        axis.x() = _x / sqw;
153
        axis.y() = _y / sqw;
154
        axis.z() = _z / sqw;
155
    }
156
157 0695bf91 Paul Du Bois (laptop)
    Matrix<3> toMatrix() const
158 4bc1c0c1 Kevin Townsend
    {
159
        Matrix<3> ret;
160 7ede6000 Gé Vissers
        ret.cell(0, 0) = 1 - 2*_y*_y - 2*_z*_z;
161
        ret.cell(0, 1) = 2*_x*_y - 2*_w*_z;
162
        ret.cell(0, 2) = 2*_x*_z + 2*_w*_y;
163 4bc1c0c1 Kevin Townsend
164 7ede6000 Gé Vissers
        ret.cell(1, 0) = 2*_x*_y + 2*_w*_z;
165
        ret.cell(1, 1) = 1 - 2*_x*_x - 2*_z*_z;
166
        ret.cell(1, 2) = 2*_y*_z - 2*_w*_x;
167 4bc1c0c1 Kevin Townsend
168 7ede6000 Gé Vissers
        ret.cell(2, 0) = 2*_x*_z - 2*_w*_y;
169
        ret.cell(2, 1) = 2*_y*_z + 2*_w*_x;
170
        ret.cell(2, 2) = 1 - 2*_x*_x - 2*_y*_y;
171 4bc1c0c1 Kevin Townsend
        return ret;
172
    }
173
174
175 0695bf91 Paul Du Bois (laptop)
    // Returns euler angles that represent the quaternion.  Angles are
176
    // returned in rotation order and right-handed about the specified
177
    // axes:
178
    //
179
    //   v[0] is applied 1st about z (ie, roll)
180
    //   v[1] is applied 2nd about y (ie, pitch)
181
    //   v[2] is applied 3rd about x (ie, yaw)
182
    //
183
    // Note that this means result.x() is not a rotation about x;
184
    // similarly for result.z().
185
    //
186
    Vector<3> toEuler() const
187 4bc1c0c1 Kevin Townsend
    {
188
        Vector<3> ret;
189
        double sqw = _w*_w;
190
        double sqx = _x*_x;
191
        double sqy = _y*_y;
192
        double sqz = _z*_z;
193
194
        ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));
195
        ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));
196
        ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));
197
198
        return ret;
199
    }
200
201 2dd31024 Gé Vissers
    Vector<3> toAngularVelocity(double dt) const
202 4bc1c0c1 Kevin Townsend
    {
203
        Vector<3> ret;
204
        Quaternion one(1.0, 0.0, 0.0, 0.0);
205
        Quaternion delta = one - *this;
206
        Quaternion r = (delta/dt);
207
        r = r * 2;
208
        r = r * one;
209
210
        ret.x() = r.x();
211
        ret.y() = r.y();
212
        ret.z() = r.z();
213
        return ret;
214
    }
215
216 2b07acc9 Gé Vissers
    Vector<3> rotateVector(const Vector<2>& v) const
217 4bc1c0c1 Kevin Townsend
    {
218 2b07acc9 Gé Vissers
        return rotateVector(Vector<3>(v.x(), v.y()));
219 4bc1c0c1 Kevin Townsend
    }
220
221 2b07acc9 Gé Vissers
    Vector<3> rotateVector(const Vector<3>& v) const
222 4bc1c0c1 Kevin Townsend
    {
223 2b07acc9 Gé Vissers
        Vector<3> qv(_x, _y, _z);
224
        Vector<3> t = qv.cross(v) * 2.0;
225
        return v + t*_w + qv.cross(t);
226 4bc1c0c1 Kevin Townsend
    }
227
228
229 88b09bb5 Gé Vissers
    Quaternion operator*(const Quaternion& q) const
230 4bc1c0c1 Kevin Townsend
    {
231 0ecc7129 Gé Vissers
        return Quaternion(
232
            _w*q._w - _x*q._x - _y*q._y - _z*q._z,
233
            _w*q._x + _x*q._w + _y*q._z - _z*q._y,
234
            _w*q._y - _x*q._z + _y*q._w + _z*q._x,
235
            _w*q._z + _x*q._y - _y*q._x + _z*q._w
236
        );
237 4bc1c0c1 Kevin Townsend
    }
238
239 88b09bb5 Gé Vissers
    Quaternion operator+(const Quaternion& q) const
240 4bc1c0c1 Kevin Townsend
    {
241 0ecc7129 Gé Vissers
        return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
242 4bc1c0c1 Kevin Townsend
    }
243
244 88b09bb5 Gé Vissers
    Quaternion operator-(const Quaternion& q) const
245 4bc1c0c1 Kevin Townsend
    {
246 0ecc7129 Gé Vissers
        return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
247 4bc1c0c1 Kevin Townsend
    }
248
249 88b09bb5 Gé Vissers
    Quaternion operator/(double scalar) const
250 4bc1c0c1 Kevin Townsend
    {
251 0ecc7129 Gé Vissers
        return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
252 4bc1c0c1 Kevin Townsend
    }
253
254 88b09bb5 Gé Vissers
    Quaternion operator*(double scalar) const
255 4bc1c0c1 Kevin Townsend
    {
256 0ecc7129 Gé Vissers
        return scale(scalar);
257 4bc1c0c1 Kevin Townsend
    }
258
259 88b09bb5 Gé Vissers
    Quaternion scale(double scalar) const
260 0695bf91 Paul Du Bois (laptop)
    {
261 0ecc7129 Gé Vissers
        return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
262 4bc1c0c1 Kevin Townsend
    }
263
264
private:
265
    double _w, _x, _y, _z;
266
};
267
268 0ecc7129 Gé Vissers
} // namespace
269 4bc1c0c1 Kevin Townsend
270
#endif