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

Files at this revision

API Documentation at this revision

Comitter:
p3p
Date:
Mon Sep 01 13:48:26 2014 +0000
Commit message:
quick Quaternion and Vector classes

Changed in this revision

Quaternion.h Show annotated file Show diff for this revision Revisions of this file
Vector3.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 3cc1a808d8c6 Quaternion.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Quaternion.h	Mon Sep 01 13:48:26 2014 +0000
@@ -0,0 +1,213 @@
+#ifndef __AHRSMATHDSP_QUATERNION_
+#define __AHRSMATHDSP_QUATERNION_
+
+#include "Vector3.h"
+
+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(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);
+
+        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);
+
+        // 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;
+    }
+    ~Quaternion(){}
+    
+    void encode(char *buffer){
+        int value = (w * (1 << 30));
+        char* bytes = (char*)&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 _w, float _x, float _y, float _z) {
+        w = _w;
+        v.set(_x, _y, _z); 
+    }
+    
+    float lengthSquared() const{
+        return w * w + (v * v);  
+    }
+    
+    float length() const{
+        return sqrt(lengthSquared());
+    }
+    
+    Quaternion normalise() const{       
+        return (*this)/length();
+    }
+   
+    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;       
+    }
+    
+    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){
+            t=0.0f;
+        }
+        return ((*this)*(1-t) + q2*t).normalise();
+    }
+    
+    Quaternion slerp( const Quaternion &q2, float t){
+        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 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); 
+        }      
+    }
+    
+    const Vector3 getEulerAngles(){
+        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;
+        
+        if (test > 0.499*unit) { // singularity at north pole
+            r.z = 2 * atan2(v.x,w);
+            r.x = PI/2;
+            r.y = 0;
+            return r;
+        }
+        if (test < -0.499*unit) { // singularity at south pole
+            r.z = -2 * atan2(v.x,w);
+            r.x = -PI/2;
+            r.y = 0;
+            return r;
+        }
+        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) {
+        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
\ No newline at end of file
diff -r 000000000000 -r 3cc1a808d8c6 Vector3.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector3.h	Mon Sep 01 13:48:26 2014 +0000
@@ -0,0 +1,106 @@
+#ifndef __AHRSMATHDSP_VECTOR3_
+#define __AHRSMATHDSP_VECTOR3_
+
+#include "arm_math.h"
+
+class Vector3 {
+public:
+    Vector3(){
+        x = y = z = 0;
+    }
+    
+    Vector3(float x, float y, float z){
+        this->x = x;
+        this->y = y;
+        this->z = z;
+    }
+    
+    void set(float x, float y, float z){
+        this->x = x;
+        this->y = y;
+        this->z = z;     
+    }
+    
+    float length_squared(){
+        return x*x + y*y + z*z;
+    }
+    
+    float length(){
+        return sqrt(length_squared());    
+    }
+    
+    void normalise(){
+        float len = length();
+        x = x/len;
+        y = y/len;
+        z = z/len;
+    }
+    
+    float dot_product(const Vector3 &v){
+        return x*v.x + y*v.y + z*v.z;
+    }
+    
+    Vector3 cross_product(const Vector3 &v){
+        Vector3 temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
+        return temp;
+    }
+    
+    //operator overides
+    void operator ()(const float x, const float y, const float z) {
+        this->x = x;
+        this->y = y;
+        this->z = z;
+    }
+
+    bool operator == (const Vector3 &v) {
+        return (x==v.x && y==v.y && z==v.z);
+    }
+
+    bool operator != (const Vector3 &v) {
+        return (x!=v.x || y!=v.y || z!=v.z);
+    }
+
+    const Vector3 &operator = (const Vector3 &v) {   
+        x = v.x;
+        y = v.y;
+        z = v.z;     
+        return *this;
+    }
+    
+    const Vector3 operator - (void) const {
+        return Vector3(-x,-y,-z);
+    }
+ 
+    const Vector3 operator + (const Vector3 &v) const {
+        return Vector3(x+v.x, y+v.y, z+v.z);
+    }  
+
+    const Vector3 operator - (const Vector3 &v) const {
+        return Vector3(x-v.x, y-v.y, z-v.z);
+    }
+    
+    const Vector3 operator * (const float num) const {
+        Vector3 temp;
+        temp.x = x * num;
+        temp.y = y * num;  
+        temp.z = z * num;  
+        return temp;
+    }
+
+    const Vector3 operator / (const float num) const {
+        Vector3 temp;
+        temp.x = x / num;
+        temp.y = y / num;  
+        temp.z = z / num;  
+        return temp;
+    }
+    
+    float operator * (const Vector3 &v) const {
+        return x*v.x + y*v.y + z*v.z;
+    }
+    
+    float x, y, z;
+       
+};
+
+#endif
\ No newline at end of file