anthony peltriaux / Mbed 2 deprecated DISCO-F746NG_blink_led

Projet interfaçage

Dependencies:   mbed BSP_DISCO_F746NG

helper_3dmath.h@1:eb044e6a9033, 2021-06-22 (annotated)

Committer:
anthonyp08
Date:
Tue Jun 22 12:04:16 2021 +0000
Revision:
1:eb044e6a9033
Parent:
0:1c6757f4b61c 
projet

Who changed what in which revision?

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