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-05-27
- Revision:
- 10:69f240dbf0df
- Parent:
- 9:7fc3fba01e2f
File content as of revision 10:69f240dbf0df:
#ifndef __AHRSMATHDSP_QUATERNION_ #define __AHRSMATHDSP_QUATERNION_ #include "Vector3.h" const static float PI = 3.1415926; class Quaternion { public: Quaternion() : w(0.0f), v(0.0f, 0.0f, 0.0f) {} Quaternion(float const _w, float const _x, float const _y, float const _z) : w(_w), v(_x, _y, _z) {} Quaternion(float const _w, const Vector3 &_v) : w(_w), v(_v) {} Quaternion(Vector3 const &row0, Vector3 const &row1, Vector3 const &row2) { // from rotation matrix float const m[3][3] = {{row0.x, row0.y, row0.z}, {row1.x, row1.y, row1.z}, {row2.x, row2.y, row2.z} }; float const tr = m[0][0] + m[1][1] + m[2][2]; if (tr > 0) { float const 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])) { float const 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]) { float const 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 { float const 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 const theta_x, float const theta_z, float const theta_y) { float const cos_z_2 = cosf(0.5f * theta_z); float const cos_y_2 = cosf(0.5f * theta_y); float const cos_x_2 = cosf(0.5f * theta_x); float const sin_z_2 = sinf(0.5f * theta_z); float const sin_y_2 = sinf(0.5f * theta_y); float const 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; } void encode(char *const buffer) const { int value = (w * (1 << 30)); char const* bytes = (char const*) & 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 const _w, float const _x, float const _y, float const _z) { w = _w; v.set(_x, _y, _z); } float lengthSquared() const { return w * w + (v * v); } float length() const { return sqrt(lengthSquared()); } void normalise() { float const magnitude = length(); w /= magnitude; // pop pop v.x /= magnitude; v.y /= magnitude; v.z /= magnitude; } Quaternion normalised() const { return (*this) / length(); } Quaternion conjugate() const { return Quaternion(w, -v); } Quaternion inverse() const { return conjugate() / lengthSquared(); } float dot_product(Quaternion const &q) const { return q.v * v + q.w * w; } Vector3 rotate(Vector3 const &v) const { return ((*this) * Quaternion(0, v) * conjugate()).v; } Quaternion lerp(Quaternion const &q2, float t) const { if (t > 1.0f) { t = 1.0f; } else if (t < 0.0f) { t = 0.0f; } return ((*this) * (1 - t) + q2 * t).normalised(); } Quaternion slerp(Quaternion const &q2, float t) const { 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 const 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(normalised()); 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(normalised()); // 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 { float const q0 = w; float const q1 = v.x; float const q2 = v.y; float const q3 = v.z; float const roll = asin(2.0 * (q0 * q2 - q3 * q1)); float const pitch = atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); float const yaw = atan2(2.0 * (q0 * q3 + q1 * q2), 1.0 - 2.0 * (q2 * q2 + q3 * q3)); return Vector3(pitch, roll, yaw); } 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