A quick implementation of Quaternion and Vector classes for use with my MPU9150 library
Dependents: cool_step_new cool_step_1 SML2
Fork of QuaternionMath by
Quaternion.h@1:857642c51139, 2015-02-24 (annotated)
- Committer:
- pvaibhav
- Date:
- Tue Feb 24 10:05:29 2015 +0000
- Revision:
- 1:857642c51139
- Parent:
- 0:3cc1a808d8c6
- Child:
- 3:c0137be74db4
const correctness modifications
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
p3p | 0:3cc1a808d8c6 | 1 | #ifndef __AHRSMATHDSP_QUATERNION_ |
p3p | 0:3cc1a808d8c6 | 2 | #define __AHRSMATHDSP_QUATERNION_ |
p3p | 0:3cc1a808d8c6 | 3 | |
p3p | 0:3cc1a808d8c6 | 4 | #include "Vector3.h" |
p3p | 0:3cc1a808d8c6 | 5 | |
pvaibhav | 1:857642c51139 | 6 | const static float PI = 3.1415926; |
pvaibhav | 1:857642c51139 | 7 | |
p3p | 0:3cc1a808d8c6 | 8 | class Quaternion { |
p3p | 0:3cc1a808d8c6 | 9 | public: |
p3p | 0:3cc1a808d8c6 | 10 | Quaternion() { |
p3p | 0:3cc1a808d8c6 | 11 | w = 0; |
p3p | 0:3cc1a808d8c6 | 12 | } |
p3p | 0:3cc1a808d8c6 | 13 | Quaternion( float _w, float _x, float _y, float _z) { |
p3p | 0:3cc1a808d8c6 | 14 | w = _w; |
p3p | 0:3cc1a808d8c6 | 15 | v.set(_x,_y,_z); |
p3p | 0:3cc1a808d8c6 | 16 | } |
p3p | 0:3cc1a808d8c6 | 17 | Quaternion( float _w, Vector3 _v) { |
p3p | 0:3cc1a808d8c6 | 18 | w = _w; |
p3p | 0:3cc1a808d8c6 | 19 | v = _v; |
p3p | 0:3cc1a808d8c6 | 20 | } |
p3p | 0:3cc1a808d8c6 | 21 | Quaternion(float theta_x, float theta_y, float theta_z) |
p3p | 0:3cc1a808d8c6 | 22 | { |
p3p | 0:3cc1a808d8c6 | 23 | float cos_z_2 = cosf(0.5f*theta_z); |
p3p | 0:3cc1a808d8c6 | 24 | float cos_y_2 = cosf(0.5f*theta_y); |
p3p | 0:3cc1a808d8c6 | 25 | float cos_x_2 = cosf(0.5f*theta_x); |
p3p | 0:3cc1a808d8c6 | 26 | |
p3p | 0:3cc1a808d8c6 | 27 | float sin_z_2 = sinf(0.5f*theta_z); |
p3p | 0:3cc1a808d8c6 | 28 | float sin_y_2 = sinf(0.5f*theta_y); |
p3p | 0:3cc1a808d8c6 | 29 | float sin_x_2 = sinf(0.5f*theta_x); |
p3p | 0:3cc1a808d8c6 | 30 | |
p3p | 0:3cc1a808d8c6 | 31 | // and now compute quaternion |
p3p | 0:3cc1a808d8c6 | 32 | w = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; |
p3p | 0:3cc1a808d8c6 | 33 | v.x = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; |
p3p | 0:3cc1a808d8c6 | 34 | v.y = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; |
p3p | 0:3cc1a808d8c6 | 35 | v.z = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; |
p3p | 0:3cc1a808d8c6 | 36 | } |
p3p | 0:3cc1a808d8c6 | 37 | ~Quaternion(){} |
p3p | 0:3cc1a808d8c6 | 38 | |
p3p | 0:3cc1a808d8c6 | 39 | void encode(char *buffer){ |
p3p | 0:3cc1a808d8c6 | 40 | int value = (w * (1 << 30)); |
p3p | 0:3cc1a808d8c6 | 41 | char* bytes = (char*)&value; |
p3p | 0:3cc1a808d8c6 | 42 | for(int i = 0; i < 4; i ++){ |
p3p | 0:3cc1a808d8c6 | 43 | buffer[i] = bytes[3-i]; |
p3p | 0:3cc1a808d8c6 | 44 | } |
p3p | 0:3cc1a808d8c6 | 45 | |
p3p | 0:3cc1a808d8c6 | 46 | value = v.x * (1 << 30); |
p3p | 0:3cc1a808d8c6 | 47 | for(int i = 0; i < 4; i ++){ |
p3p | 0:3cc1a808d8c6 | 48 | buffer[i+4] = bytes[3-i]; |
p3p | 0:3cc1a808d8c6 | 49 | } |
p3p | 0:3cc1a808d8c6 | 50 | |
p3p | 0:3cc1a808d8c6 | 51 | value = v.y * (1 << 30); |
p3p | 0:3cc1a808d8c6 | 52 | for(int i = 0; i < 4; i ++){ |
p3p | 0:3cc1a808d8c6 | 53 | buffer[i+8] = bytes[3-i]; |
p3p | 0:3cc1a808d8c6 | 54 | } |
p3p | 0:3cc1a808d8c6 | 55 | |
p3p | 0:3cc1a808d8c6 | 56 | value = v.z * (1 << 30); |
p3p | 0:3cc1a808d8c6 | 57 | for(int i = 0; i < 4; i ++){ |
p3p | 0:3cc1a808d8c6 | 58 | buffer[i+12] = bytes[3-i]; |
p3p | 0:3cc1a808d8c6 | 59 | } |
p3p | 0:3cc1a808d8c6 | 60 | } |
p3p | 0:3cc1a808d8c6 | 61 | |
p3p | 0:3cc1a808d8c6 | 62 | void decode(const char *buffer){ |
p3p | 0:3cc1a808d8c6 | 63 | set((float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), |
p3p | 0:3cc1a808d8c6 | 64 | (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), |
p3p | 0:3cc1a808d8c6 | 65 | (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), |
p3p | 0:3cc1a808d8c6 | 66 | (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); |
p3p | 0:3cc1a808d8c6 | 67 | } |
p3p | 0:3cc1a808d8c6 | 68 | |
p3p | 0:3cc1a808d8c6 | 69 | void set( float _w, float _x, float _y, float _z) { |
p3p | 0:3cc1a808d8c6 | 70 | w = _w; |
p3p | 0:3cc1a808d8c6 | 71 | v.set(_x, _y, _z); |
p3p | 0:3cc1a808d8c6 | 72 | } |
p3p | 0:3cc1a808d8c6 | 73 | |
p3p | 0:3cc1a808d8c6 | 74 | float lengthSquared() const{ |
p3p | 0:3cc1a808d8c6 | 75 | return w * w + (v * v); |
p3p | 0:3cc1a808d8c6 | 76 | } |
p3p | 0:3cc1a808d8c6 | 77 | |
p3p | 0:3cc1a808d8c6 | 78 | float length() const{ |
p3p | 0:3cc1a808d8c6 | 79 | return sqrt(lengthSquared()); |
p3p | 0:3cc1a808d8c6 | 80 | } |
p3p | 0:3cc1a808d8c6 | 81 | |
p3p | 0:3cc1a808d8c6 | 82 | Quaternion normalise() const{ |
p3p | 0:3cc1a808d8c6 | 83 | return (*this)/length(); |
p3p | 0:3cc1a808d8c6 | 84 | } |
p3p | 0:3cc1a808d8c6 | 85 | |
p3p | 0:3cc1a808d8c6 | 86 | Quaternion conjugate() const{ |
p3p | 0:3cc1a808d8c6 | 87 | return Quaternion(w, -v); |
p3p | 0:3cc1a808d8c6 | 88 | } |
p3p | 0:3cc1a808d8c6 | 89 | |
p3p | 0:3cc1a808d8c6 | 90 | Quaternion inverse() const { |
p3p | 0:3cc1a808d8c6 | 91 | return conjugate() / lengthSquared(); |
p3p | 0:3cc1a808d8c6 | 92 | } |
p3p | 0:3cc1a808d8c6 | 93 | |
p3p | 0:3cc1a808d8c6 | 94 | float dot_product(const Quaternion &q){ |
p3p | 0:3cc1a808d8c6 | 95 | return q.v * v + q.w*w; |
p3p | 0:3cc1a808d8c6 | 96 | } |
p3p | 0:3cc1a808d8c6 | 97 | |
p3p | 0:3cc1a808d8c6 | 98 | Vector3 rotate(const Vector3 &v){ |
p3p | 0:3cc1a808d8c6 | 99 | return ((*this) * Quaternion(0, v) * conjugate()).v; |
p3p | 0:3cc1a808d8c6 | 100 | } |
p3p | 0:3cc1a808d8c6 | 101 | |
p3p | 0:3cc1a808d8c6 | 102 | Quaternion lerp(const Quaternion &q2, float t) { |
p3p | 0:3cc1a808d8c6 | 103 | if(t>1.0f) { |
p3p | 0:3cc1a808d8c6 | 104 | t=1.0f; |
p3p | 0:3cc1a808d8c6 | 105 | } else if(t < 0.0f){ |
p3p | 0:3cc1a808d8c6 | 106 | t=0.0f; |
p3p | 0:3cc1a808d8c6 | 107 | } |
p3p | 0:3cc1a808d8c6 | 108 | return ((*this)*(1-t) + q2*t).normalise(); |
p3p | 0:3cc1a808d8c6 | 109 | } |
p3p | 0:3cc1a808d8c6 | 110 | |
p3p | 0:3cc1a808d8c6 | 111 | Quaternion slerp( const Quaternion &q2, float t){ |
p3p | 0:3cc1a808d8c6 | 112 | if(t>1.0f) { |
p3p | 0:3cc1a808d8c6 | 113 | t=1.0f; |
p3p | 0:3cc1a808d8c6 | 114 | } else if(t < 0.0f){ |
p3p | 0:3cc1a808d8c6 | 115 | t=0.0f; |
p3p | 0:3cc1a808d8c6 | 116 | } |
p3p | 0:3cc1a808d8c6 | 117 | |
p3p | 0:3cc1a808d8c6 | 118 | Quaternion q3; |
p3p | 0:3cc1a808d8c6 | 119 | float dot = dot_product(q2); |
p3p | 0:3cc1a808d8c6 | 120 | |
p3p | 0:3cc1a808d8c6 | 121 | if (dot < 0) |
p3p | 0:3cc1a808d8c6 | 122 | { |
p3p | 0:3cc1a808d8c6 | 123 | dot = -dot; |
p3p | 0:3cc1a808d8c6 | 124 | q3 = -q2; |
p3p | 0:3cc1a808d8c6 | 125 | } else q3 = q2; |
p3p | 0:3cc1a808d8c6 | 126 | |
p3p | 0:3cc1a808d8c6 | 127 | if (dot < 0.95f) |
p3p | 0:3cc1a808d8c6 | 128 | { |
p3p | 0:3cc1a808d8c6 | 129 | float angle = acosf(dot); |
p3p | 0:3cc1a808d8c6 | 130 | return ((*this)*sinf(angle*(1-t)) + q3*sinf(angle*t))/sinf(angle); |
p3p | 0:3cc1a808d8c6 | 131 | } else { |
p3p | 0:3cc1a808d8c6 | 132 | // if the angle is small, use linear interpolation |
p3p | 0:3cc1a808d8c6 | 133 | return lerp(q3,t); |
p3p | 0:3cc1a808d8c6 | 134 | } |
p3p | 0:3cc1a808d8c6 | 135 | } |
p3p | 0:3cc1a808d8c6 | 136 | |
pvaibhav | 1:857642c51139 | 137 | const Vector3 getEulerAngles() const { |
p3p | 0:3cc1a808d8c6 | 138 | double sqw = w*w; |
p3p | 0:3cc1a808d8c6 | 139 | double sqx = v.x*v.x; |
p3p | 0:3cc1a808d8c6 | 140 | double sqy = v.y*v.y; |
p3p | 0:3cc1a808d8c6 | 141 | double sqz = v.z*v.z; |
p3p | 0:3cc1a808d8c6 | 142 | double unit = sqx + sqy + sqz + sqw; |
p3p | 0:3cc1a808d8c6 | 143 | double test = v.x*v.y + v.z*w; |
p3p | 0:3cc1a808d8c6 | 144 | Vector3 r; |
p3p | 0:3cc1a808d8c6 | 145 | |
p3p | 0:3cc1a808d8c6 | 146 | if (test > 0.499*unit) { // singularity at north pole |
p3p | 0:3cc1a808d8c6 | 147 | r.z = 2 * atan2(v.x,w); |
p3p | 0:3cc1a808d8c6 | 148 | r.x = PI/2; |
p3p | 0:3cc1a808d8c6 | 149 | r.y = 0; |
p3p | 0:3cc1a808d8c6 | 150 | return r; |
p3p | 0:3cc1a808d8c6 | 151 | } |
p3p | 0:3cc1a808d8c6 | 152 | if (test < -0.499*unit) { // singularity at south pole |
p3p | 0:3cc1a808d8c6 | 153 | r.z = -2 * atan2(v.x,w); |
p3p | 0:3cc1a808d8c6 | 154 | r.x = -PI/2; |
p3p | 0:3cc1a808d8c6 | 155 | r.y = 0; |
p3p | 0:3cc1a808d8c6 | 156 | return r; |
p3p | 0:3cc1a808d8c6 | 157 | } |
p3p | 0:3cc1a808d8c6 | 158 | r.z = atan2((double)(2*v.y*w-2*v.x*v.z ), (double)(sqx - sqy - sqz + sqw)); |
p3p | 0:3cc1a808d8c6 | 159 | r.x = asin(2*test/unit); |
p3p | 0:3cc1a808d8c6 | 160 | r.y = atan2((double)(2*v.x*w-2*v.y*v.z) ,(double)( -sqx + sqy - sqz + sqw)); |
p3p | 0:3cc1a808d8c6 | 161 | |
p3p | 0:3cc1a808d8c6 | 162 | return r; |
p3p | 0:3cc1a808d8c6 | 163 | } |
p3p | 0:3cc1a808d8c6 | 164 | |
p3p | 0:3cc1a808d8c6 | 165 | Quaternion difference(const Quaternion &q2) const { |
p3p | 0:3cc1a808d8c6 | 166 | return(Quaternion(q2*(*this).inverse())); |
p3p | 0:3cc1a808d8c6 | 167 | } |
p3p | 0:3cc1a808d8c6 | 168 | |
p3p | 0:3cc1a808d8c6 | 169 | |
p3p | 0:3cc1a808d8c6 | 170 | |
p3p | 0:3cc1a808d8c6 | 171 | //operators |
p3p | 0:3cc1a808d8c6 | 172 | Quaternion &operator = (const Quaternion &q) { |
p3p | 0:3cc1a808d8c6 | 173 | w = q.w; |
p3p | 0:3cc1a808d8c6 | 174 | v = q.v; |
p3p | 0:3cc1a808d8c6 | 175 | return *this; |
p3p | 0:3cc1a808d8c6 | 176 | } |
p3p | 0:3cc1a808d8c6 | 177 | |
p3p | 0:3cc1a808d8c6 | 178 | const Quaternion operator + (const Quaternion &q) const { |
p3p | 0:3cc1a808d8c6 | 179 | return Quaternion(w+q.w, v+q.v); |
p3p | 0:3cc1a808d8c6 | 180 | } |
p3p | 0:3cc1a808d8c6 | 181 | |
p3p | 0:3cc1a808d8c6 | 182 | const Quaternion operator - (const Quaternion &q) const { |
p3p | 0:3cc1a808d8c6 | 183 | return Quaternion(w - q.w, v - q.v); |
p3p | 0:3cc1a808d8c6 | 184 | } |
p3p | 0:3cc1a808d8c6 | 185 | |
p3p | 0:3cc1a808d8c6 | 186 | const Quaternion operator * (const Quaternion &q) const { |
p3p | 0:3cc1a808d8c6 | 187 | return Quaternion(w * q.w - v * q.v, |
p3p | 0:3cc1a808d8c6 | 188 | v.y * q.v.z - v.z * q.v.y + w * q.v.x + v.x * q.w, |
p3p | 0:3cc1a808d8c6 | 189 | v.z * q.v.x - v.x * q.v.z + w * q.v.y + v.y * q.w, |
p3p | 0:3cc1a808d8c6 | 190 | v.x * q.v.y - v.y * q.v.x + w * q.v.z + v.z * q.w); |
p3p | 0:3cc1a808d8c6 | 191 | } |
p3p | 0:3cc1a808d8c6 | 192 | |
p3p | 0:3cc1a808d8c6 | 193 | const Quaternion operator / (const Quaternion &q) const { |
p3p | 0:3cc1a808d8c6 | 194 | Quaternion p = q.inverse(); |
p3p | 0:3cc1a808d8c6 | 195 | return p; |
p3p | 0:3cc1a808d8c6 | 196 | } |
p3p | 0:3cc1a808d8c6 | 197 | |
p3p | 0:3cc1a808d8c6 | 198 | const Quaternion operator - () const { |
p3p | 0:3cc1a808d8c6 | 199 | return Quaternion(-w, -v); |
p3p | 0:3cc1a808d8c6 | 200 | } |
p3p | 0:3cc1a808d8c6 | 201 | |
p3p | 0:3cc1a808d8c6 | 202 | //scaler operators |
p3p | 0:3cc1a808d8c6 | 203 | const Quaternion operator * (float scaler) const { |
p3p | 0:3cc1a808d8c6 | 204 | return Quaternion(w * scaler, v * scaler); |
p3p | 0:3cc1a808d8c6 | 205 | } |
p3p | 0:3cc1a808d8c6 | 206 | |
p3p | 0:3cc1a808d8c6 | 207 | const Quaternion operator / (float scaler) const { |
p3p | 0:3cc1a808d8c6 | 208 | return Quaternion(w / scaler, v / scaler); |
p3p | 0:3cc1a808d8c6 | 209 | } |
p3p | 0:3cc1a808d8c6 | 210 | |
p3p | 0:3cc1a808d8c6 | 211 | float w; |
p3p | 0:3cc1a808d8c6 | 212 | Vector3 v; |
p3p | 0:3cc1a808d8c6 | 213 | }; |
p3p | 0:3cc1a808d8c6 | 214 | |
p3p | 0:3cc1a808d8c6 | 215 | #endif |