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@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?

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 }
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