MPUQuaternion.cpp
Go to the documentation of this file.
00001 
00002 //
00003 //  This file is part of MPU9150Lib
00004 //
00005 //  Copyright (c) 2013 Pansenti, LLC
00006 //
00007 //  Permission is hereby granted, free of charge, to any person obtaining a copy of 
00008 //  this software and associated documentation files (the "Software"), to deal in 
00009 //  the Software without restriction, including without limitation the rights to use, 
00010 //  copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 
00011 //  Software, and to permit persons to whom the Software is furnished to do so, 
00012 //  subject to the following conditions:
00013 //
00014 //  The above copyright notice and this permission notice shall be included in all 
00015 //  copies or substantial portions of the Software.
00016 //
00017 //  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
00018 //  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
00019 //  PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 
00020 //  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 
00021 //  OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 
00022 //  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00023 
00024 #include "MPUQuaternion.h"
00025 
00026 void MPUQuaternionNormalize(MPUQuaternion q)
00027 {
00028   float length = MPUQuaternionNorm(q);
00029   
00030   if (length == 0)
00031     return;
00032   q[QUAT_W] /= length;
00033   q[QUAT_X] /= length;
00034   q[QUAT_Y] /= length;
00035   q[QUAT_Z] /= length;
00036 }
00037 
00038 void MPUQuaternionQuaternionToEuler(const MPUQuaternion q, MPUVector3 v)
00039 {
00040   float pole = M_PI/2.0f - 0.05f;                           // fix roll near poles with this tolerance
00041 
00042   v[VEC3_Y] = asin(2.0f * (q[QUAT_W] * q[QUAT_Y] - q[QUAT_X] * q[QUAT_Z]));
00043 
00044   if ((v[VEC3_Y] < pole) && (v[VEC3_Y] > -pole))
00045           v[VEC3_X] = atan2(2.0f * (q[QUAT_Y] * q[QUAT_Z] + q[QUAT_W] * q[QUAT_X]),
00046                     1.0f - 2.0f * (q[QUAT_X] * q[QUAT_X] + q[QUAT_Y] * q[QUAT_Y]));
00047         
00048   v[VEC3_Z] = atan2(2.0f * (q[QUAT_X] * q[QUAT_Y] + q[QUAT_W] * q[QUAT_Z]),
00049                     1.0f - 2.0f * (q[QUAT_Y] * q[QUAT_Y] + q[QUAT_Z] * q[QUAT_Z]));
00050 }
00051 
00052 void MPUQuaternionEulerToQuaternion(const MPUVector3 v, MPUQuaternion q)
00053 {
00054   float cosX2 = cos(v[VEC3_X] / 2.0f);
00055   float sinX2 = sin(v[VEC3_X] / 2.0f);
00056   float cosY2 = cos(v[VEC3_Y] / 2.0f);
00057   float sinY2 = sin(v[VEC3_Y] / 2.0f);
00058   float cosZ2 = cos(v[VEC3_Z] / 2.0f);
00059   float sinZ2 = sin(v[VEC3_Z] / 2.0f);
00060 
00061   q[QUAT_W] = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
00062   q[QUAT_X] = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
00063   q[QUAT_Y] = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
00064   q[QUAT_Z] = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
00065   MPUQuaternionNormalize(q);
00066 }
00067 
00068 void MPUQuaternionConjugate(const MPUQuaternion s, MPUQuaternion d) 
00069 {
00070   d[QUAT_W] = s[QUAT_W];
00071   d[QUAT_X] = -s[QUAT_X];
00072   d[QUAT_Y] = -s[QUAT_Y];
00073   d[QUAT_Z] = -s[QUAT_Z];
00074 }
00075         
00076 void MPUQuaternionMultiply(const MPUQuaternion qa, const MPUQuaternion qb, MPUQuaternion qd) 
00077 {
00078   MPUVector3 va;
00079   MPUVector3 vb;
00080   float dotAB;
00081   MPUVector3 crossAB;
00082                 
00083   va[VEC3_X] = qa[QUAT_X];
00084   va[VEC3_Y] = qa[QUAT_Y];
00085   va[VEC3_Z] = qa[QUAT_Z];
00086         
00087   vb[VEC3_X] = qb[QUAT_X];
00088   vb[VEC3_Y] = qb[QUAT_Y];
00089   vb[VEC3_Z] = qb[QUAT_Z];
00090         
00091   MPUVector3DotProduct(va, vb, &dotAB);
00092   MPUVector3CrossProduct(va, vb, crossAB);
00093                 
00094   qd[QUAT_W] = qa[QUAT_W] * qb[QUAT_W] - dotAB;
00095   qd[QUAT_X] = qa[QUAT_W] * vb[VEC3_X] + qb[QUAT_W] * va[VEC3_X] + crossAB[VEC3_X];
00096   qd[QUAT_Y] = qa[QUAT_W] * vb[VEC3_Y] + qb[QUAT_W] * va[VEC3_Y] + crossAB[VEC3_Y];
00097   qd[QUAT_Z] = qa[QUAT_W] * vb[VEC3_Z] + qb[QUAT_W] * va[VEC3_Z] + crossAB[VEC3_Z];
00098 }


lizi_arduino
Author(s): RoboTiCan
autogenerated on Wed Aug 26 2015 12:24:22