BMX055
Dependents: 9Dsensor_test LINE_TRACE_CAR
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 }
Generated on Sat Jul 16 2022 06:27:02 by
1.7.2