1
Dependencies: ArduinoSerial I2Cdev
Fork of MPU6050 by
helper_3dmath.h@7:d5845b617139, 2016-01-31 (annotated)
- Committer:
- syundo0730
- Date:
- Sun Jan 31 09:12:19 2016 +0000
- Revision:
- 7:d5845b617139
- Parent:
- 0:662207e34fba
trace changes in arduino project
Who changed what in which revision?
User | Revision | Line number | New 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_ */ |