Chris Pepper / QuaternionMath

A quick implementation of Quaternion and Vector classes for use with my MPU9150 library

Dependents:   MPU9150_Example MPU9150_nucleo_noni2cdev CANSAT_COMBINED SolarOnFoils_MainModule_20150518 ... more

Quaternion.h@0:3cc1a808d8c6, 2014-09-01 (annotated)

Committer:
p3p
Date:
Mon Sep 01 13:48:26 2014 +0000
Revision:
0:3cc1a808d8c6
quick Quaternion and Vector classes

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