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@3:c0137be74db4, 2015-03-13 (annotated)

Committer:
pvaibhav
Date:
Fri Mar 13 09:10:41 2015 +0000
Revision:
3:c0137be74db4
Parent:
1:857642c51139
Child:
4:1ced03aa8c75
Added Quaternion constructor from rotation matrix. Fixed Vector3.normalised()

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