Johannes Stratmann / MPU9150

Lib for MPU9150/9250 IMU sensors

Dependents:   ezSBC_MPU9250

helper_3dmath.h@0:ab00d9bcd00d, 2017-04-24 (annotated)

Committer:
JojoS
Date:
Mon Apr 24 16:21:35 2017 +0000
Revision:
0:ab00d9bcd00d
modified for mbed

Who changed what in which revision?

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