Carter Sharer / MPU6050_Lab6_Part3

This one compatable with brobot V3, V4, Lab6 Part 3. first commit to BroBot

Dependencies:   I2Cdev

Dependents:   robotcode

Fork of MPU6050_V3 by Carter Sharer

helper_3dmath.h@0:662207e34fba, 2013-01-11 (annotated)

Committer:
garfieldsg
Date:
Fri Jan 11 00:54:28 2013 +0000
Revision:
0:662207e34fba
Child:
7:d5845b617139
first beta release

Who changed what in which revision?

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