p kj / Mbed 2 deprecated LPC824-BalanceCar

Microduino

Dependencies:   mbed

Fork of BalanceCar by Li Weiyi

helper_3dmath.h@0:a4d8f5b3c546, 2016-06-04 (annotated)

Committer:
lixianyu
Date:
Sat Jun 04 03:16:52 2016 +0000
Revision:
0:a4d8f5b3c546
Child:
1:620da20b810b
Pass compile!!

Who changed what in which revision?

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