BMX055

Dependents:   9Dsensor_test LINE_TRACE_CAR

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Quaternion.cpp Source File

Quaternion.cpp

00001 #include "mbed.h"
00002 #include "Quaternion.h"
00003 #include <math.h>
00004 
00005 Quaternion::Quaternion()
00006 {
00007     w = 1;
00008     x = 0;
00009     y = 0;
00010     z = 0;
00011 }
00012 
00013 Quaternion::Quaternion(double x1, double x2, double x3, double x4)
00014 {
00015     w = x1;
00016     x = x2;
00017     y = x3;
00018     z = x4;
00019 }
00020 
00021 Quaternion Quaternion::operator + (Quaternion p)
00022 {
00023     Quaternion q;
00024     q.w = w + p.w;
00025     q.x = x + p.x;
00026     q.y = y + p.y;
00027     q.z = z + p.z;
00028 
00029     return q;
00030 }
00031 
00032 Quaternion Quaternion::operator+=(Quaternion p)
00033 {
00034     w += p.w;
00035     x += p.x;
00036     y += p.y;
00037     z += p.z;
00038 
00039     return *this;
00040 }
00041 
00042 Quaternion Quaternion::operator * (double s)
00043 {
00044     Quaternion q;
00045     q.w = w * s;
00046     q.x = x * s;
00047     q.y = y * s;
00048     q.z = z * s;
00049 
00050     return q;
00051 }
00052 
00053 Quaternion Quaternion::operator * (Quaternion p)
00054 {
00055     Quaternion q;
00056     q.w = w * p.w - x * p.x - y * p.y - z * p.z;
00057     q.x = w * p.x + x * p.w + y * p.z - z * p.y;
00058     q.y = w * p.y - x * p.z + y * p.w + z * p.x;
00059     q.z = w * p.z + x * p.y - y * p.x + z * p.w;
00060 
00061     return q;
00062 }
00063 
00064 Quaternion Quaternion::operator*=(double s)
00065 {
00066     w *= s;
00067     x *= s;
00068     y *= s;
00069     z *= s;
00070     return *this;
00071 }
00072 
00073 Quaternion Quaternion::operator*=(Quaternion p)
00074 {
00075     w = w * p.w - x * p.x - y * p.y - z * p.z;
00076     x = w * p.x + x * p.w + y * p.z - z * p.y;
00077     y = w * p.y - x * p.z + y * p.w + z * p.x;
00078     z = w * p.z + x * p.y - y * p.x + z * p.w;
00079     return *this;
00080 }
00081 
00082 Quaternion Quaternion::operator / (double s)
00083 {
00084     Quaternion q;
00085     q.w = w / s;
00086     q.x = x / s;
00087     q.y = y / s;
00088     q.z = z / s;
00089 
00090     return q;
00091 }
00092 
00093 Quaternion Quaternion::operator/=(double s)
00094 {
00095     w /= s;
00096     x /= s;
00097     y /= s;
00098     z /= s;
00099     return *this;
00100 }
00101 
00102 Quaternion Quaternion::conjugate(void)
00103 {
00104     Quaternion q;
00105     q.w = w;
00106     q.x = -x;
00107     q.y = -y;
00108     q.z = -z;
00109     return q;
00110 }
00111 
00112 double Quaternion::norm(void)
00113 {
00114     return sqrt(w * w + x * x + y * y + z * z);
00115 }