Diff: Quaternions.cpp

Revision:

4:e36c7042d3bb

Child:

5:155d224d855c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Quaternions.cpp	Sat Feb 23 01:20:08 2019 +0000
@@ -0,0 +1,413 @@
+#include "Quaternions.h"
+#include <math.h>
+
+
+//----------------------------------------------------------------------------------------------------------------//
+/*
+ *Takes the angular rate in roll, pitch, yaw form and converts it into quaternion notation
+*/
+Quaternion toQuaternionNotation(vector AngularRate, double dt) {
+//--------------Variables--------------//
+    Quaternion q;
+//--------------Variables--------------//
+    
+    
+    double l = sqrt(pow(AngularRate.x, 2) + pow(AngularRate.y, 2) + pow(AngularRate.z, 2));      //Normalise the quaternion such that it a unit quaternion between 0 and 1.
+    double theta = (dt * l) / 2;                                                //Get Theta in quaternions
+    
+    //double thetaRad = theta * 0.01745; // 0.01745 = pi/180
+                                                                                //Convert each component of rotation
+    q.w = cosf(theta);                                                          //* Magnitude 
+    q.x = sinf(theta) * (AngularRate.x / l);                                    //*
+    q.y = sinf(theta) * (AngularRate.y / l);                                    //*
+    q.z = sinf(theta) * (AngularRate.z / l);                                    //*
+                                                                                //Convert each component of rotation
+    return q;
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+Quaternion toQuaternionNotation123(vector AngularRate, double dt) {
+//--------------Variables--------------//
+    Quaternion Q;
+    float Sin_Mag;
+//--------------Variables--------------//
+    
+    Q.x  = AngularRate.x * dt;
+    Q.y  = AngularRate.y * dt;
+    Q.z  = AngularRate.z * dt;
+    Q.w  = 1.0f - 0.5f * (Q.x * Q.x +Q.y * Q.y + Q.z * Q.z);
+    
+    
+    
+    double l = sqrt(pow(AngularRate.x, 2) + pow(AngularRate.y, 2) + pow(AngularRate.z, 2));      //Normalise the quaternion such that it a unit quaternion between 0 and 1.
+    double theta = l / 2;                                                //Get Theta in quaternions
+    
+    //double thetaRad = theta * 0.01745; // 0.01745 = pi/180
+    
+    Sin_Mag = sin(theta) / l;  
+                                                                                //Convert each component of rotation
+    Q.x = AngularRate.x * Sin_Mag;
+    Q.y = AngularRate.y * Sin_Mag;
+    Q.z = AngularRate.z * Sin_Mag;
+    Q.w = cos(theta);                                                                            
+                                                                                
+    return Q;
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+/*
+ *This function updates the old orientation by rotating the reference frame by
+ *the change in angle. All this is done in quaternion form.
+ */
+Quaternion getQuaternionProduct(Quaternion Q1, Quaternion Q2) {
+//--------------Variables--------------//
+    Quaternion currentQ;
+//--------------Variables--------------//    
+    
+    
+    // Piecewise matrix multiplication
+    currentQ.w = (Q1.w*Q2.w - Q1.x*Q2.x - Q1.y*Q2.y - Q1.z*Q2.z);
+    currentQ.x = (Q1.w*Q2.x + Q1.x*Q2.w + Q1.y*Q2.z - Q1.z*Q2.y);
+    currentQ.y = (Q1.w*Q2.y - Q1.x*Q2.z + Q1.y*Q2.w + Q1.z*Q2.x);
+    currentQ.z = (Q1.w*Q2.z + Q1.x*Q2.y - Q1.y*Q2.x + Q1.z*Q2.w);
+    
+    return currentQ;
+
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+/*
+ *This function updates the old orientation by rotating the reference frame by
+ *the change in angle. All this is done in quaternion form.
+ */
+Quaternion getQVproduct(Quaternion Q, vector v) {
+//--------------Variables--------------//
+    Quaternion currentQ;
+//--------------Variables--------------//    
+    
+    
+    // Piecewise matrix multiplication
+    currentQ.x = (Q.w*v.x + Q.z*v.y - Q.y*v.z);
+    currentQ.y = (Q.w*v.y + Q.x*v.z - Q.z*v.x);
+    currentQ.z = (Q.y*v.x - Q.x*v.y + Q.w*v.z);
+    currentQ.w = (-Q.x*v.x - Q.y*v.y - Q.z*v.z);
+    
+    return currentQ;
+
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+Quaternion quaternionConjugate(Quaternion Q) {
+//--------------Variables--------------//
+    Quaternion localQ;
+//--------------Variables--------------//   
+    
+    localQ.w = Q.w;
+    localQ.x = -Q.x;
+    localQ.y = -Q.y;
+    localQ.z = -Q.z;
+    
+    return localQ;
+    
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+//----------------------------------------------------------------------------------------------------------------//
+Quaternion sumQuaternion(Quaternion Q1, Quaternion Q2) {
+//--------------Variables--------------//
+    Quaternion localQ;
+//--------------Variables--------------//   
+    
+    localQ.w = Q1.w + Q2.w;
+    localQ.x = Q1.x + Q2.x;
+    localQ.y = Q1.y + Q2.y;
+    localQ.z = Q1.z + Q2.z;
+    
+    return localQ;
+    
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+//----------------------------------------------------------------------------------------------------------------//
+vector sumVector(vector v1, vector v2) {
+//--------------Variables--------------//
+    vector localv;
+//--------------Variables--------------//   
+    
+    localv.x = v1.x + v2.x;
+    localv.y = v1.y + v2.y;
+    localv.z = v1.z + v2.z;
+    
+    return localv;
+    
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+vector rotateVector(Quaternion Q, vector v) {
+//--------------Variables--------------//
+Quaternion vectorQ;
+Quaternion Qconjugate;
+Quaternion rotationQ;
+vector localv;
+vector rotatedVector;
+//--------------Variables--------------// 
+
+    localv = v;  
+    vectorQ = vector2Quaternion(localv);
+    Qconjugate = quaternionConjugate(Q);
+    
+    //q*p*q*. Our point is our rotation axis therefore q*v*q*
+    rotationQ = getQuaternionProduct(Q, vectorQ);
+    rotationQ = getQuaternionProduct(rotationQ, Qconjugate);
+    rotatedVector = quaternion2Vector(rotationQ);
+    
+    return rotatedVector;
+    
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+Quaternion vector2Quaternion(vector v) {
+//--------------Variables--------------//
+Quaternion localQ;
+//--------------Variables--------------//   
+    
+    localQ.w = 0;
+    localQ.x = v.x;
+    localQ.y = v.y;
+    localQ.z = v.z;
+
+    return localQ; 
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+vector quaternion2Vector(Quaternion Q) {
+//--------------Variables--------------//
+vector localv;
+//--------------Variables--------------//   
+    
+    localv.x = Q.x;
+    localv.y = Q.y;
+    localv.z = Q.z;
+
+    return localv; 
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+Quaternion normaliseQuaternion(Quaternion Q) {
+//--------------Variables--------------//
+Quaternion localQ;
+double l;
+//--------------Variables--------------// 
+    localQ = Q;  
+    l = getQuaternionNorm(localQ);
+    
+    localQ.w = localQ.w / l;
+    localQ.x = localQ.x / l;
+    localQ.y = localQ.y / l;
+    localQ.z = localQ.z / l;
+
+    return localQ; 
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+double getQuaternionNorm(Quaternion Q) {
+//--------------Variables--------------//
+double l;
+//--------------Variables--------------//   
+    
+    l = sqrt(pow(Q.w, 2) + pow(Q.x, 2) + pow(Q.y, 2) + pow(Q.z, 2));      //Normalise the quaternion such that it a unit quaternion between 0 and 1.
+
+    return l; 
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+Quaternion getQaDelta(Quaternion Q) {
+//--------------Variables--------------//
+Quaternion localQ;
+//--------------Variables--------------//   
+    
+    localQ.w = sqrt((Q.z+1)/2);
+    localQ.x = -(Q.y/sqrt(2*(Q.z +1 )));
+    localQ.y = Q.x/sqrt(2*(Q.z +1 ));
+    localQ.z = 0;
+
+    return localQ; 
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+vector crossProduct(vector v1, vector v2) {
+//--------------Variables--------------//
+vector localv;
+//--------------Variables--------------//   
+    
+    
+    localv.x = v1.y * v2.z - v1.z * v2.y;
+    localv.y = v1.z * v2.x - v1.x * v2.z;
+    localv.z = v1.x * v2.y - v1.y * v2.x;
+    
+    return localv; 
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+//----------------------------------------------------------------------------------------------------------------//
+vector rotateGlobal(vector v1, Quaternion Q) {
+//--------------Variables--------------//
+vector localv;
+Quaternion localQ1;
+Quaternion localQ2;
+Quaternion localQconjugate;
+vector r;
+//--------------Variables--------------//   
+    
+    localQconjugate = quaternionConjugate(Q);
+    localQ1 = getQVproduct(Q, v1);
+    localQ2 = getQuaternionProduct(localQ1, localQconjugate);
+    localv = quaternion2Vector(localQ2);
+    
+    return localv;
+    
+    //r.x = -Q.x;
+    //r.y = -Q.y;
+    //r.z = -Q.z;
+    //return sumVector(v1, crossProduct(sumVector(r, r), sumVector(crossProduct(r, v1), mul(Q.w, v1))));
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+vector mul(float b, vector a) {
+    
+    
+    
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    
+    return a;
+}    
+
+
+//----------------------------------------------------------------------------------------------------------------//
+vector rotateLocal(vector v1, Quaternion Q) {
+//--------------Variables--------------//
+vector localv;
+Quaternion localQ1;
+Quaternion localQ2;
+Quaternion localQconjugate;
+vector r;
+//--------------Variables--------------//   
+    
+    localQconjugate = quaternionConjugate(Q);
+    localQ1 = getQVproduct(localQconjugate, v1);
+    localQ2 = getQuaternionProduct(localQ1, Q);
+    localv = quaternion2Vector(localQ2);
+    
+    return localv;
+    
+    
+    //r.x = Q.x;
+    //r.y = Q.y;
+    //r.z = Q.z;
+    
+    //return sumVector(v1, crossProduct(sumVector(r, r), sumVector(crossProduct(r, v1), mul(Q.w, v1))));
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+Quaternion updateQuaternion(Quaternion Qprevious, vector angularRate, double deltaTime)
+{
+    vector AngularRates;
+    Quaternion localQ, Qprime;
+    
+    //The angular rates will be sent from each IMU object in form a structure and not an array and therefroe the angularRate[] will have to be chnaged
+    AngularRates.x = angularRate.x;
+    AngularRates.y = angularRate.y;
+    AngularRates.z = angularRate.z;
+    
+    //Need to add variable delta_t when I add the function that gets me the time between samples
+    localQ = toQuaternionNotation(AngularRates, deltaTime);
+    Qprime = getQuaternionProduct(Qprevious, localQ); // Perform the rotation on the old quaternion
+    
+    return Qprime;
+}
+//----------------------------------------------------------------------------------------------------------------//
+
+
+
+//----------------------------------------------------------------------------------------------------------------//
+vector eulerA(Quaternion Q)
+{
+    vector EulerAngles;
+    
+
+    
+    EulerAngles.x = atan2((2*(Q.w*Q.x + Q.y*Q.z)), (pow(Q.w,2)-pow(Q.x,2)-pow(Q.y,2)+pow(Q.z,2))) * (180.0f/3.141592654f); //atan2 returns the angle between -π and π radians (equivalent to -180 and 180 degrees)
+    EulerAngles.y = -asin(2*(Q.x*Q.z - Q.w*Q.y)) * (180.0f/3.141592654f); //asin returns the angle between -π/2 and π/2 radians (equivalent to -90 and 90 degrees) 
+    EulerAngles.z = atan2((2*(Q.w*Q.z + Q.x*Q.y)), (pow(Q.w,2)+pow(Q.x,2)-pow(Q.y,2)-pow(Q.z,2))) * (180.0f/3.141592654f);
+   
+    return EulerAngles;
+}
+//----------------------------------------------------------------------------------------------------------------//
\ No newline at end of file