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
00001 #ifndef __AHRSMATHDSP_QUATERNION_ 00002 #define __AHRSMATHDSP_QUATERNION_ 00003 00004 #include "Vector3.h" 00005 00006 class Quaternion { 00007 public: 00008 Quaternion() { 00009 w = 0; 00010 } 00011 Quaternion( float _w, float _x, float _y, float _z) { 00012 w = _w; 00013 v.set(_x,_y,_z); 00014 } 00015 Quaternion( float _w, Vector3 _v) { 00016 w = _w; 00017 v = _v; 00018 } 00019 Quaternion(float theta_x, float theta_y, float theta_z) 00020 { 00021 float cos_z_2 = cosf(0.5f*theta_z); 00022 float cos_y_2 = cosf(0.5f*theta_y); 00023 float cos_x_2 = cosf(0.5f*theta_x); 00024 00025 float sin_z_2 = sinf(0.5f*theta_z); 00026 float sin_y_2 = sinf(0.5f*theta_y); 00027 float sin_x_2 = sinf(0.5f*theta_x); 00028 00029 // and now compute quaternion 00030 w = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; 00031 v.x = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; 00032 v.y = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; 00033 v.z = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; 00034 } 00035 ~Quaternion(){} 00036 00037 void encode(char *buffer){ 00038 int value = (w * (1 << 30)); 00039 char* bytes = (char*)&value; 00040 for(int i = 0; i < 4; i ++){ 00041 buffer[i] = bytes[3-i]; 00042 } 00043 00044 value = v.x * (1 << 30); 00045 for(int i = 0; i < 4; i ++){ 00046 buffer[i+4] = bytes[3-i]; 00047 } 00048 00049 value = v.y * (1 << 30); 00050 for(int i = 0; i < 4; i ++){ 00051 buffer[i+8] = bytes[3-i]; 00052 } 00053 00054 value = v.z * (1 << 30); 00055 for(int i = 0; i < 4; i ++){ 00056 buffer[i+12] = bytes[3-i]; 00057 } 00058 } 00059 00060 void decode(const char *buffer){ 00061 set((float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), 00062 (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), 00063 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), 00064 (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); 00065 } 00066 00067 void set( float _w, float _x, float _y, float _z) { 00068 w = _w; 00069 v.set(_x, _y, _z); 00070 } 00071 00072 float lengthSquared() const{ 00073 return w * w + (v * v); 00074 } 00075 00076 float length() const{ 00077 return sqrt(lengthSquared()); 00078 } 00079 00080 Quaternion normalise() const{ 00081 return (*this)/length(); 00082 } 00083 00084 Quaternion conjugate() const{ 00085 return Quaternion(w, -v); 00086 } 00087 00088 Quaternion inverse() const { 00089 return conjugate() / lengthSquared(); 00090 } 00091 00092 float dot_product(const Quaternion &q){ 00093 return q.v * v + q.w*w; 00094 } 00095 00096 Vector3 rotate(const Vector3 &v){ 00097 return ((*this) * Quaternion(0, v) * conjugate()).v; 00098 } 00099 00100 Quaternion lerp(const Quaternion &q2, float t) { 00101 if(t>1.0f) { 00102 t=1.0f; 00103 } else if(t < 0.0f){ 00104 t=0.0f; 00105 } 00106 return ((*this)*(1-t) + q2*t).normalise(); 00107 } 00108 00109 Quaternion slerp( const Quaternion &q2, float t){ 00110 if(t>1.0f) { 00111 t=1.0f; 00112 } else if(t < 0.0f){ 00113 t=0.0f; 00114 } 00115 00116 Quaternion q3; 00117 float dot = dot_product(q2); 00118 00119 if (dot < 0) 00120 { 00121 dot = -dot; 00122 q3 = -q2; 00123 } else q3 = q2; 00124 00125 if (dot < 0.95f) 00126 { 00127 float angle = acosf(dot); 00128 return ((*this)*sinf(angle*(1-t)) + q3*sinf(angle*t))/sinf(angle); 00129 } else { 00130 // if the angle is small, use linear interpolation 00131 return lerp(q3,t); 00132 } 00133 } 00134 00135 const Vector3 getEulerAngles(){ 00136 double sqw = w*w; 00137 double sqx = v.x*v.x; 00138 double sqy = v.y*v.y; 00139 double sqz = v.z*v.z; 00140 double unit = sqx + sqy + sqz + sqw; 00141 double test = v.x*v.y + v.z*w; 00142 Vector3 r; 00143 00144 if (test > 0.499*unit) { // singularity at north pole 00145 r.z = 2 * atan2(v.x,w); 00146 r.x = PI/2; 00147 r.y = 0; 00148 return r; 00149 } 00150 if (test < -0.499*unit) { // singularity at south pole 00151 r.z = -2 * atan2(v.x,w); 00152 r.x = -PI/2; 00153 r.y = 0; 00154 return r; 00155 } 00156 r.z = atan2((double)(2*v.y*w-2*v.x*v.z ), (double)(sqx - sqy - sqz + sqw)); 00157 r.x = asin(2*test/unit); 00158 r.y = atan2((double)(2*v.x*w-2*v.y*v.z) ,(double)( -sqx + sqy - sqz + sqw)); 00159 00160 return r; 00161 } 00162 00163 Quaternion difference(const Quaternion &q2) const { 00164 return(Quaternion(q2*(*this).inverse())); 00165 } 00166 00167 00168 00169 //operators 00170 Quaternion &operator = (const Quaternion &q) { 00171 w = q.w; 00172 v = q.v; 00173 return *this; 00174 } 00175 00176 const Quaternion operator + (const Quaternion &q) const { 00177 return Quaternion(w+q.w, v+q.v); 00178 } 00179 00180 const Quaternion operator - (const Quaternion &q) const { 00181 return Quaternion(w - q.w, v - q.v); 00182 } 00183 00184 const Quaternion operator * (const Quaternion &q) const { 00185 return Quaternion(w * q.w - v * q.v, 00186 v.y * q.v.z - v.z * q.v.y + w * q.v.x + v.x * q.w, 00187 v.z * q.v.x - v.x * q.v.z + w * q.v.y + v.y * q.w, 00188 v.x * q.v.y - v.y * q.v.x + w * q.v.z + v.z * q.w); 00189 } 00190 00191 const Quaternion operator / (const Quaternion &q) const { 00192 Quaternion p = q.inverse(); 00193 return p; 00194 } 00195 00196 const Quaternion operator - () const { 00197 return Quaternion(-w, -v); 00198 } 00199 00200 //scaler operators 00201 const Quaternion operator * (float scaler) const { 00202 return Quaternion(w * scaler, v * scaler); 00203 } 00204 00205 const Quaternion operator / (float scaler) const { 00206 return Quaternion(w / scaler, v / scaler); 00207 } 00208 00209 float w; 00210 Vector3 v; 00211 }; 00212 00213 #endif
Generated on Wed Jul 13 2022 05:32:14 by 1.7.2