Carter Sharer / MPU6050_V3

This one compatable with brobot V3. first commit to BroBot

Dependencies:   I2Cdev

Dependents:   BroBot_ESE350_Skeleton

Fork of MPU6050 by Carter Sharer

helper_3dmath.h@13:241bea255ef7, 2016-12-17 (annotated)

Committer:
csharer
Date:
Sat Dec 17 23:25:55 2016 +0000
Revision:
13:241bea255ef7
Parent:
7:d5845b617139 
Working Stability and Position Controller

Who changed what in which revision?

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