航空研究会 / MPU6050_2

aaa

Dependencies:   ArduinoSerial I2Cdev2

Dependents:   AutoFlight2017_now2 AutoFlight2018_Control sbus_test_2018 Autoflight2018_sbusread ... more

helper_3dmath.h@2:ff025b9c6c7c, 2018-09-18 (annotated)

Committer:
taknokolat
Date:
Tue Sep 18 05:53:06 2018 +0000
Revision:
2:ff025b9c6c7c
Parent:
0:c3af3416e383 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TUATBM 0:c3af3416e383 1 #ifndef _HELPER_3DMATH_H_
TUATBM 0:c3af3416e383 2 #define _HELPER_3DMATH_H_
TUATBM 0:c3af3416e383 3
TUATBM 0:c3af3416e383 4 class Quaternion {
TUATBM 0:c3af3416e383 5 public:
TUATBM 0:c3af3416e383 6 float w;
TUATBM 0:c3af3416e383 7 float x;
TUATBM 0:c3af3416e383 8 float y;
TUATBM 0:c3af3416e383 9 float z;
TUATBM 0:c3af3416e383 10
TUATBM 0:c3af3416e383 11 Quaternion() {
TUATBM 0:c3af3416e383 12 w = 1.0f;
TUATBM 0:c3af3416e383 13 x = 0.0f;
TUATBM 0:c3af3416e383 14 y = 0.0f;
TUATBM 0:c3af3416e383 15 z = 0.0f;
TUATBM 0:c3af3416e383 16 }
TUATBM 0:c3af3416e383 17
TUATBM 0:c3af3416e383 18 Quaternion(float nw, float nx, float ny, float nz) {
TUATBM 0:c3af3416e383 19 w = nw;
TUATBM 0:c3af3416e383 20 x = nx;
TUATBM 0:c3af3416e383 21 y = ny;
TUATBM 0:c3af3416e383 22 z = nz;
TUATBM 0:c3af3416e383 23 }
TUATBM 0:c3af3416e383 24
TUATBM 0:c3af3416e383 25 Quaternion getProduct(Quaternion q) {
TUATBM 0:c3af3416e383 26 // Quaternion multiplication is defined by:
TUATBM 0:c3af3416e383 27 // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
TUATBM 0:c3af3416e383 28 // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
TUATBM 0:c3af3416e383 29 // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
TUATBM 0:c3af3416e383 30 // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
TUATBM 0:c3af3416e383 31 return Quaternion(
TUATBM 0:c3af3416e383 32 w*q.w - x*q.x - y*q.y - z*q.z, // new w
TUATBM 0:c3af3416e383 33 w*q.x + x*q.w + y*q.z - z*q.y, // new x
TUATBM 0:c3af3416e383 34 w*q.y - x*q.z + y*q.w + z*q.x, // new y
TUATBM 0:c3af3416e383 35 w*q.z + x*q.y - y*q.x + z*q.w); // new z
TUATBM 0:c3af3416e383 36 }
TUATBM 0:c3af3416e383 37
TUATBM 0:c3af3416e383 38 Quaternion getConjugate() {
TUATBM 0:c3af3416e383 39 return Quaternion(w, -x, -y, -z);
TUATBM 0:c3af3416e383 40 }
TUATBM 0:c3af3416e383 41
TUATBM 0:c3af3416e383 42 float getMagnitude() {
TUATBM 0:c3af3416e383 43 return sqrt((float)(w*w + x*x + y*y + z*z));
TUATBM 0:c3af3416e383 44 }
TUATBM 0:c3af3416e383 45
TUATBM 0:c3af3416e383 46 void normalize() {
TUATBM 0:c3af3416e383 47 float m = getMagnitude();
TUATBM 0:c3af3416e383 48 w /= m;
TUATBM 0:c3af3416e383 49 x /= m;
TUATBM 0:c3af3416e383 50 y /= m;
TUATBM 0:c3af3416e383 51 z /= m;
TUATBM 0:c3af3416e383 52 }
TUATBM 0:c3af3416e383 53
TUATBM 0:c3af3416e383 54 Quaternion getNormalized() {
TUATBM 0:c3af3416e383 55 Quaternion r(w, x, y, z);
TUATBM 0:c3af3416e383 56 r.normalize();
TUATBM 0:c3af3416e383 57 return r;
TUATBM 0:c3af3416e383 58 }
TUATBM 0:c3af3416e383 59 };
TUATBM 0:c3af3416e383 60
TUATBM 0:c3af3416e383 61 class VectorInt16 {
TUATBM 0:c3af3416e383 62 public:
TUATBM 0:c3af3416e383 63 int16_t x;
TUATBM 0:c3af3416e383 64 int16_t y;
TUATBM 0:c3af3416e383 65 int16_t z;
TUATBM 0:c3af3416e383 66
TUATBM 0:c3af3416e383 67 VectorInt16() {
TUATBM 0:c3af3416e383 68 x = 0;
TUATBM 0:c3af3416e383 69 y = 0;
TUATBM 0:c3af3416e383 70 z = 0;
TUATBM 0:c3af3416e383 71 }
TUATBM 0:c3af3416e383 72
TUATBM 0:c3af3416e383 73 VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
TUATBM 0:c3af3416e383 74 x = nx;
TUATBM 0:c3af3416e383 75 y = ny;
TUATBM 0:c3af3416e383 76 z = nz;
TUATBM 0:c3af3416e383 77 }
TUATBM 0:c3af3416e383 78
TUATBM 0:c3af3416e383 79 float getMagnitude() {
TUATBM 0:c3af3416e383 80 return sqrt((float)(x*x + y*y + z*z));
TUATBM 0:c3af3416e383 81 }
TUATBM 0:c3af3416e383 82
TUATBM 0:c3af3416e383 83 void normalize() {
TUATBM 0:c3af3416e383 84 float m = getMagnitude();
TUATBM 0:c3af3416e383 85 x /= m;
TUATBM 0:c3af3416e383 86 y /= m;
TUATBM 0:c3af3416e383 87 z /= m;
TUATBM 0:c3af3416e383 88 }
TUATBM 0:c3af3416e383 89
TUATBM 0:c3af3416e383 90 VectorInt16 getNormalized() {
TUATBM 0:c3af3416e383 91 VectorInt16 r(x, y, z);
TUATBM 0:c3af3416e383 92 r.normalize();
TUATBM 0:c3af3416e383 93 return r;
TUATBM 0:c3af3416e383 94 }
TUATBM 0:c3af3416e383 95
TUATBM 0:c3af3416e383 96 void rotate(Quaternion *q) {
TUATBM 0:c3af3416e383 97 // http://www.cprogramming.com/tutorial/3d/quaternions.html
TUATBM 0:c3af3416e383 98 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
TUATBM 0:c3af3416e383 99 // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
TUATBM 0:c3af3416e383 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
TUATBM 0:c3af3416e383 101
TUATBM 0:c3af3416e383 102 // P_out = q * P_in * conj(q)
TUATBM 0:c3af3416e383 103 // - P_out is the output vector
TUATBM 0:c3af3416e383 104 // - q is the orientation quaternion
TUATBM 0:c3af3416e383 105 // - P_in is the input vector (a*aReal)
TUATBM 0:c3af3416e383 106 // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
TUATBM 0:c3af3416e383 107 Quaternion p(0, x, y, z);
TUATBM 0:c3af3416e383 108
TUATBM 0:c3af3416e383 109 // quaternion multiplication: q * p, stored back in p
TUATBM 0:c3af3416e383 110 p = q -> getProduct(p);
TUATBM 0:c3af3416e383 111
TUATBM 0:c3af3416e383 112 // quaternion multiplication: p * conj(q), stored back in p
TUATBM 0:c3af3416e383 113 p = p.getProduct(q -> getConjugate());
TUATBM 0:c3af3416e383 114
TUATBM 0:c3af3416e383 115 // p quaternion is now [0, x', y', z']
TUATBM 0:c3af3416e383 116 x = p.x;
TUATBM 0:c3af3416e383 117 y = p.y;
TUATBM 0:c3af3416e383 118 z = p.z;
TUATBM 0:c3af3416e383 119 }
TUATBM 0:c3af3416e383 120
TUATBM 0:c3af3416e383 121 VectorInt16 getRotated(Quaternion *q) {
TUATBM 0:c3af3416e383 122 VectorInt16 r(x, y, z);
TUATBM 0:c3af3416e383 123 r.rotate(q);
TUATBM 0:c3af3416e383 124 return r;
TUATBM 0:c3af3416e383 125 }
TUATBM 0:c3af3416e383 126 };
TUATBM 0:c3af3416e383 127
TUATBM 0:c3af3416e383 128 class VectorFloat {
TUATBM 0:c3af3416e383 129 public:
TUATBM 0:c3af3416e383 130 float x;
TUATBM 0:c3af3416e383 131 float y;
TUATBM 0:c3af3416e383 132 float z;
TUATBM 0:c3af3416e383 133
TUATBM 0:c3af3416e383 134 VectorFloat() {
TUATBM 0:c3af3416e383 135 x = 0;
TUATBM 0:c3af3416e383 136 y = 0;
TUATBM 0:c3af3416e383 137 z = 0;
TUATBM 0:c3af3416e383 138 }
TUATBM 0:c3af3416e383 139
TUATBM 0:c3af3416e383 140 VectorFloat(float nx, float ny, float nz) {
TUATBM 0:c3af3416e383 141 x = nx;
TUATBM 0:c3af3416e383 142 y = ny;
TUATBM 0:c3af3416e383 143 z = nz;
TUATBM 0:c3af3416e383 144 }
TUATBM 0:c3af3416e383 145
TUATBM 0:c3af3416e383 146 float getMagnitude() {
TUATBM 0:c3af3416e383 147 return sqrt((float)(x*x + y*y + z*z));
TUATBM 0:c3af3416e383 148 }
TUATBM 0:c3af3416e383 149
TUATBM 0:c3af3416e383 150 void normalize() {
TUATBM 0:c3af3416e383 151 float m = getMagnitude();
TUATBM 0:c3af3416e383 152 x /= m;
TUATBM 0:c3af3416e383 153 y /= m;
TUATBM 0:c3af3416e383 154 z /= m;
TUATBM 0:c3af3416e383 155 }
TUATBM 0:c3af3416e383 156
TUATBM 0:c3af3416e383 157 VectorFloat getNormalized() {
TUATBM 0:c3af3416e383 158 VectorFloat r(x, y, z);
TUATBM 0:c3af3416e383 159 r.normalize();
TUATBM 0:c3af3416e383 160 return r;
TUATBM 0:c3af3416e383 161 }
TUATBM 0:c3af3416e383 162
TUATBM 0:c3af3416e383 163 void rotate(Quaternion *q) {
TUATBM 0:c3af3416e383 164 Quaternion p(0, x, y, z);
TUATBM 0:c3af3416e383 165
TUATBM 0:c3af3416e383 166 // quaternion multiplication: q * p, stored back in p
TUATBM 0:c3af3416e383 167 p = q -> getProduct(p);
TUATBM 0:c3af3416e383 168
TUATBM 0:c3af3416e383 169 // quaternion multiplication: p * conj(q), stored back in p
TUATBM 0:c3af3416e383 170 p = p.getProduct(q -> getConjugate());
TUATBM 0:c3af3416e383 171
TUATBM 0:c3af3416e383 172 // p quaternion is now [0, x', y', z']
TUATBM 0:c3af3416e383 173 x = p.x;
TUATBM 0:c3af3416e383 174 y = p.y;
TUATBM 0:c3af3416e383 175 z = p.z;
TUATBM 0:c3af3416e383 176 }
TUATBM 0:c3af3416e383 177
TUATBM 0:c3af3416e383 178 VectorFloat getRotated(Quaternion *q) {
TUATBM 0:c3af3416e383 179 VectorFloat r(x, y, z);
TUATBM 0:c3af3416e383 180 r.rotate(q);
TUATBM 0:c3af3416e383 181 return r;
TUATBM 0:c3af3416e383 182 }
TUATBM 0:c3af3416e383 183 };
TUATBM 0:c3af3416e383 184
TUATBM 0:c3af3416e383 185 #endif /* _HELPER_3DMATH_H_ */