Adafruit driver converted to Mbed OS 6.x.

Dependents:   Adafruit-BNO055-test

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers quaternion.h Source File

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