Diff: Quaternion.h

Revision:

4:1ced03aa8c75

Parent:

3:c0137be74db4

Child:

5:e31eb7f8925d

--- a/Quaternion.h	Fri Mar 13 09:10:41 2015 +0000
+++ b/Quaternion.h	Fri Mar 13 09:11:22 2015 +0000
@@ -5,10 +5,11 @@
 
 const static float PI = 3.1415926;
 
-class Quaternion {
+class Quaternion
+{
 public:
     Quaternion() {
-        w = 0;    
+        w = 0;
     }
     Quaternion( float _w, float _x, float _y, float _z) {
         w = _w;
@@ -25,44 +26,36 @@
             { 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;
+
+        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)
-    {
+    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);
@@ -77,106 +70,104 @@
         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){
+    ~Quaternion() {}
+
+    void encode(char *buffer) {
         int value = (w * (1 << 30));
         char* bytes = (char*)&value;
-        for(int i = 0; i < 4; i ++){
+        for(int i = 0; i < 4; i ++) {
             buffer[i] = bytes[3-i];
         }
-        
+
         value = v.x * (1 << 30);
-        for(int i = 0; i < 4; i ++){
+        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 ++){
+        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 ++){
+        for(int i = 0; i < 4; i ++) {
             buffer[i+12] = bytes[3-i];
-        }            
+        }
     }
-    
-    void decode(const char *buffer){
+
+    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)));    
+            (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); 
+        v.set(_x, _y, _z);
     }
-    
-    float lengthSquared() const{
-        return w * w + (v * v);  
+
+    float lengthSquared() const {
+        return w * w + (v * v);
     }
-    
-    float length() const{
+
+    float length() const {
         return sqrt(lengthSquared());
     }
-    
-    Quaternion normalise() const{       
+
+    Quaternion normalise() const {
         return (*this)/length();
     }
-   
-    Quaternion conjugate() const{       
+
+    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;       
+
+    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;    
+
+    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){
+        } else if(t < 0.0f) {
             t=0.0f;
         }
         return ((*this)*(1-t) + q2*t).normalise();
     }
-    
-    Quaternion slerp( const Quaternion &q2, float t){
+
+    Quaternion slerp( const Quaternion &q2, float t) {
         if(t>1.0f) {
             t=1.0f;
-        } else if(t < 0.0f){
+        } else if(t < 0.0f) {
             t=0.0f;
         }
-        
+
         Quaternion q3;
         float dot = dot_product(q2);
 
-        if (dot < 0)
-        {
+        if (dot < 0) {
             dot = -dot;
             q3 = -q2;
         } else q3 = q2;
-        
-        if (dot < 0.95f)
-        {
+
+        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); 
-        }      
+            // if the angle is small, use linear interpolation
+            return lerp(q3,t);
+        }
     }
-    
+
     const Vector3 getEulerAngles() const {
         double sqw = w*w;
         double sqx = v.x*v.x;
@@ -185,7 +176,7 @@
         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;
@@ -201,15 +192,15 @@
         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) {
@@ -232,16 +223,16 @@
                           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);
@@ -249,10 +240,10 @@
 
     const Quaternion operator / (float scaler) const {
         return Quaternion(w / scaler, v / scaler);
-    }    
-    
+    }
+
     float w;
-    Vector3 v;      
+    Vector3 v;
 };
 
 #endif
\ No newline at end of file