Toyomasa Watarai / Adafruit_BNO055

Adafruit driver converted to Mbed OS 6.x.

Dependents:   Adafruit-BNO055-test

quaternion.h@0:22c544c8741a, 2015-09-16 (annotated)

Committer:
simonscott
Date:
Wed Sep 16 19:48:33 2015 +0000
Revision:
0:22c544c8741a
First version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simonscott 0:22c544c8741a 1 /*
simonscott 0:22c544c8741a 2 Inertial Measurement Unit Maths Library
simonscott 0:22c544c8741a 3 Copyright (C) 2013-2014 Samuel Cowen
simonscott 0:22c544c8741a 4 www.camelsoftware.com
simonscott 0:22c544c8741a 5
simonscott 0:22c544c8741a 6 This program is free software: you can redistribute it and/or modify
simonscott 0:22c544c8741a 7 it under the terms of the GNU General Public License as published by
simonscott 0:22c544c8741a 8 the Free Software Foundation, either version 3 of the License, or
simonscott 0:22c544c8741a 9 (at your option) any later version.
simonscott 0:22c544c8741a 10
simonscott 0:22c544c8741a 11 This program is distributed in the hope that it will be useful,
simonscott 0:22c544c8741a 12 but WITHOUT ANY WARRANTY; without even the implied warranty of
simonscott 0:22c544c8741a 13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
simonscott 0:22c544c8741a 14 GNU General Public License for more details.
simonscott 0:22c544c8741a 15
simonscott 0:22c544c8741a 16 You should have received a copy of the GNU General Public License
simonscott 0:22c544c8741a 17 along with this program. If not, see <http://www.gnu.org/licenses/>.
simonscott 0:22c544c8741a 18 */
simonscott 0:22c544c8741a 19
simonscott 0:22c544c8741a 20
simonscott 0:22c544c8741a 21 #ifndef IMUMATH_QUATERNION_HPP
simonscott 0:22c544c8741a 22 #define IMUMATH_QUATERNION_HPP
simonscott 0:22c544c8741a 23
simonscott 0:22c544c8741a 24 #include <stdlib.h>
simonscott 0:22c544c8741a 25 #include <string.h>
simonscott 0:22c544c8741a 26 #include <stdint.h>
simonscott 0:22c544c8741a 27 #include <math.h>
simonscott 0:22c544c8741a 28
simonscott 0:22c544c8741a 29 #include "vector.h"
simonscott 0:22c544c8741a 30
simonscott 0:22c544c8741a 31
simonscott 0:22c544c8741a 32 namespace imu
simonscott 0:22c544c8741a 33 {
simonscott 0:22c544c8741a 34
simonscott 0:22c544c8741a 35
simonscott 0:22c544c8741a 36
simonscott 0:22c544c8741a 37 class Quaternion
simonscott 0:22c544c8741a 38 {
simonscott 0:22c544c8741a 39 public:
simonscott 0:22c544c8741a 40 Quaternion()
simonscott 0:22c544c8741a 41 {
simonscott 0:22c544c8741a 42 _w = 1.0;
simonscott 0:22c544c8741a 43 _x = _y = _z = 0.0;
simonscott 0:22c544c8741a 44 }
simonscott 0:22c544c8741a 45
simonscott 0:22c544c8741a 46 Quaternion(double iw, double ix, double iy, double iz)
simonscott 0:22c544c8741a 47 {
simonscott 0:22c544c8741a 48 _w = iw;
simonscott 0:22c544c8741a 49 _x = ix;
simonscott 0:22c544c8741a 50 _y = iy;
simonscott 0:22c544c8741a 51 _z = iz;
simonscott 0:22c544c8741a 52 }
simonscott 0:22c544c8741a 53
simonscott 0:22c544c8741a 54 Quaternion(double w, Vector<3> vec)
simonscott 0:22c544c8741a 55 {
simonscott 0:22c544c8741a 56 _w = w;
simonscott 0:22c544c8741a 57 _x = vec.x();
simonscott 0:22c544c8741a 58 _y = vec.y();
simonscott 0:22c544c8741a 59 _z = vec.z();
simonscott 0:22c544c8741a 60 }
simonscott 0:22c544c8741a 61
simonscott 0:22c544c8741a 62 double& w()
simonscott 0:22c544c8741a 63 {
simonscott 0:22c544c8741a 64 return _w;
simonscott 0:22c544c8741a 65 }
simonscott 0:22c544c8741a 66 double& x()
simonscott 0:22c544c8741a 67 {
simonscott 0:22c544c8741a 68 return _x;
simonscott 0:22c544c8741a 69 }
simonscott 0:22c544c8741a 70 double& y()
simonscott 0:22c544c8741a 71 {
simonscott 0:22c544c8741a 72 return _y;
simonscott 0:22c544c8741a 73 }
simonscott 0:22c544c8741a 74 double& z()
simonscott 0:22c544c8741a 75 {
simonscott 0:22c544c8741a 76 return _z;
simonscott 0:22c544c8741a 77 }
simonscott 0:22c544c8741a 78
simonscott 0:22c544c8741a 79 double w() const
simonscott 0:22c544c8741a 80 {
simonscott 0:22c544c8741a 81 return _w;
simonscott 0:22c544c8741a 82 }
simonscott 0:22c544c8741a 83 double x() const
simonscott 0:22c544c8741a 84 {
simonscott 0:22c544c8741a 85 return _x;
simonscott 0:22c544c8741a 86 }
simonscott 0:22c544c8741a 87 double y() const
simonscott 0:22c544c8741a 88 {
simonscott 0:22c544c8741a 89 return _y;
simonscott 0:22c544c8741a 90 }
simonscott 0:22c544c8741a 91 double z() const
simonscott 0:22c544c8741a 92 {
simonscott 0:22c544c8741a 93 return _z;
simonscott 0:22c544c8741a 94 }
simonscott 0:22c544c8741a 95
simonscott 0:22c544c8741a 96 double magnitude() const
simonscott 0:22c544c8741a 97 {
simonscott 0:22c544c8741a 98 double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);
simonscott 0:22c544c8741a 99 return sqrt(res);
simonscott 0:22c544c8741a 100 }
simonscott 0:22c544c8741a 101
simonscott 0:22c544c8741a 102 void normalize()
simonscott 0:22c544c8741a 103 {
simonscott 0:22c544c8741a 104 double mag = magnitude();
simonscott 0:22c544c8741a 105 *this = this->scale(1/mag);
simonscott 0:22c544c8741a 106 }
simonscott 0:22c544c8741a 107
simonscott 0:22c544c8741a 108
simonscott 0:22c544c8741a 109 Quaternion conjugate() const
simonscott 0:22c544c8741a 110 {
simonscott 0:22c544c8741a 111 Quaternion q;
simonscott 0:22c544c8741a 112 q.w() = _w;
simonscott 0:22c544c8741a 113 q.x() = -_x;
simonscott 0:22c544c8741a 114 q.y() = -_y;
simonscott 0:22c544c8741a 115 q.z() = -_z;
simonscott 0:22c544c8741a 116 return q;
simonscott 0:22c544c8741a 117 }
simonscott 0:22c544c8741a 118
simonscott 0:22c544c8741a 119 void fromAxisAngle(Vector<3> axis, double theta)
simonscott 0:22c544c8741a 120 {
simonscott 0:22c544c8741a 121 _w = cos(theta/2);
simonscott 0:22c544c8741a 122 //only need to calculate sine of half theta once
simonscott 0:22c544c8741a 123 double sht = sin(theta/2);
simonscott 0:22c544c8741a 124 _x = axis.x() * sht;
simonscott 0:22c544c8741a 125 _y = axis.y() * sht;
simonscott 0:22c544c8741a 126 _z = axis.z() * sht;
simonscott 0:22c544c8741a 127 }
simonscott 0:22c544c8741a 128
simonscott 0:22c544c8741a 129 void fromMatrix(Matrix<3> m)
simonscott 0:22c544c8741a 130 {
simonscott 0:22c544c8741a 131 float tr = m(0, 0) + m(1, 1) + m(2, 2);
simonscott 0:22c544c8741a 132
simonscott 0:22c544c8741a 133 float S = 0.0;
simonscott 0:22c544c8741a 134 if (tr > 0)
simonscott 0:22c544c8741a 135 {
simonscott 0:22c544c8741a 136 S = sqrt(tr+1.0) * 2;
simonscott 0:22c544c8741a 137 _w = 0.25 * S;
simonscott 0:22c544c8741a 138 _x = (m(2, 1) - m(1, 2)) / S;
simonscott 0:22c544c8741a 139 _y = (m(0, 2) - m(2, 0)) / S;
simonscott 0:22c544c8741a 140 _z = (m(1, 0) - m(0, 1)) / S;
simonscott 0:22c544c8741a 141 }
simonscott 0:22c544c8741a 142 else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2)))
simonscott 0:22c544c8741a 143 {
simonscott 0:22c544c8741a 144 S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
simonscott 0:22c544c8741a 145 _w = (m(2, 1) - m(1, 2)) / S;
simonscott 0:22c544c8741a 146 _x = 0.25 * S;
simonscott 0:22c544c8741a 147 _y = (m(0, 1) + m(1, 0)) / S;
simonscott 0:22c544c8741a 148 _z = (m(0, 2) + m(2, 0)) / S;
simonscott 0:22c544c8741a 149 }
simonscott 0:22c544c8741a 150 else if (m(1, 1) < m(2, 2))
simonscott 0:22c544c8741a 151 {
simonscott 0:22c544c8741a 152 S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
simonscott 0:22c544c8741a 153 _w = (m(0, 2) - m(2, 0)) / S;
simonscott 0:22c544c8741a 154 _x = (m(0, 1) + m(1, 0)) / S;
simonscott 0:22c544c8741a 155 _y = 0.25 * S;
simonscott 0:22c544c8741a 156 _z = (m(1, 2) + m(2, 1)) / S;
simonscott 0:22c544c8741a 157 }
simonscott 0:22c544c8741a 158 else
simonscott 0:22c544c8741a 159 {
simonscott 0:22c544c8741a 160 S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
simonscott 0:22c544c8741a 161 _w = (m(1, 0) - m(0, 1)) / S;
simonscott 0:22c544c8741a 162 _x = (m(0, 2) + m(2, 0)) / S;
simonscott 0:22c544c8741a 163 _y = (m(1, 2) + m(2, 1)) / S;
simonscott 0:22c544c8741a 164 _z = 0.25 * S;
simonscott 0:22c544c8741a 165 }
simonscott 0:22c544c8741a 166 }
simonscott 0:22c544c8741a 167
simonscott 0:22c544c8741a 168 void toAxisAngle(Vector<3>& axis, float& angle) const
simonscott 0:22c544c8741a 169 {
simonscott 0:22c544c8741a 170 float sqw = sqrt(1-_w*_w);
simonscott 0:22c544c8741a 171 if(sqw == 0) //it's a singularity and divide by zero, avoid
simonscott 0:22c544c8741a 172 return;
simonscott 0:22c544c8741a 173
simonscott 0:22c544c8741a 174 angle = 2 * acos(_w);
simonscott 0:22c544c8741a 175 axis.x() = _x / sqw;
simonscott 0:22c544c8741a 176 axis.y() = _y / sqw;
simonscott 0:22c544c8741a 177 axis.z() = _z / sqw;
simonscott 0:22c544c8741a 178 }
simonscott 0:22c544c8741a 179
simonscott 0:22c544c8741a 180 Matrix<3> toMatrix() const
simonscott 0:22c544c8741a 181 {
simonscott 0:22c544c8741a 182 Matrix<3> ret;
simonscott 0:22c544c8741a 183 ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z));
simonscott 0:22c544c8741a 184 ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z);
simonscott 0:22c544c8741a 185 ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y);
simonscott 0:22c544c8741a 186
simonscott 0:22c544c8741a 187 ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z);
simonscott 0:22c544c8741a 188 ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z));
simonscott 0:22c544c8741a 189 ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x));
simonscott 0:22c544c8741a 190
simonscott 0:22c544c8741a 191 ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y);
simonscott 0:22c544c8741a 192 ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x);
simonscott 0:22c544c8741a 193 ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y));
simonscott 0:22c544c8741a 194 return ret;
simonscott 0:22c544c8741a 195 }
simonscott 0:22c544c8741a 196
simonscott 0:22c544c8741a 197
simonscott 0:22c544c8741a 198 // Returns euler angles that represent the quaternion. Angles are
simonscott 0:22c544c8741a 199 // returned in rotation order and right-handed about the specified
simonscott 0:22c544c8741a 200 // axes:
simonscott 0:22c544c8741a 201 //
simonscott 0:22c544c8741a 202 // v[0] is applied 1st about z (ie, roll)
simonscott 0:22c544c8741a 203 // v[1] is applied 2nd about y (ie, pitch)
simonscott 0:22c544c8741a 204 // v[2] is applied 3rd about x (ie, yaw)
simonscott 0:22c544c8741a 205 //
simonscott 0:22c544c8741a 206 // Note that this means result.x() is not a rotation about x;
simonscott 0:22c544c8741a 207 // similarly for result.z().
simonscott 0:22c544c8741a 208 //
simonscott 0:22c544c8741a 209 Vector<3> toEuler() const
simonscott 0:22c544c8741a 210 {
simonscott 0:22c544c8741a 211 Vector<3> ret;
simonscott 0:22c544c8741a 212 double sqw = _w*_w;
simonscott 0:22c544c8741a 213 double sqx = _x*_x;
simonscott 0:22c544c8741a 214 double sqy = _y*_y;
simonscott 0:22c544c8741a 215 double sqz = _z*_z;
simonscott 0:22c544c8741a 216
simonscott 0:22c544c8741a 217 ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));
simonscott 0:22c544c8741a 218 ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));
simonscott 0:22c544c8741a 219 ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));
simonscott 0:22c544c8741a 220
simonscott 0:22c544c8741a 221 return ret;
simonscott 0:22c544c8741a 222 }
simonscott 0:22c544c8741a 223
simonscott 0:22c544c8741a 224 Vector<3> toAngularVelocity(float dt) const
simonscott 0:22c544c8741a 225 {
simonscott 0:22c544c8741a 226 Vector<3> ret;
simonscott 0:22c544c8741a 227 Quaternion one(1.0, 0.0, 0.0, 0.0);
simonscott 0:22c544c8741a 228 Quaternion delta = one - *this;
simonscott 0:22c544c8741a 229 Quaternion r = (delta/dt);
simonscott 0:22c544c8741a 230 r = r * 2;
simonscott 0:22c544c8741a 231 r = r * one;
simonscott 0:22c544c8741a 232
simonscott 0:22c544c8741a 233 ret.x() = r.x();
simonscott 0:22c544c8741a 234 ret.y() = r.y();
simonscott 0:22c544c8741a 235 ret.z() = r.z();
simonscott 0:22c544c8741a 236 return ret;
simonscott 0:22c544c8741a 237 }
simonscott 0:22c544c8741a 238
simonscott 0:22c544c8741a 239 Vector<3> rotateVector(Vector<2> v) const
simonscott 0:22c544c8741a 240 {
simonscott 0:22c544c8741a 241 Vector<3> ret(v.x(), v.y(), 0.0);
simonscott 0:22c544c8741a 242 return rotateVector(ret);
simonscott 0:22c544c8741a 243 }
simonscott 0:22c544c8741a 244
simonscott 0:22c544c8741a 245 Vector<3> rotateVector(Vector<3> v) const
simonscott 0:22c544c8741a 246 {
simonscott 0:22c544c8741a 247 Vector<3> qv(this->x(), this->y(), this->z());
simonscott 0:22c544c8741a 248 Vector<3> t;
simonscott 0:22c544c8741a 249 t = qv.cross(v) * 2.0;
simonscott 0:22c544c8741a 250 return v + (t * _w) + qv.cross(t);
simonscott 0:22c544c8741a 251 }
simonscott 0:22c544c8741a 252
simonscott 0:22c544c8741a 253
simonscott 0:22c544c8741a 254 Quaternion operator * (Quaternion q) const
simonscott 0:22c544c8741a 255 {
simonscott 0:22c544c8741a 256 Quaternion ret;
simonscott 0:22c544c8741a 257 ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z));
simonscott 0:22c544c8741a 258 ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y));
simonscott 0:22c544c8741a 259 ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x));
simonscott 0:22c544c8741a 260 ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w));
simonscott 0:22c544c8741a 261 return ret;
simonscott 0:22c544c8741a 262 }
simonscott 0:22c544c8741a 263
simonscott 0:22c544c8741a 264 Quaternion operator + (Quaternion q) const
simonscott 0:22c544c8741a 265 {
simonscott 0:22c544c8741a 266 Quaternion ret;
simonscott 0:22c544c8741a 267 ret._w = _w + q._w;
simonscott 0:22c544c8741a 268 ret._x = _x + q._x;
simonscott 0:22c544c8741a 269 ret._y = _y + q._y;
simonscott 0:22c544c8741a 270 ret._z = _z + q._z;
simonscott 0:22c544c8741a 271 return ret;
simonscott 0:22c544c8741a 272 }
simonscott 0:22c544c8741a 273
simonscott 0:22c544c8741a 274 Quaternion operator - (Quaternion q) const
simonscott 0:22c544c8741a 275 {
simonscott 0:22c544c8741a 276 Quaternion ret;
simonscott 0:22c544c8741a 277 ret._w = _w - q._w;
simonscott 0:22c544c8741a 278 ret._x = _x - q._x;
simonscott 0:22c544c8741a 279 ret._y = _y - q._y;
simonscott 0:22c544c8741a 280 ret._z = _z - q._z;
simonscott 0:22c544c8741a 281 return ret;
simonscott 0:22c544c8741a 282 }
simonscott 0:22c544c8741a 283
simonscott 0:22c544c8741a 284 Quaternion operator / (float scalar) const
simonscott 0:22c544c8741a 285 {
simonscott 0:22c544c8741a 286 Quaternion ret;
simonscott 0:22c544c8741a 287 ret._w = this->_w/scalar;
simonscott 0:22c544c8741a 288 ret._x = this->_x/scalar;
simonscott 0:22c544c8741a 289 ret._y = this->_y/scalar;
simonscott 0:22c544c8741a 290 ret._z = this->_z/scalar;
simonscott 0:22c544c8741a 291 return ret;
simonscott 0:22c544c8741a 292 }
simonscott 0:22c544c8741a 293
simonscott 0:22c544c8741a 294 Quaternion operator * (float scalar) const
simonscott 0:22c544c8741a 295 {
simonscott 0:22c544c8741a 296 Quaternion ret;
simonscott 0:22c544c8741a 297 ret._w = this->_w*scalar;
simonscott 0:22c544c8741a 298 ret._x = this->_x*scalar;
simonscott 0:22c544c8741a 299 ret._y = this->_y*scalar;
simonscott 0:22c544c8741a 300 ret._z = this->_z*scalar;
simonscott 0:22c544c8741a 301 return ret;
simonscott 0:22c544c8741a 302 }
simonscott 0:22c544c8741a 303
simonscott 0:22c544c8741a 304 Quaternion scale(double scalar) const
simonscott 0:22c544c8741a 305 {
simonscott 0:22c544c8741a 306 Quaternion ret;
simonscott 0:22c544c8741a 307 ret._w = this->_w*scalar;
simonscott 0:22c544c8741a 308 ret._x = this->_x*scalar;
simonscott 0:22c544c8741a 309 ret._y = this->_y*scalar;
simonscott 0:22c544c8741a 310 ret._z = this->_z*scalar;
simonscott 0:22c544c8741a 311 return ret;
simonscott 0:22c544c8741a 312 }
simonscott 0:22c544c8741a 313
simonscott 0:22c544c8741a 314 private:
simonscott 0:22c544c8741a 315 double _w, _x, _y, _z;
simonscott 0:22c544c8741a 316 };
simonscott 0:22c544c8741a 317
simonscott 0:22c544c8741a 318
simonscott 0:22c544c8741a 319 };
simonscott 0:22c544c8741a 320
simonscott 0:22c544c8741a 321 #endif