mpu9150
Fork of MPU9150 by
helper_3dmath.h@3:16a08969e192, 2017-06-21 (annotated)
- Committer:
- donghuoyinzi
- Date:
- Wed Jun 21 05:06:51 2017 +0000
- Revision:
- 3:16a08969e192
- Parent:
- 0:78ba160ba5f3
new fusion
Who changed what in which revision?
User | Revision | Line number | New 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_ */ |