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:
- pvaibhav
- Date:
- 2015-03-18
- Revision:
- 6:7ba72ec26bd1
- Parent:
- 5:e31eb7f8925d
- Child:
- 7:9fc4176dde36
File content as of revision 6:7ba72ec26bd1:
#ifndef __AHRSMATHDSP_QUATERNION_ #define __AHRSMATHDSP_QUATERNION_ #include "Vector3.h" const static float PI = 3.1415926; 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(Vector3 row0, Vector3 row1, Vector3 row2) { // from rotation matrix const float m[3][3] = { { row0.x, row0.y, row0.z }, { row1.x, row1.y, row1.z }, { row2.x, row2.y, row2.z } }; const float tr = m[0][0] + m[1][1] + m[2][2]; if (tr > 0) { const float S = sqrt(tr+1.0) * 2; w = 0.25 * S; v.x = (m[2][1] - m[1][2]) / S; v.y = (m[0][2] - m[2][0]) / S; v.z = (m[1][0] - m[0][1]) / S; } else if ((m[0][0] < m[1][1])&(m[0][0] < m[2][2])) { const float S = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2; w = (m[2][1] - m[1][2]) / S; v.x = 0.25 * S; v.y = (m[0][1] + m[1][0]) / S; v.z = (m[0][2] + m[2][0]) / S; } else if (m[1][1] < m[2][2]) { const float S = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2; w = (m[0][2] - m[2][0]) / S; v.x = (m[0][1] + m[1][0]) / S; v.y = 0.25 * S; v.z = (m[1][2] + m[2][1]) / S; } else { const float S = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2; w = (m[1][0] - m[0][1]) / S; v.x = (m[0][2] + m[2][0]) / S; v.y = (m[1][2] + m[2][1]) / S; v.z = 0.25 * S; } } 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); } } void getRotationMatrix(Vector3& row0, Vector3& row1, Vector3& row2) const { Quaternion q = this->normalise(); const double _w = q.w; const double _x = q.v.x; const double _y = q.v.y; const double _z = q.v.z; row0.x = 1-(2*(_y*_y))-(2*(_z*_z)); row0.y = (2*_x*_y)-(2*_w*_z); row0.z = (2*_x*_z)+(2*_w*_y); row1.x = (2*_x*_y)+(2*_w*_z); row1.y = 1-(2*(_x*_x))-(2*(_z*_z)); row1.z = (2*(_y*_z))-(2*(_w*_x)); row2.x = (2*(_x*_z))-(2*_w*_y); row2.y = (2*_y*_z)+(2*_w*_x); row2.z = 1-(2*(_x*_x))-(2*(_y*_y)); } Quaternion getAxisAngle() const { Quaternion q1(normalise()); // get normalised version float const angle = 2 * acos(q1.w); double const s = sqrt(1 - q1.w * q1.w); // assuming quaternion normalised then w is less than 1, so term always positive. if (s < 0.001) { // test to avoid divide by zero, s is always positive due to sqrt // if s close to zero then direction of axis not important q1.v = Vector3(1, 0, 0); } else { q1.v = q1.v / s; // normalise axis } return q1; } const Vector3 getEulerAngles() const { 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