Diff: Quaternion.h

Revision:

8:08d9c0010cc0

Parent:

7:9fc4176dde36

Child:

9:7fc3fba01e2f

--- a/Quaternion.h	Thu Mar 26 15:41:59 2015 +0000
+++ b/Quaternion.h	Mon Apr 20 13:38:01 2015 +0000
@@ -8,101 +8,103 @@
 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) {
+    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
-        const float m[3][3] = {
-            { row0.x, row0.y, row0.z },
-            { row1.x, row1.y, row1.z },
-            { row2.x, row2.y, row2.z }
+        float const 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];
+        float const tr = m[0][0] + m[1][1] + m[2][2];
 
         if (tr > 0) {
-            const float S = sqrt(tr+1.0) * 2;
+            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])) {
-            const float S = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2;
+        } 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]) {
-            const float S = sqrt(1.0 + m[1][1] - m[0][0] - m[2][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 {
-            const float S = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2;
+            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 theta_x, float theta_z, float theta_y) {
-        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);
+    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 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);
+        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;
+        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) {
+    void encode(char *const buffer) const {
         int value = (w * (1 << 30));
-        char* bytes = (char*)&value;
-        for(int i = 0; i < 4; i ++) {
-            buffer[i] = bytes[3-i];
+        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];
+        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];
+        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];
+        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)));
+        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) {
+    void set(float const _w, float const _x, float const _y, float const _z) {
         w = _w;
         v.set(_x, _y, _z);
     }
@@ -115,8 +117,16 @@
         return sqrt(lengthSquared());
     }
 
-    Quaternion normalise() const {
-        return (*this)/length();
+    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 {
@@ -127,28 +137,28 @@
         return conjugate() / lengthSquared();
     }
 
-    float dot_product(const Quaternion &q) {
-        return q.v * v + q.w*w;
+    float dot_product(Quaternion const &q) const {
+        return q.v * v + q.w * w;
     }
 
-    Vector3 rotate(const Vector3 &v) {
-        return ((*this) *  Quaternion(0, v) * conjugate()).v;
+    Vector3 rotate(Vector3 const &v) const {
+        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;
+    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).normalise();
+        return ((*this) * (1 - t) + q2 * t).normalised();
     }
 
-    Quaternion slerp( const Quaternion &q2, float t) {
-        if(t>1.0f) {
-            t=1.0f;
-        } else if(t < 0.0f) {
-            t=0.0f;
+    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;
@@ -157,42 +167,47 @@
         if (dot < 0) {
             dot = -dot;
             q3 = -q2;
-        } else 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);
+            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);
+            return lerp(q3, t);
         }
     }
-    
-    void getRotationMatrix(Vector3& row0, Vector3& row1, Vector3& row2) const {
-        Quaternion q = this->normalise();
+
+    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));
+        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
-        
+        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
+        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 {
@@ -202,76 +217,61 @@
     }
 
     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;
+        float const q0 = w;
+        float const q1 = v.x;
+        float const q2 = v.y;
+        float const q3 = v.z;
 
-        if (test > 0.499*unit) { // singularity at north pole
-            r.z = 2 * atan2(v.x,w);
-            r.y = PI/2;
-            r.x = 0;
-            return r;
-        }
-        if (test < -0.499*unit) { // singularity at south pole
-            r.z = -2 * atan2(v.x,w);
-            r.y = -PI/2;
-            r.x = 0;
-            return r;
-        }
-        r.z = atan2((double)(2*v.y*w-2*v.x*v.z ), (double)(sqx - sqy - sqz + sqw));
-        r.y = asin(2*test/unit);
-        r.x = atan2((double)(2*v.x*w-2*v.y*v.z) ,(double)( -sqx + sqy - sqz + sqw));
+        float const roll = asin(2 * (q0 * q2 - q3 * q1));
+        float const pitch =
+            atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
+        float const yaw =
+            atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
 
-        return r;
+        return Vector3(pitch, roll, yaw);
     }
 
     Quaternion difference(const Quaternion &q2) const {
-        return(Quaternion(q2*(*this).inverse()));
+        return Quaternion(q2 * (*this).inverse());
     }
 
-
-
-    //operators
-    Quaternion &operator = (const Quaternion &q) {
+    // 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 {
+    const Quaternion operator-(const Quaternion &q) const {
         return Quaternion(w - q.w, v - q.v);
     }
 
-    const Quaternion operator * (const Quaternion &q) const {
+    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 {
+    const Quaternion operator/(const Quaternion &q) const {
         Quaternion p = q.inverse();
         return p;
     }
 
-    const Quaternion operator - () const {
+    const Quaternion operator-() const {
         return Quaternion(-w, -v);
     }
 
-    //scaler operators
-    const Quaternion operator * (float scaler) const {
+    // scaler operators
+    const Quaternion operator*(float scaler) const {
         return Quaternion(w * scaler, v * scaler);
     }
 
-    const Quaternion operator / (float scaler) const {
+    const Quaternion operator/(float scaler) const {
         return Quaternion(w / scaler, v / scaler);
     }