Junichi Katsu / Mbed 2 deprecated BLE_MPU6050_test6_challenge_sb

テスト用です。

Dependencies:   mbed

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?

UserRevisionLine numberNew 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_ */