Adafruit driver converted to Mbed OS 6.x.
Dependents: Adafruit-BNO055-test
quaternion.h
00001 /* 00002 Inertial Measurement Unit Maths Library 00003 Copyright (C) 2013-2014 Samuel Cowen 00004 www.camelsoftware.com 00005 00006 This program is free software: you can redistribute it and/or modify 00007 it under the terms of the GNU General Public License as published by 00008 the Free Software Foundation, either version 3 of the License, or 00009 (at your option) any later version. 00010 00011 This program is distributed in the hope that it will be useful, 00012 but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 GNU General Public License for more details. 00015 00016 You should have received a copy of the GNU General Public License 00017 along with this program. If not, see <http://www.gnu.org/licenses/>. 00018 */ 00019 00020 00021 #ifndef IMUMATH_QUATERNION_HPP 00022 #define IMUMATH_QUATERNION_HPP 00023 00024 #include <stdlib.h> 00025 #include <string.h> 00026 #include <stdint.h> 00027 #include <math.h> 00028 00029 #include "vector.h" 00030 00031 00032 namespace imu 00033 { 00034 00035 00036 00037 class Quaternion 00038 { 00039 public: 00040 Quaternion() 00041 { 00042 _w = 1.0; 00043 _x = _y = _z = 0.0; 00044 } 00045 00046 Quaternion(double iw, double ix, double iy, double iz) 00047 { 00048 _w = iw; 00049 _x = ix; 00050 _y = iy; 00051 _z = iz; 00052 } 00053 00054 Quaternion(double w, Vector<3> vec) 00055 { 00056 _w = w; 00057 _x = vec.x(); 00058 _y = vec.y(); 00059 _z = vec.z(); 00060 } 00061 00062 double& w() 00063 { 00064 return _w; 00065 } 00066 double& x() 00067 { 00068 return _x; 00069 } 00070 double& y() 00071 { 00072 return _y; 00073 } 00074 double& z() 00075 { 00076 return _z; 00077 } 00078 00079 double w() const 00080 { 00081 return _w; 00082 } 00083 double x() const 00084 { 00085 return _x; 00086 } 00087 double y() const 00088 { 00089 return _y; 00090 } 00091 double z() const 00092 { 00093 return _z; 00094 } 00095 00096 double magnitude() const 00097 { 00098 double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z); 00099 return sqrt(res); 00100 } 00101 00102 void normalize() 00103 { 00104 double mag = magnitude(); 00105 *this = this->scale(1/mag); 00106 } 00107 00108 00109 Quaternion conjugate() const 00110 { 00111 Quaternion q; 00112 q.w() = _w; 00113 q.x() = -_x; 00114 q.y() = -_y; 00115 q.z() = -_z; 00116 return q; 00117 } 00118 00119 void fromAxisAngle(Vector<3> axis, double theta) 00120 { 00121 _w = cos(theta/2); 00122 //only need to calculate sine of half theta once 00123 double sht = sin(theta/2); 00124 _x = axis.x() * sht; 00125 _y = axis.y() * sht; 00126 _z = axis.z() * sht; 00127 } 00128 00129 void fromMatrix(Matrix<3> m) 00130 { 00131 float tr = m(0, 0) + m(1, 1) + m(2, 2); 00132 00133 float S = 0.0; 00134 if (tr > 0) 00135 { 00136 S = sqrt(tr+1.0) * 2; 00137 _w = 0.25 * S; 00138 _x = (m(2, 1) - m(1, 2)) / S; 00139 _y = (m(0, 2) - m(2, 0)) / S; 00140 _z = (m(1, 0) - m(0, 1)) / S; 00141 } 00142 else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2))) 00143 { 00144 S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; 00145 _w = (m(2, 1) - m(1, 2)) / S; 00146 _x = 0.25 * S; 00147 _y = (m(0, 1) + m(1, 0)) / S; 00148 _z = (m(0, 2) + m(2, 0)) / S; 00149 } 00150 else if (m(1, 1) < m(2, 2)) 00151 { 00152 S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; 00153 _w = (m(0, 2) - m(2, 0)) / S; 00154 _x = (m(0, 1) + m(1, 0)) / S; 00155 _y = 0.25 * S; 00156 _z = (m(1, 2) + m(2, 1)) / S; 00157 } 00158 else 00159 { 00160 S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; 00161 _w = (m(1, 0) - m(0, 1)) / S; 00162 _x = (m(0, 2) + m(2, 0)) / S; 00163 _y = (m(1, 2) + m(2, 1)) / S; 00164 _z = 0.25 * S; 00165 } 00166 } 00167 00168 void toAxisAngle(Vector<3>& axis, float& angle) const 00169 { 00170 float sqw = sqrt(1-_w*_w); 00171 if(sqw == 0) //it's a singularity and divide by zero, avoid 00172 return; 00173 00174 angle = 2 * acos(_w); 00175 axis.x() = _x / sqw; 00176 axis.y() = _y / sqw; 00177 axis.z() = _z / sqw; 00178 } 00179 00180 Matrix<3> toMatrix() const 00181 { 00182 Matrix<3> ret; 00183 ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z)); 00184 ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z); 00185 ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y); 00186 00187 ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z); 00188 ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z)); 00189 ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x)); 00190 00191 ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y); 00192 ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x); 00193 ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y)); 00194 return ret; 00195 } 00196 00197 00198 // Returns euler angles that represent the quaternion. Angles are 00199 // returned in rotation order and right-handed about the specified 00200 // axes: 00201 // 00202 // v[0] is applied 1st about z (ie, roll) 00203 // v[1] is applied 2nd about y (ie, pitch) 00204 // v[2] is applied 3rd about x (ie, yaw) 00205 // 00206 // Note that this means result.x() is not a rotation about x; 00207 // similarly for result.z(). 00208 // 00209 Vector<3> toEuler() const 00210 { 00211 Vector<3> ret; 00212 double sqw = _w*_w; 00213 double sqx = _x*_x; 00214 double sqy = _y*_y; 00215 double sqz = _z*_z; 00216 00217 ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw)); 00218 ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw)); 00219 ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw)); 00220 00221 return ret; 00222 } 00223 00224 Vector<3> toAngularVelocity(float dt) const 00225 { 00226 Vector<3> ret; 00227 Quaternion one(1.0, 0.0, 0.0, 0.0); 00228 Quaternion delta = one - *this; 00229 Quaternion r = (delta/dt); 00230 r = r * 2; 00231 r = r * one; 00232 00233 ret.x() = r.x(); 00234 ret.y() = r.y(); 00235 ret.z() = r.z(); 00236 return ret; 00237 } 00238 00239 Vector<3> rotateVector(Vector<2> v) const 00240 { 00241 Vector<3> ret(v.x(), v.y(), 0.0); 00242 return rotateVector(ret); 00243 } 00244 00245 Vector<3> rotateVector(Vector<3> v) const 00246 { 00247 Vector<3> qv(this->x(), this->y(), this->z()); 00248 Vector<3> t; 00249 t = qv.cross(v) * 2.0; 00250 return v + (t * _w) + qv.cross(t); 00251 } 00252 00253 00254 Quaternion operator * (Quaternion q) const 00255 { 00256 Quaternion ret; 00257 ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z)); 00258 ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y)); 00259 ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x)); 00260 ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w)); 00261 return ret; 00262 } 00263 00264 Quaternion operator + (Quaternion q) const 00265 { 00266 Quaternion ret; 00267 ret._w = _w + q._w; 00268 ret._x = _x + q._x; 00269 ret._y = _y + q._y; 00270 ret._z = _z + q._z; 00271 return ret; 00272 } 00273 00274 Quaternion operator - (Quaternion q) const 00275 { 00276 Quaternion ret; 00277 ret._w = _w - q._w; 00278 ret._x = _x - q._x; 00279 ret._y = _y - q._y; 00280 ret._z = _z - q._z; 00281 return ret; 00282 } 00283 00284 Quaternion operator / (float scalar) const 00285 { 00286 Quaternion ret; 00287 ret._w = this->_w/scalar; 00288 ret._x = this->_x/scalar; 00289 ret._y = this->_y/scalar; 00290 ret._z = this->_z/scalar; 00291 return ret; 00292 } 00293 00294 Quaternion operator * (float scalar) const 00295 { 00296 Quaternion ret; 00297 ret._w = this->_w*scalar; 00298 ret._x = this->_x*scalar; 00299 ret._y = this->_y*scalar; 00300 ret._z = this->_z*scalar; 00301 return ret; 00302 } 00303 00304 Quaternion scale(double scalar) const 00305 { 00306 Quaternion ret; 00307 ret._w = this->_w*scalar; 00308 ret._x = this->_x*scalar; 00309 ret._y = this->_y*scalar; 00310 ret._z = this->_z*scalar; 00311 return ret; 00312 } 00313 00314 private: 00315 double _w, _x, _y, _z; 00316 }; 00317 00318 00319 }; 00320 00321 #endif
Generated on Wed Jul 13 2022 00:45:44 by 1.7.2