Aloïs Wolff / HK10DOF

mbed implementation of the FreeIMU IMU for HobbyKing's 10DOF board

Dependents:   testHK10DOF

helper_3dmath.h@1:85fcfcb7b137, 2013-07-17 (annotated)

Committer:
pommzorz
Date:
Wed Jul 17 18:53:37 2013 +0000
Revision:
1:85fcfcb7b137
Parent:
0:9a1682a09c50 
oops forgot one file...

Who changed what in which revision?

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