TobyRich GmbH / QuaternionMath

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 Chris Pepper

Quaternion.h@4:1ced03aa8c75, 2015-03-13 (annotated)

Committer:
pvaibhav
Date:
Fri Mar 13 09:11:22 2015 +0000
Revision:
4:1ced03aa8c75
Parent:
3:c0137be74db4
Child:
5:e31eb7f8925d
Code reformatting

Who changed what in which revision?

UserRevisionLine numberNew 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
pvaibhav 4:1ced03aa8c75 8 class Quaternion
pvaibhav 4:1ced03aa8c75 9 {
p3p 0:3cc1a808d8c6 10 public:
p3p 0:3cc1a808d8c6 11 Quaternion() {
pvaibhav 4:1ced03aa8c75 12 w = 0;
p3p 0:3cc1a808d8c6 13 }
p3p 0:3cc1a808d8c6 14 Quaternion( float _w, float _x, float _y, float _z) {
p3p 0:3cc1a808d8c6 15 w = _w;
p3p 0:3cc1a808d8c6 16 v.set(_x,_y,_z);
p3p 0:3cc1a808d8c6 17 }
p3p 0:3cc1a808d8c6 18 Quaternion( float _w, Vector3 _v) {
p3p 0:3cc1a808d8c6 19 w = _w;
p3p 0:3cc1a808d8c6 20 v = _v;
p3p 0:3cc1a808d8c6 21 }
pvaibhav 3:c0137be74db4 22 Quaternion(Vector3 row0, Vector3 row1, Vector3 row2) {
pvaibhav 3:c0137be74db4 23 // from rotation matrix
pvaibhav 3:c0137be74db4 24 const float m[3][3] = {
pvaibhav 3:c0137be74db4 25 { row0.x, row0.y, row0.z },
pvaibhav 3:c0137be74db4 26 { row1.x, row1.y, row1.z },
pvaibhav 3:c0137be74db4 27 { row2.x, row2.y, row2.z }
pvaibhav 3:c0137be74db4 28 };
pvaibhav 4:1ced03aa8c75 29
pvaibhav 3:c0137be74db4 30 const float tr = m[0][0] + m[1][1] + m[2][2];
pvaibhav 4:1ced03aa8c75 31
pvaibhav 4:1ced03aa8c75 32 if (tr > 0) {
pvaibhav 4:1ced03aa8c75 33 const float S = sqrt(tr+1.0) * 2;
pvaibhav 4:1ced03aa8c75 34 w = 0.25 * S;
pvaibhav 4:1ced03aa8c75 35 v.x = (m[2][1] - m[1][2]) / S;
pvaibhav 4:1ced03aa8c75 36 v.y = (m[0][2] - m[2][0]) / S;
pvaibhav 4:1ced03aa8c75 37 v.z = (m[1][0] - m[0][1]) / S;
pvaibhav 4:1ced03aa8c75 38 } else if ((m[0][0] < m[1][1])&(m[0][0] < m[2][2])) {
pvaibhav 4:1ced03aa8c75 39 const float S = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2;
pvaibhav 4:1ced03aa8c75 40 w = (m[2][1] - m[1][2]) / S;
pvaibhav 4:1ced03aa8c75 41 v.x = 0.25 * S;
pvaibhav 4:1ced03aa8c75 42 v.y = (m[0][1] + m[1][0]) / S;
pvaibhav 4:1ced03aa8c75 43 v.z = (m[0][2] + m[2][0]) / S;
pvaibhav 4:1ced03aa8c75 44 } else if (m[1][1] < m[2][2]) {
pvaibhav 4:1ced03aa8c75 45 const float S = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2;
pvaibhav 4:1ced03aa8c75 46 w = (m[0][2] - m[2][0]) / S;
pvaibhav 4:1ced03aa8c75 47 v.x = (m[0][1] + m[1][0]) / S;
pvaibhav 4:1ced03aa8c75 48 v.y = 0.25 * S;
pvaibhav 4:1ced03aa8c75 49 v.z = (m[1][2] + m[2][1]) / S;
pvaibhav 4:1ced03aa8c75 50 } else {
pvaibhav 4:1ced03aa8c75 51 const float S = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2;
pvaibhav 4:1ced03aa8c75 52 w = (m[1][0] - m[0][1]) / S;
pvaibhav 4:1ced03aa8c75 53 v.x = (m[0][2] + m[2][0]) / S;
pvaibhav 4:1ced03aa8c75 54 v.y = (m[1][2] + m[2][1]) / S;
pvaibhav 4:1ced03aa8c75 55 v.z = 0.25 * S;
pvaibhav 3:c0137be74db4 56 }
pvaibhav 3:c0137be74db4 57 }
pvaibhav 4:1ced03aa8c75 58 Quaternion(float theta_x, float theta_y, float theta_z) {
p3p 0:3cc1a808d8c6 59 float cos_z_2 = cosf(0.5f*theta_z);
p3p 0:3cc1a808d8c6 60 float cos_y_2 = cosf(0.5f*theta_y);
p3p 0:3cc1a808d8c6 61 float cos_x_2 = cosf(0.5f*theta_x);
p3p 0:3cc1a808d8c6 62
p3p 0:3cc1a808d8c6 63 float sin_z_2 = sinf(0.5f*theta_z);
p3p 0:3cc1a808d8c6 64 float sin_y_2 = sinf(0.5f*theta_y);
p3p 0:3cc1a808d8c6 65 float sin_x_2 = sinf(0.5f*theta_x);
p3p 0:3cc1a808d8c6 66
p3p 0:3cc1a808d8c6 67 // and now compute quaternion
p3p 0:3cc1a808d8c6 68 w = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
p3p 0:3cc1a808d8c6 69 v.x = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
p3p 0:3cc1a808d8c6 70 v.y = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
p3p 0:3cc1a808d8c6 71 v.z = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
p3p 0:3cc1a808d8c6 72 }
pvaibhav 4:1ced03aa8c75 73 ~Quaternion() {}
pvaibhav 4:1ced03aa8c75 74
pvaibhav 4:1ced03aa8c75 75 void encode(char *buffer) {
p3p 0:3cc1a808d8c6 76 int value = (w * (1 << 30));
p3p 0:3cc1a808d8c6 77 char* bytes = (char*)&value;
pvaibhav 4:1ced03aa8c75 78 for(int i = 0; i < 4; i ++) {
p3p 0:3cc1a808d8c6 79 buffer[i] = bytes[3-i];
p3p 0:3cc1a808d8c6 80 }
pvaibhav 4:1ced03aa8c75 81
p3p 0:3cc1a808d8c6 82 value = v.x * (1 << 30);
pvaibhav 4:1ced03aa8c75 83 for(int i = 0; i < 4; i ++) {
p3p 0:3cc1a808d8c6 84 buffer[i+4] = bytes[3-i];
p3p 0:3cc1a808d8c6 85 }
pvaibhav 4:1ced03aa8c75 86
p3p 0:3cc1a808d8c6 87 value = v.y * (1 << 30);
pvaibhav 4:1ced03aa8c75 88 for(int i = 0; i < 4; i ++) {
p3p 0:3cc1a808d8c6 89 buffer[i+8] = bytes[3-i];
p3p 0:3cc1a808d8c6 90 }
pvaibhav 4:1ced03aa8c75 91
p3p 0:3cc1a808d8c6 92 value = v.z * (1 << 30);
pvaibhav 4:1ced03aa8c75 93 for(int i = 0; i < 4; i ++) {
p3p 0:3cc1a808d8c6 94 buffer[i+12] = bytes[3-i];
pvaibhav 4:1ced03aa8c75 95 }
p3p 0:3cc1a808d8c6 96 }
pvaibhav 4:1ced03aa8c75 97
pvaibhav 4:1ced03aa8c75 98 void decode(const char *buffer) {
p3p 0:3cc1a808d8c6 99 set((float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
pvaibhav 4:1ced03aa8c75 100 (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
pvaibhav 4:1ced03aa8c75 101 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
pvaibhav 4:1ced03aa8c75 102 (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 103 }
pvaibhav 4:1ced03aa8c75 104
p3p 0:3cc1a808d8c6 105 void set( float _w, float _x, float _y, float _z) {
p3p 0:3cc1a808d8c6 106 w = _w;
pvaibhav 4:1ced03aa8c75 107 v.set(_x, _y, _z);
p3p 0:3cc1a808d8c6 108 }
pvaibhav 4:1ced03aa8c75 109
pvaibhav 4:1ced03aa8c75 110 float lengthSquared() const {
pvaibhav 4:1ced03aa8c75 111 return w * w + (v * v);
p3p 0:3cc1a808d8c6 112 }
pvaibhav 4:1ced03aa8c75 113
pvaibhav 4:1ced03aa8c75 114 float length() const {
p3p 0:3cc1a808d8c6 115 return sqrt(lengthSquared());
p3p 0:3cc1a808d8c6 116 }
pvaibhav 4:1ced03aa8c75 117
pvaibhav 4:1ced03aa8c75 118 Quaternion normalise() const {
p3p 0:3cc1a808d8c6 119 return (*this)/length();
p3p 0:3cc1a808d8c6 120 }
pvaibhav 4:1ced03aa8c75 121
pvaibhav 4:1ced03aa8c75 122 Quaternion conjugate() const {
p3p 0:3cc1a808d8c6 123 return Quaternion(w, -v);
p3p 0:3cc1a808d8c6 124 }
pvaibhav 4:1ced03aa8c75 125
p3p 0:3cc1a808d8c6 126 Quaternion inverse() const {
p3p 0:3cc1a808d8c6 127 return conjugate() / lengthSquared();
p3p 0:3cc1a808d8c6 128 }
pvaibhav 4:1ced03aa8c75 129
pvaibhav 4:1ced03aa8c75 130 float dot_product(const Quaternion &q) {
pvaibhav 4:1ced03aa8c75 131 return q.v * v + q.w*w;
p3p 0:3cc1a808d8c6 132 }
pvaibhav 4:1ced03aa8c75 133
pvaibhav 4:1ced03aa8c75 134 Vector3 rotate(const Vector3 &v) {
pvaibhav 4:1ced03aa8c75 135 return ((*this) * Quaternion(0, v) * conjugate()).v;
p3p 0:3cc1a808d8c6 136 }
pvaibhav 4:1ced03aa8c75 137
p3p 0:3cc1a808d8c6 138 Quaternion lerp(const Quaternion &q2, float t) {
p3p 0:3cc1a808d8c6 139 if(t>1.0f) {
p3p 0:3cc1a808d8c6 140 t=1.0f;
pvaibhav 4:1ced03aa8c75 141 } else if(t < 0.0f) {
p3p 0:3cc1a808d8c6 142 t=0.0f;
p3p 0:3cc1a808d8c6 143 }
p3p 0:3cc1a808d8c6 144 return ((*this)*(1-t) + q2*t).normalise();
p3p 0:3cc1a808d8c6 145 }
pvaibhav 4:1ced03aa8c75 146
pvaibhav 4:1ced03aa8c75 147 Quaternion slerp( const Quaternion &q2, float t) {
p3p 0:3cc1a808d8c6 148 if(t>1.0f) {
p3p 0:3cc1a808d8c6 149 t=1.0f;
pvaibhav 4:1ced03aa8c75 150 } else if(t < 0.0f) {
p3p 0:3cc1a808d8c6 151 t=0.0f;
p3p 0:3cc1a808d8c6 152 }
pvaibhav 4:1ced03aa8c75 153
p3p 0:3cc1a808d8c6 154 Quaternion q3;
p3p 0:3cc1a808d8c6 155 float dot = dot_product(q2);
p3p 0:3cc1a808d8c6 156
pvaibhav 4:1ced03aa8c75 157 if (dot < 0) {
p3p 0:3cc1a808d8c6 158 dot = -dot;
p3p 0:3cc1a808d8c6 159 q3 = -q2;
p3p 0:3cc1a808d8c6 160 } else q3 = q2;
pvaibhav 4:1ced03aa8c75 161
pvaibhav 4:1ced03aa8c75 162 if (dot < 0.95f) {
p3p 0:3cc1a808d8c6 163 float angle = acosf(dot);
p3p 0:3cc1a808d8c6 164 return ((*this)*sinf(angle*(1-t)) + q3*sinf(angle*t))/sinf(angle);
p3p 0:3cc1a808d8c6 165 } else {
pvaibhav 4:1ced03aa8c75 166 // if the angle is small, use linear interpolation
pvaibhav 4:1ced03aa8c75 167 return lerp(q3,t);
pvaibhav 4:1ced03aa8c75 168 }
p3p 0:3cc1a808d8c6 169 }
pvaibhav 4:1ced03aa8c75 170
pvaibhav 1:857642c51139 171 const Vector3 getEulerAngles() const {
p3p 0:3cc1a808d8c6 172 double sqw = w*w;
p3p 0:3cc1a808d8c6 173 double sqx = v.x*v.x;
p3p 0:3cc1a808d8c6 174 double sqy = v.y*v.y;
p3p 0:3cc1a808d8c6 175 double sqz = v.z*v.z;
p3p 0:3cc1a808d8c6 176 double unit = sqx + sqy + sqz + sqw;
p3p 0:3cc1a808d8c6 177 double test = v.x*v.y + v.z*w;
p3p 0:3cc1a808d8c6 178 Vector3 r;
pvaibhav 4:1ced03aa8c75 179
p3p 0:3cc1a808d8c6 180 if (test > 0.499*unit) { // singularity at north pole
p3p 0:3cc1a808d8c6 181 r.z = 2 * atan2(v.x,w);
p3p 0:3cc1a808d8c6 182 r.x = PI/2;
p3p 0:3cc1a808d8c6 183 r.y = 0;
p3p 0:3cc1a808d8c6 184 return r;
p3p 0:3cc1a808d8c6 185 }
p3p 0:3cc1a808d8c6 186 if (test < -0.499*unit) { // singularity at south pole
p3p 0:3cc1a808d8c6 187 r.z = -2 * atan2(v.x,w);
p3p 0:3cc1a808d8c6 188 r.x = -PI/2;
p3p 0:3cc1a808d8c6 189 r.y = 0;
p3p 0:3cc1a808d8c6 190 return r;
p3p 0:3cc1a808d8c6 191 }
p3p 0:3cc1a808d8c6 192 r.z = atan2((double)(2*v.y*w-2*v.x*v.z ), (double)(sqx - sqy - sqz + sqw));
p3p 0:3cc1a808d8c6 193 r.x = asin(2*test/unit);
p3p 0:3cc1a808d8c6 194 r.y = atan2((double)(2*v.x*w-2*v.y*v.z) ,(double)( -sqx + sqy - sqz + sqw));
pvaibhav 4:1ced03aa8c75 195
p3p 0:3cc1a808d8c6 196 return r;
p3p 0:3cc1a808d8c6 197 }
pvaibhav 4:1ced03aa8c75 198
p3p 0:3cc1a808d8c6 199 Quaternion difference(const Quaternion &q2) const {
p3p 0:3cc1a808d8c6 200 return(Quaternion(q2*(*this).inverse()));
pvaibhav 4:1ced03aa8c75 201 }
pvaibhav 4:1ced03aa8c75 202
pvaibhav 4:1ced03aa8c75 203
p3p 0:3cc1a808d8c6 204
p3p 0:3cc1a808d8c6 205 //operators
p3p 0:3cc1a808d8c6 206 Quaternion &operator = (const Quaternion &q) {
p3p 0:3cc1a808d8c6 207 w = q.w;
p3p 0:3cc1a808d8c6 208 v = q.v;
p3p 0:3cc1a808d8c6 209 return *this;
p3p 0:3cc1a808d8c6 210 }
p3p 0:3cc1a808d8c6 211
p3p 0:3cc1a808d8c6 212 const Quaternion operator + (const Quaternion &q) const {
p3p 0:3cc1a808d8c6 213 return Quaternion(w+q.w, v+q.v);
p3p 0:3cc1a808d8c6 214 }
p3p 0:3cc1a808d8c6 215
p3p 0:3cc1a808d8c6 216 const Quaternion operator - (const Quaternion &q) const {
p3p 0:3cc1a808d8c6 217 return Quaternion(w - q.w, v - q.v);
p3p 0:3cc1a808d8c6 218 }
p3p 0:3cc1a808d8c6 219
p3p 0:3cc1a808d8c6 220 const Quaternion operator * (const Quaternion &q) const {
p3p 0:3cc1a808d8c6 221 return Quaternion(w * q.w - v * q.v,
p3p 0:3cc1a808d8c6 222 v.y * q.v.z - v.z * q.v.y + w * q.v.x + v.x * q.w,
p3p 0:3cc1a808d8c6 223 v.z * q.v.x - v.x * q.v.z + w * q.v.y + v.y * q.w,
p3p 0:3cc1a808d8c6 224 v.x * q.v.y - v.y * q.v.x + w * q.v.z + v.z * q.w);
p3p 0:3cc1a808d8c6 225 }
pvaibhav 4:1ced03aa8c75 226
p3p 0:3cc1a808d8c6 227 const Quaternion operator / (const Quaternion &q) const {
p3p 0:3cc1a808d8c6 228 Quaternion p = q.inverse();
p3p 0:3cc1a808d8c6 229 return p;
p3p 0:3cc1a808d8c6 230 }
pvaibhav 4:1ced03aa8c75 231
p3p 0:3cc1a808d8c6 232 const Quaternion operator - () const {
p3p 0:3cc1a808d8c6 233 return Quaternion(-w, -v);
p3p 0:3cc1a808d8c6 234 }
pvaibhav 4:1ced03aa8c75 235
p3p 0:3cc1a808d8c6 236 //scaler operators
p3p 0:3cc1a808d8c6 237 const Quaternion operator * (float scaler) const {
p3p 0:3cc1a808d8c6 238 return Quaternion(w * scaler, v * scaler);
p3p 0:3cc1a808d8c6 239 }
p3p 0:3cc1a808d8c6 240
p3p 0:3cc1a808d8c6 241 const Quaternion operator / (float scaler) const {
p3p 0:3cc1a808d8c6 242 return Quaternion(w / scaler, v / scaler);
pvaibhav 4:1ced03aa8c75 243 }
pvaibhav 4:1ced03aa8c75 244
p3p 0:3cc1a808d8c6 245 float w;
pvaibhav 4:1ced03aa8c75 246 Vector3 v;
p3p 0:3cc1a808d8c6 247 };
p3p 0:3cc1a808d8c6 248
p3p 0:3cc1a808d8c6 249 #endif