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