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
Quaternion.h
- Committer:
- p3p
- Date:
- 2014-09-01
- Revision:
- 0:3cc1a808d8c6
- Child:
- 1:857642c51139
File content as of revision 0:3cc1a808d8c6:
#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