File content as of revision 15:791f35b0f220:

#include "Quaternions.h"
#include "CustomDatatypes.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;
}
//----------------------------------------------------------------------------------------------------------------//



//----------------------------------------------------------------------------------------------------------------//
Quaternion euler2Quaternion(Quaternion Q)
{
    
    

    // Abbreviations for the various angular functions
    double cy = cosf(Q.z * 0.5f);
    double sy = sinf(Q.z * 0.5f);
    double cp = cosf(Q.y * 0.5f);
    double sp = sinf(Q.y * 0.5f);
    double cr = cosf(Q.x * 0.5f);
    double sr = sinf(Q.x * 0.5f);

    Quaternion q;
    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * cp * sr - sy * sp * cr;
    q.y = sy * cp * sr + cy * sp * cr;
    q.z = sy * cp * cr - cy * sp * sr;
    
    return q;

}
//----------------------------------------------------------------------------------------------------------------//