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