aaa
Dependencies: ArduinoSerial I2Cdev2
Dependents: AutoFlight2017_now2 AutoFlight2018_Control sbus_test_2018 Autoflight2018_sbusread ... more
helper_3dmath.h@0:c3af3416e383, 2018-08-28 (annotated)
- Committer:
- TUATBM
- Date:
- Tue Aug 28 07:09:21 2018 +0000
- Revision:
- 0:c3af3416e383
a
Who changed what in which revision?
User | Revision | Line number | New 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_ */ |