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
Diff: Quaternion.h
- Revision:
- 0:3cc1a808d8c6
- Child:
- 1:857642c51139
diff -r 000000000000 -r 3cc1a808d8c6 Quaternion.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Quaternion.h Mon Sep 01 13:48:26 2014 +0000 @@ -0,0 +1,213 @@ +#ifndef __AHRSMATHDSP_QUATERNION_ +#define __AHRSMATHDSP_QUATERNION_ + +#include "Vector3.h" + +class Quaternion { +public: + Quaternion() { + w = 0; + } + Quaternion( float _w, float _x, float _y, float _z) { + w = _w; + v.set(_x,_y,_z); + } + Quaternion( float _w, Vector3 _v) { + w = _w; + v = _v; + } + Quaternion(float theta_x, float theta_y, float theta_z) + { + float cos_z_2 = cosf(0.5f*theta_z); + float cos_y_2 = cosf(0.5f*theta_y); + float cos_x_2 = cosf(0.5f*theta_x); + + float sin_z_2 = sinf(0.5f*theta_z); + float sin_y_2 = sinf(0.5f*theta_y); + float sin_x_2 = sinf(0.5f*theta_x); + + // and now compute quaternion + w = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; + v.x = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; + v.y = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; + v.z = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; + } + ~Quaternion(){} + + void encode(char *buffer){ + int value = (w * (1 << 30)); + char* bytes = (char*)&value; + for(int i = 0; i < 4; i ++){ + buffer[i] = bytes[3-i]; + } + + value = v.x * (1 << 30); + for(int i = 0; i < 4; i ++){ + buffer[i+4] = bytes[3-i]; + } + + value = v.y * (1 << 30); + for(int i = 0; i < 4; i ++){ + buffer[i+8] = bytes[3-i]; + } + + value = v.z * (1 << 30); + for(int i = 0; i < 4; i ++){ + buffer[i+12] = bytes[3-i]; + } + } + + void decode(const char *buffer){ + set((float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), + (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), + (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), + (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); + } + + void set( float _w, float _x, float _y, float _z) { + w = _w; + v.set(_x, _y, _z); + } + + float lengthSquared() const{ + return w * w + (v * v); + } + + float length() const{ + return sqrt(lengthSquared()); + } + + Quaternion normalise() const{ + return (*this)/length(); + } + + Quaternion conjugate() const{ + return Quaternion(w, -v); + } + + Quaternion inverse() const { + return conjugate() / lengthSquared(); + } + + float dot_product(const Quaternion &q){ + return q.v * v + q.w*w; + } + + Vector3 rotate(const Vector3 &v){ + return ((*this) * Quaternion(0, v) * conjugate()).v; + } + + Quaternion lerp(const Quaternion &q2, float t) { + if(t>1.0f) { + t=1.0f; + } else if(t < 0.0f){ + t=0.0f; + } + return ((*this)*(1-t) + q2*t).normalise(); + } + + Quaternion slerp( const Quaternion &q2, float t){ + if(t>1.0f) { + t=1.0f; + } else if(t < 0.0f){ + t=0.0f; + } + + Quaternion q3; + float dot = dot_product(q2); + + if (dot < 0) + { + dot = -dot; + q3 = -q2; + } else q3 = q2; + + if (dot < 0.95f) + { + float angle = acosf(dot); + return ((*this)*sinf(angle*(1-t)) + q3*sinf(angle*t))/sinf(angle); + } else { + // if the angle is small, use linear interpolation + return lerp(q3,t); + } + } + + const Vector3 getEulerAngles(){ + double sqw = w*w; + double sqx = v.x*v.x; + double sqy = v.y*v.y; + double sqz = v.z*v.z; + double unit = sqx + sqy + sqz + sqw; + double test = v.x*v.y + v.z*w; + Vector3 r; + + if (test > 0.499*unit) { // singularity at north pole + r.z = 2 * atan2(v.x,w); + r.x = PI/2; + r.y = 0; + return r; + } + if (test < -0.499*unit) { // singularity at south pole + r.z = -2 * atan2(v.x,w); + r.x = -PI/2; + r.y = 0; + return r; + } + r.z = atan2((double)(2*v.y*w-2*v.x*v.z ), (double)(sqx - sqy - sqz + sqw)); + r.x = asin(2*test/unit); + r.y = atan2((double)(2*v.x*w-2*v.y*v.z) ,(double)( -sqx + sqy - sqz + sqw)); + + return r; + } + + Quaternion difference(const Quaternion &q2) const { + return(Quaternion(q2*(*this).inverse())); + } + + + + //operators + Quaternion &operator = (const Quaternion &q) { + w = q.w; + v = q.v; + return *this; + } + + const Quaternion operator + (const Quaternion &q) const { + return Quaternion(w+q.w, v+q.v); + } + + const Quaternion operator - (const Quaternion &q) const { + return Quaternion(w - q.w, v - q.v); + } + + const Quaternion operator * (const Quaternion &q) const { + return Quaternion(w * q.w - v * q.v, + v.y * q.v.z - v.z * q.v.y + w * q.v.x + v.x * q.w, + v.z * q.v.x - v.x * q.v.z + w * q.v.y + v.y * q.w, + v.x * q.v.y - v.y * q.v.x + w * q.v.z + v.z * q.w); + } + + const Quaternion operator / (const Quaternion &q) const { + Quaternion p = q.inverse(); + return p; + } + + const Quaternion operator - () const { + return Quaternion(-w, -v); + } + + //scaler operators + const Quaternion operator * (float scaler) const { + return Quaternion(w * scaler, v * scaler); + } + + const Quaternion operator / (float scaler) const { + return Quaternion(w / scaler, v / scaler); + } + + float w; + Vector3 v; +}; + +#endif \ No newline at end of file