A quick implementation of Quaternion and Vector classes for use with my MPU9150 library

## Quaternion.h

Committer:
p3p
Date:
2014-09-01
Revision:
0:3cc1a808d8c6

### File content as of revision 0:3cc1a808d8c6:

```#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
```