Shundo Kishi / MPU9150

9Axis IMU MPU9150 's library. This project is ported from this Arduino's project https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU9150. Connect pinName 27 to SCL, PinName 28 to SDA, GND to GND, and VOUT to VCC to try this library. The example is here

Dependencies:   ArduinoSerial I2Cdev

Dependents:   MPU9150_Example

helper_3dmath.h@1:1a6f1948f43d, 2016-02-01 (annotated)

Committer:
syundo0730
Date:
Mon Feb 01 16:13:13 2016 +0000
Revision:
1:1a6f1948f43d
Parent:
0:78ba160ba5f3 
added LICENSE file

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 0:78ba160ba5f3 1 #ifndef _HELPER_3DMATH_H_
syundo0730 0:78ba160ba5f3 2 #define _HELPER_3DMATH_H_
syundo0730 0:78ba160ba5f3 3
syundo0730 0:78ba160ba5f3 4 class Quaternion {
syundo0730 0:78ba160ba5f3 5 public:
syundo0730 0:78ba160ba5f3 6 float w;
syundo0730 0:78ba160ba5f3 7 float x;
syundo0730 0:78ba160ba5f3 8 float y;
syundo0730 0:78ba160ba5f3 9 float z;
syundo0730 0:78ba160ba5f3 10
syundo0730 0:78ba160ba5f3 11 Quaternion() {
syundo0730 0:78ba160ba5f3 12 w = 1.0f;
syundo0730 0:78ba160ba5f3 13 x = 0.0f;
syundo0730 0:78ba160ba5f3 14 y = 0.0f;
syundo0730 0:78ba160ba5f3 15 z = 0.0f;
syundo0730 0:78ba160ba5f3 16 }
syundo0730 0:78ba160ba5f3 17
syundo0730 0:78ba160ba5f3 18 Quaternion(float nw, float nx, float ny, float nz) {
syundo0730 0:78ba160ba5f3 19 w = nw;
syundo0730 0:78ba160ba5f3 20 x = nx;
syundo0730 0:78ba160ba5f3 21 y = ny;
syundo0730 0:78ba160ba5f3 22 z = nz;
syundo0730 0:78ba160ba5f3 23 }
syundo0730 0:78ba160ba5f3 24
syundo0730 0:78ba160ba5f3 25 Quaternion getProduct(Quaternion q) {
syundo0730 0:78ba160ba5f3 26 // Quaternion multiplication is defined by:
syundo0730 0:78ba160ba5f3 27 // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
syundo0730 0:78ba160ba5f3 28 // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
syundo0730 0:78ba160ba5f3 29 // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
syundo0730 0:78ba160ba5f3 30 // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
syundo0730 0:78ba160ba5f3 31 return Quaternion(
syundo0730 0:78ba160ba5f3 32 w*q.w - x*q.x - y*q.y - z*q.z, // new w
syundo0730 0:78ba160ba5f3 33 w*q.x + x*q.w + y*q.z - z*q.y, // new x
syundo0730 0:78ba160ba5f3 34 w*q.y - x*q.z + y*q.w + z*q.x, // new y
syundo0730 0:78ba160ba5f3 35 w*q.z + x*q.y - y*q.x + z*q.w); // new z
syundo0730 0:78ba160ba5f3 36 }
syundo0730 0:78ba160ba5f3 37
syundo0730 0:78ba160ba5f3 38 Quaternion getConjugate() {
syundo0730 0:78ba160ba5f3 39 return Quaternion(w, -x, -y, -z);
syundo0730 0:78ba160ba5f3 40 }
syundo0730 0:78ba160ba5f3 41
syundo0730 0:78ba160ba5f3 42 float getMagnitude() {
syundo0730 0:78ba160ba5f3 43 return sqrt((float)(w*w + x*x + y*y + z*z));
syundo0730 0:78ba160ba5f3 44 }
syundo0730 0:78ba160ba5f3 45
syundo0730 0:78ba160ba5f3 46 void normalize() {
syundo0730 0:78ba160ba5f3 47 float m = getMagnitude();
syundo0730 0:78ba160ba5f3 48 w /= m;
syundo0730 0:78ba160ba5f3 49 x /= m;
syundo0730 0:78ba160ba5f3 50 y /= m;
syundo0730 0:78ba160ba5f3 51 z /= m;
syundo0730 0:78ba160ba5f3 52 }
syundo0730 0:78ba160ba5f3 53
syundo0730 0:78ba160ba5f3 54 Quaternion getNormalized() {
syundo0730 0:78ba160ba5f3 55 Quaternion r(w, x, y, z);
syundo0730 0:78ba160ba5f3 56 r.normalize();
syundo0730 0:78ba160ba5f3 57 return r;
syundo0730 0:78ba160ba5f3 58 }
syundo0730 0:78ba160ba5f3 59 };
syundo0730 0:78ba160ba5f3 60
syundo0730 0:78ba160ba5f3 61 class VectorInt16 {
syundo0730 0:78ba160ba5f3 62 public:
syundo0730 0:78ba160ba5f3 63 int16_t x;
syundo0730 0:78ba160ba5f3 64 int16_t y;
syundo0730 0:78ba160ba5f3 65 int16_t z;
syundo0730 0:78ba160ba5f3 66
syundo0730 0:78ba160ba5f3 67 VectorInt16() {
syundo0730 0:78ba160ba5f3 68 x = 0;
syundo0730 0:78ba160ba5f3 69 y = 0;
syundo0730 0:78ba160ba5f3 70 z = 0;
syundo0730 0:78ba160ba5f3 71 }
syundo0730 0:78ba160ba5f3 72
syundo0730 0:78ba160ba5f3 73 VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
syundo0730 0:78ba160ba5f3 74 x = nx;
syundo0730 0:78ba160ba5f3 75 y = ny;
syundo0730 0:78ba160ba5f3 76 z = nz;
syundo0730 0:78ba160ba5f3 77 }
syundo0730 0:78ba160ba5f3 78
syundo0730 0:78ba160ba5f3 79 float getMagnitude() {
syundo0730 0:78ba160ba5f3 80 return sqrt((float)(x*x + y*y + z*z));
syundo0730 0:78ba160ba5f3 81 }
syundo0730 0:78ba160ba5f3 82
syundo0730 0:78ba160ba5f3 83 void normalize() {
syundo0730 0:78ba160ba5f3 84 float m = getMagnitude();
syundo0730 0:78ba160ba5f3 85 x /= m;
syundo0730 0:78ba160ba5f3 86 y /= m;
syundo0730 0:78ba160ba5f3 87 z /= m;
syundo0730 0:78ba160ba5f3 88 }
syundo0730 0:78ba160ba5f3 89
syundo0730 0:78ba160ba5f3 90 VectorInt16 getNormalized() {
syundo0730 0:78ba160ba5f3 91 VectorInt16 r(x, y, z);
syundo0730 0:78ba160ba5f3 92 r.normalize();
syundo0730 0:78ba160ba5f3 93 return r;
syundo0730 0:78ba160ba5f3 94 }
syundo0730 0:78ba160ba5f3 95
syundo0730 0:78ba160ba5f3 96 void rotate(Quaternion *q) {
syundo0730 0:78ba160ba5f3 97 // http://www.cprogramming.com/tutorial/3d/quaternions.html
syundo0730 0:78ba160ba5f3 98 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
syundo0730 0:78ba160ba5f3 99 // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
syundo0730 0:78ba160ba5f3 100 // ^ 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
syundo0730 0:78ba160ba5f3 101
syundo0730 0:78ba160ba5f3 102 // P_out = q * P_in * conj(q)
syundo0730 0:78ba160ba5f3 103 // - P_out is the output vector
syundo0730 0:78ba160ba5f3 104 // - q is the orientation quaternion
syundo0730 0:78ba160ba5f3 105 // - P_in is the input vector (a*aReal)
syundo0730 0:78ba160ba5f3 106 // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
syundo0730 0:78ba160ba5f3 107 Quaternion p(0, x, y, z);
syundo0730 0:78ba160ba5f3 108
syundo0730 0:78ba160ba5f3 109 // quaternion multiplication: q * p, stored back in p
syundo0730 0:78ba160ba5f3 110 p = q -> getProduct(p);
syundo0730 0:78ba160ba5f3 111
syundo0730 0:78ba160ba5f3 112 // quaternion multiplication: p * conj(q), stored back in p
syundo0730 0:78ba160ba5f3 113 p = p.getProduct(q -> getConjugate());
syundo0730 0:78ba160ba5f3 114
syundo0730 0:78ba160ba5f3 115 // p quaternion is now [0, x', y', z']
syundo0730 0:78ba160ba5f3 116 x = p.x;
syundo0730 0:78ba160ba5f3 117 y = p.y;
syundo0730 0:78ba160ba5f3 118 z = p.z;
syundo0730 0:78ba160ba5f3 119 }
syundo0730 0:78ba160ba5f3 120
syundo0730 0:78ba160ba5f3 121 VectorInt16 getRotated(Quaternion *q) {
syundo0730 0:78ba160ba5f3 122 VectorInt16 r(x, y, z);
syundo0730 0:78ba160ba5f3 123 r.rotate(q);
syundo0730 0:78ba160ba5f3 124 return r;
syundo0730 0:78ba160ba5f3 125 }
syundo0730 0:78ba160ba5f3 126 };
syundo0730 0:78ba160ba5f3 127
syundo0730 0:78ba160ba5f3 128 class VectorFloat {
syundo0730 0:78ba160ba5f3 129 public:
syundo0730 0:78ba160ba5f3 130 float x;
syundo0730 0:78ba160ba5f3 131 float y;
syundo0730 0:78ba160ba5f3 132 float z;
syundo0730 0:78ba160ba5f3 133
syundo0730 0:78ba160ba5f3 134 VectorFloat() {
syundo0730 0:78ba160ba5f3 135 x = 0;
syundo0730 0:78ba160ba5f3 136 y = 0;
syundo0730 0:78ba160ba5f3 137 z = 0;
syundo0730 0:78ba160ba5f3 138 }
syundo0730 0:78ba160ba5f3 139
syundo0730 0:78ba160ba5f3 140 VectorFloat(float nx, float ny, float nz) {
syundo0730 0:78ba160ba5f3 141 x = nx;
syundo0730 0:78ba160ba5f3 142 y = ny;
syundo0730 0:78ba160ba5f3 143 z = nz;
syundo0730 0:78ba160ba5f3 144 }
syundo0730 0:78ba160ba5f3 145
syundo0730 0:78ba160ba5f3 146 float getMagnitude() {
syundo0730 0:78ba160ba5f3 147 return sqrt((float)(x*x + y*y + z*z));
syundo0730 0:78ba160ba5f3 148 }
syundo0730 0:78ba160ba5f3 149
syundo0730 0:78ba160ba5f3 150 void normalize() {
syundo0730 0:78ba160ba5f3 151 float m = getMagnitude();
syundo0730 0:78ba160ba5f3 152 x /= m;
syundo0730 0:78ba160ba5f3 153 y /= m;
syundo0730 0:78ba160ba5f3 154 z /= m;
syundo0730 0:78ba160ba5f3 155 }
syundo0730 0:78ba160ba5f3 156
syundo0730 0:78ba160ba5f3 157 VectorFloat getNormalized() {
syundo0730 0:78ba160ba5f3 158 VectorFloat r(x, y, z);
syundo0730 0:78ba160ba5f3 159 r.normalize();
syundo0730 0:78ba160ba5f3 160 return r;
syundo0730 0:78ba160ba5f3 161 }
syundo0730 0:78ba160ba5f3 162
syundo0730 0:78ba160ba5f3 163 void rotate(Quaternion *q) {
syundo0730 0:78ba160ba5f3 164 Quaternion p(0, x, y, z);
syundo0730 0:78ba160ba5f3 165
syundo0730 0:78ba160ba5f3 166 // quaternion multiplication: q * p, stored back in p
syundo0730 0:78ba160ba5f3 167 p = q -> getProduct(p);
syundo0730 0:78ba160ba5f3 168
syundo0730 0:78ba160ba5f3 169 // quaternion multiplication: p * conj(q), stored back in p
syundo0730 0:78ba160ba5f3 170 p = p.getProduct(q -> getConjugate());
syundo0730 0:78ba160ba5f3 171
syundo0730 0:78ba160ba5f3 172 // p quaternion is now [0, x', y', z']
syundo0730 0:78ba160ba5f3 173 x = p.x;
syundo0730 0:78ba160ba5f3 174 y = p.y;
syundo0730 0:78ba160ba5f3 175 z = p.z;
syundo0730 0:78ba160ba5f3 176 }
syundo0730 0:78ba160ba5f3 177
syundo0730 0:78ba160ba5f3 178 VectorFloat getRotated(Quaternion *q) {
syundo0730 0:78ba160ba5f3 179 VectorFloat r(x, y, z);
syundo0730 0:78ba160ba5f3 180 r.rotate(q);
syundo0730 0:78ba160ba5f3 181 return r;
syundo0730 0:78ba160ba5f3 182 }
syundo0730 0:78ba160ba5f3 183 };
syundo0730 0:78ba160ba5f3 184
syundo0730 0:78ba160ba5f3 185 #endif /* _HELPER_3DMATH_H_ */