EdgeBotix / Mbed 2 deprecated eBot_Firmware_V1

Initial Commit

Dependencies:   mbed HC05 QEI MODSERIAL SWSPI mbed-rtos

Drivers/MPU6050/helper_3dmath.h@6:5ab1735265a9, 2014-10-21 (annotated)

Committer:
harshb
Date:
Tue Oct 21 21:59:15 2014 +0000
Revision:
6:5ab1735265a9
Parent:
1:1da89c13dfa1 
Initial Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Throwbot 1:1da89c13dfa1 1 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
Throwbot 1:1da89c13dfa1 2 // 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
Throwbot 1:1da89c13dfa1 3 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
Throwbot 1:1da89c13dfa1 4 //
Throwbot 1:1da89c13dfa1 5 // Changelog:
Throwbot 1:1da89c13dfa1 6 // 2012-06-05 - add 3D math helper file to DMP6 example sketch
Throwbot 1:1da89c13dfa1 7
Throwbot 1:1da89c13dfa1 8 /* ============================================
Throwbot 1:1da89c13dfa1 9 I2Cdev device library code is placed under the MIT license
Throwbot 1:1da89c13dfa1 10 Copyright (c) 2012 Jeff Rowberg
Throwbot 1:1da89c13dfa1 11
Throwbot 1:1da89c13dfa1 12 Permission is hereby granted, free of charge, to any person obtaining a copy
Throwbot 1:1da89c13dfa1 13 of this software and associated documentation files (the "Software"), to deal
Throwbot 1:1da89c13dfa1 14 in the Software without restriction, including without limitation the rights
Throwbot 1:1da89c13dfa1 15 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
Throwbot 1:1da89c13dfa1 16 copies of the Software, and to permit persons to whom the Software is
Throwbot 1:1da89c13dfa1 17 furnished to do so, subject to the following conditions:
Throwbot 1:1da89c13dfa1 18
Throwbot 1:1da89c13dfa1 19 The above copyright notice and this permission notice shall be included in
Throwbot 1:1da89c13dfa1 20 all copies or substantial portions of the Software.
Throwbot 1:1da89c13dfa1 21
Throwbot 1:1da89c13dfa1 22 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Throwbot 1:1da89c13dfa1 23 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Throwbot 1:1da89c13dfa1 24 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
Throwbot 1:1da89c13dfa1 25 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Throwbot 1:1da89c13dfa1 26 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
Throwbot 1:1da89c13dfa1 27 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
Throwbot 1:1da89c13dfa1 28 THE SOFTWARE.
Throwbot 1:1da89c13dfa1 29 ===============================================
Throwbot 1:1da89c13dfa1 30 */
Throwbot 1:1da89c13dfa1 31
Throwbot 1:1da89c13dfa1 32 #ifndef _HELPER_3DMATH_H_
Throwbot 1:1da89c13dfa1 33 #define _HELPER_3DMATH_H_
Throwbot 1:1da89c13dfa1 34
Throwbot 1:1da89c13dfa1 35 class Quaternion {
Throwbot 1:1da89c13dfa1 36 public:
Throwbot 1:1da89c13dfa1 37 float w;
Throwbot 1:1da89c13dfa1 38 float x;
Throwbot 1:1da89c13dfa1 39 float y;
Throwbot 1:1da89c13dfa1 40 float z;
Throwbot 1:1da89c13dfa1 41
Throwbot 1:1da89c13dfa1 42 Quaternion() {
Throwbot 1:1da89c13dfa1 43 w = 1.0f;
Throwbot 1:1da89c13dfa1 44 x = 0.0f;
Throwbot 1:1da89c13dfa1 45 y = 0.0f;
Throwbot 1:1da89c13dfa1 46 z = 0.0f;
Throwbot 1:1da89c13dfa1 47 }
Throwbot 1:1da89c13dfa1 48
Throwbot 1:1da89c13dfa1 49 Quaternion(float nw, float nx, float ny, float nz) {
Throwbot 1:1da89c13dfa1 50 w = nw;
Throwbot 1:1da89c13dfa1 51 x = nx;
Throwbot 1:1da89c13dfa1 52 y = ny;
Throwbot 1:1da89c13dfa1 53 z = nz;
Throwbot 1:1da89c13dfa1 54 }
Throwbot 1:1da89c13dfa1 55
Throwbot 1:1da89c13dfa1 56 Quaternion getProduct(Quaternion q) {
Throwbot 1:1da89c13dfa1 57 // Quaternion multiplication is defined by:
Throwbot 1:1da89c13dfa1 58 // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
Throwbot 1:1da89c13dfa1 59 // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
Throwbot 1:1da89c13dfa1 60 // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
Throwbot 1:1da89c13dfa1 61 // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
Throwbot 1:1da89c13dfa1 62 return Quaternion(
Throwbot 1:1da89c13dfa1 63 w*q.w - x*q.x - y*q.y - z*q.z, // new w
Throwbot 1:1da89c13dfa1 64 w*q.x + x*q.w + y*q.z - z*q.y, // new x
Throwbot 1:1da89c13dfa1 65 w*q.y - x*q.z + y*q.w + z*q.x, // new y
Throwbot 1:1da89c13dfa1 66 w*q.z + x*q.y - y*q.x + z*q.w); // new z
Throwbot 1:1da89c13dfa1 67 }
Throwbot 1:1da89c13dfa1 68
Throwbot 1:1da89c13dfa1 69 Quaternion getConjugate() {
Throwbot 1:1da89c13dfa1 70 return Quaternion(w, -x, -y, -z);
Throwbot 1:1da89c13dfa1 71 }
Throwbot 1:1da89c13dfa1 72
Throwbot 1:1da89c13dfa1 73 float getMagnitude() {
Throwbot 1:1da89c13dfa1 74 return sqrt(w*w + x*x + y*y + z*z);
Throwbot 1:1da89c13dfa1 75 }
Throwbot 1:1da89c13dfa1 76
Throwbot 1:1da89c13dfa1 77 void normalize() {
Throwbot 1:1da89c13dfa1 78 float m = getMagnitude();
Throwbot 1:1da89c13dfa1 79 w /= m;
Throwbot 1:1da89c13dfa1 80 x /= m;
Throwbot 1:1da89c13dfa1 81 y /= m;
Throwbot 1:1da89c13dfa1 82 z /= m;
Throwbot 1:1da89c13dfa1 83 }
Throwbot 1:1da89c13dfa1 84
Throwbot 1:1da89c13dfa1 85 Quaternion getNormalized() {
Throwbot 1:1da89c13dfa1 86 Quaternion r(w, x, y, z);
Throwbot 1:1da89c13dfa1 87 r.normalize();
Throwbot 1:1da89c13dfa1 88 return r;
Throwbot 1:1da89c13dfa1 89 }
Throwbot 1:1da89c13dfa1 90 };
Throwbot 1:1da89c13dfa1 91
Throwbot 1:1da89c13dfa1 92 class VectorInt16 {
Throwbot 1:1da89c13dfa1 93 public:
Throwbot 1:1da89c13dfa1 94 int16_t x;
Throwbot 1:1da89c13dfa1 95 int16_t y;
Throwbot 1:1da89c13dfa1 96 int16_t z;
Throwbot 1:1da89c13dfa1 97
Throwbot 1:1da89c13dfa1 98 VectorInt16() {
Throwbot 1:1da89c13dfa1 99 x = 0;
Throwbot 1:1da89c13dfa1 100 y = 0;
Throwbot 1:1da89c13dfa1 101 z = 0;
Throwbot 1:1da89c13dfa1 102 }
Throwbot 1:1da89c13dfa1 103
Throwbot 1:1da89c13dfa1 104 VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
Throwbot 1:1da89c13dfa1 105 x = nx;
Throwbot 1:1da89c13dfa1 106 y = ny;
Throwbot 1:1da89c13dfa1 107 z = nz;
Throwbot 1:1da89c13dfa1 108 }
Throwbot 1:1da89c13dfa1 109
Throwbot 1:1da89c13dfa1 110 float getMagnitude() {
Throwbot 1:1da89c13dfa1 111 return sqrt((float)(x*x + y*y + z*z));
Throwbot 1:1da89c13dfa1 112 }
Throwbot 1:1da89c13dfa1 113
Throwbot 1:1da89c13dfa1 114 void normalize() {
Throwbot 1:1da89c13dfa1 115 float m = getMagnitude();
Throwbot 1:1da89c13dfa1 116 x /= m;
Throwbot 1:1da89c13dfa1 117 y /= m;
Throwbot 1:1da89c13dfa1 118 z /= m;
Throwbot 1:1da89c13dfa1 119 }
Throwbot 1:1da89c13dfa1 120
Throwbot 1:1da89c13dfa1 121 VectorInt16 getNormalized() {
Throwbot 1:1da89c13dfa1 122 VectorInt16 r(x, y, z);
Throwbot 1:1da89c13dfa1 123 r.normalize();
Throwbot 1:1da89c13dfa1 124 return r;
Throwbot 1:1da89c13dfa1 125 }
Throwbot 1:1da89c13dfa1 126
Throwbot 1:1da89c13dfa1 127 void rotate(Quaternion *q) {
Throwbot 1:1da89c13dfa1 128 // http://www.cprogramming.com/tutorial/3d/quaternions.html
Throwbot 1:1da89c13dfa1 129 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
Throwbot 1:1da89c13dfa1 130 // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
Throwbot 1:1da89c13dfa1 131 // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
Throwbot 1:1da89c13dfa1 132
Throwbot 1:1da89c13dfa1 133 // P_out = q * P_in * conj(q)
Throwbot 1:1da89c13dfa1 134 // - P_out is the output vector
Throwbot 1:1da89c13dfa1 135 // - q is the orientation quaternion
Throwbot 1:1da89c13dfa1 136 // - P_in is the input vector (a*aReal)
Throwbot 1:1da89c13dfa1 137 // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
Throwbot 1:1da89c13dfa1 138 Quaternion p(0, x, y, z);
Throwbot 1:1da89c13dfa1 139
Throwbot 1:1da89c13dfa1 140 // quaternion multiplication: q * p, stored back in p
Throwbot 1:1da89c13dfa1 141 p = q -> getProduct(p);
Throwbot 1:1da89c13dfa1 142
Throwbot 1:1da89c13dfa1 143 // quaternion multiplication: p * conj(q), stored back in p
Throwbot 1:1da89c13dfa1 144 p = p.getProduct(q -> getConjugate());
Throwbot 1:1da89c13dfa1 145
Throwbot 1:1da89c13dfa1 146 // p quaternion is now [0, x', y', z']
Throwbot 1:1da89c13dfa1 147 x = p.x;
Throwbot 1:1da89c13dfa1 148 y = p.y;
Throwbot 1:1da89c13dfa1 149 z = p.z;
Throwbot 1:1da89c13dfa1 150 }
Throwbot 1:1da89c13dfa1 151
Throwbot 1:1da89c13dfa1 152 VectorInt16 getRotated(Quaternion *q) {
Throwbot 1:1da89c13dfa1 153 VectorInt16 r(x, y, z);
Throwbot 1:1da89c13dfa1 154 r.rotate(q);
Throwbot 1:1da89c13dfa1 155 return r;
Throwbot 1:1da89c13dfa1 156 }
Throwbot 1:1da89c13dfa1 157 };
Throwbot 1:1da89c13dfa1 158
Throwbot 1:1da89c13dfa1 159 class VectorFloat {
Throwbot 1:1da89c13dfa1 160 public:
Throwbot 1:1da89c13dfa1 161 float x;
Throwbot 1:1da89c13dfa1 162 float y;
Throwbot 1:1da89c13dfa1 163 float z;
Throwbot 1:1da89c13dfa1 164
Throwbot 1:1da89c13dfa1 165 VectorFloat() {
Throwbot 1:1da89c13dfa1 166 x = 0;
Throwbot 1:1da89c13dfa1 167 y = 0;
Throwbot 1:1da89c13dfa1 168 z = 0;
Throwbot 1:1da89c13dfa1 169 }
Throwbot 1:1da89c13dfa1 170
Throwbot 1:1da89c13dfa1 171 VectorFloat(float nx, float ny, float nz) {
Throwbot 1:1da89c13dfa1 172 x = nx;
Throwbot 1:1da89c13dfa1 173 y = ny;
Throwbot 1:1da89c13dfa1 174 z = nz;
Throwbot 1:1da89c13dfa1 175 }
Throwbot 1:1da89c13dfa1 176
Throwbot 1:1da89c13dfa1 177 float getMagnitude() {
Throwbot 1:1da89c13dfa1 178 return sqrt(x*x + y*y + z*z);
Throwbot 1:1da89c13dfa1 179 }
Throwbot 1:1da89c13dfa1 180
Throwbot 1:1da89c13dfa1 181 void normalize() {
Throwbot 1:1da89c13dfa1 182 float m = getMagnitude();
Throwbot 1:1da89c13dfa1 183 x /= m;
Throwbot 1:1da89c13dfa1 184 y /= m;
Throwbot 1:1da89c13dfa1 185 z /= m;
Throwbot 1:1da89c13dfa1 186 }
Throwbot 1:1da89c13dfa1 187
Throwbot 1:1da89c13dfa1 188 VectorFloat getNormalized() {
Throwbot 1:1da89c13dfa1 189 VectorFloat r(x, y, z);
Throwbot 1:1da89c13dfa1 190 r.normalize();
Throwbot 1:1da89c13dfa1 191 return r;
Throwbot 1:1da89c13dfa1 192 }
Throwbot 1:1da89c13dfa1 193
Throwbot 1:1da89c13dfa1 194 void rotate(Quaternion *q) {
Throwbot 1:1da89c13dfa1 195 Quaternion p(0, x, y, z);
Throwbot 1:1da89c13dfa1 196
Throwbot 1:1da89c13dfa1 197 // quaternion multiplication: q * p, stored back in p
Throwbot 1:1da89c13dfa1 198 p = q -> getProduct(p);
Throwbot 1:1da89c13dfa1 199
Throwbot 1:1da89c13dfa1 200 // quaternion multiplication: p * conj(q), stored back in p
Throwbot 1:1da89c13dfa1 201 p = p.getProduct(q -> getConjugate());
Throwbot 1:1da89c13dfa1 202
Throwbot 1:1da89c13dfa1 203 // p quaternion is now [0, x', y', z']
Throwbot 1:1da89c13dfa1 204 x = p.x;
Throwbot 1:1da89c13dfa1 205 y = p.y;
Throwbot 1:1da89c13dfa1 206 z = p.z;
Throwbot 1:1da89c13dfa1 207 }
Throwbot 1:1da89c13dfa1 208
Throwbot 1:1da89c13dfa1 209 VectorFloat getRotated(Quaternion *q) {
Throwbot 1:1da89c13dfa1 210 VectorFloat r(x, y, z);
Throwbot 1:1da89c13dfa1 211 r.rotate(q);
Throwbot 1:1da89c13dfa1 212 return r;
Throwbot 1:1da89c13dfa1 213 }
Throwbot 1:1da89c13dfa1 214 };
Throwbot 1:1da89c13dfa1 215
Throwbot 1:1da89c13dfa1 216 #endif /* _HELPER_3DMATH_H_ */