Mathieu Ducrocq / Mbed 2 deprecated DISCO-F746NG_Projet_Interfacage

USSRII Mathieu DUCROCQ

Dependencies:   mbed BSP_DISCO_F746NG

helper_3dmath.h@0:11a51c9ebb09, 2021-06-15 (annotated)

Committer:
mducrocq
Date:
Tue Jun 15 10:10:18 2021 +0000
Revision:
0:11a51c9ebb09
USRRII Mathieu Ducrocq

Who changed what in which revision?

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