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@10:69f240dbf0df, 2015-05-27 (annotated)

Committer:
pvaibhav
Date:
Wed May 27 13:01:34 2015 +0000
Revision:
10:69f240dbf0df
Parent:
9:7fc3fba01e2f 
Code reformatted

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