jawad ateeq / Mbed 2 deprecated LowBack_Demo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

MPU6050/helper_3dmath.h@0:d2713e1a96b2, 2015-01-28 (annotated)

Committer:
jateeq
Date:
Wed Jan 28 17:29:19 2015 +0000
Revision:
0:d2713e1a96b2
Initial commit

Who changed what in which revision?

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