Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #ifndef QUATERNION_H_
00009 #define QUATERNION_H_
00010
00011 #include <math.h>
00012 #include <iostream>
00013 #include "TransformationTypes.h"
00014 #include "MathHelper.h"
00015
00016
00017 namespace rotateOp {
00018
00025 class Quaternion {
00026 private:
00027
00028 double w;
00029 double x;
00030 double y;
00031 double z;
00032
00033 public:
00034
00038 Quaternion() {
00039 w = 1;
00040 x = 0;
00041 y = 0;
00042 z = 0;
00043
00044 }
00045
00054 Quaternion(double w, double x, double y, double z) {
00055 this->w = w;
00056 this->x = x;
00057 this->y = y;
00058 this->z = z;
00059
00060 }
00061
00062
00072 void fromEuler(double roll, double pitch, double yaw,
00073 TransformationTypes::EulerType type) {
00074
00075 double c1 = (double) (cos(roll * 0.5f));
00076 double s1 = (double) (sin(roll * 0.5f));
00077 double c2 = (double) (cos(pitch * 0.5f));
00078 double s2 = (double) (sin(pitch * 0.5f));
00079 double c3 = (double) (cos(yaw * 0.5f));
00080 double s3 = (double) (sin(yaw * 0.5f));
00081 switch (type) {
00082 case TransformationTypes::EULER123:
00083
00084 w = MathHelper::roundDecimal(c1 * c2 * c3 - s1 * s2 * s3, 5);
00085 x = MathHelper::roundDecimal(s1 * c2 * c3 + c1 * s2 * s3, 5);
00086 y = MathHelper::roundDecimal(c1 * s2 * c3 - s1 * c2 * s3, 5);
00087 z = MathHelper::roundDecimal(c1 * c2 * s3 + s1 * s2 * c3, 5);
00088 break;
00089 case TransformationTypes::EULER321:
00090
00091 w = MathHelper::roundDecimal(c3 * c2 * c1 + s3 * s2 * s1, 5);
00092 x = MathHelper::roundDecimal(c3 * c2 * s1 - s3 * s2 * c1, 5);
00093 y = MathHelper::roundDecimal(c3 * s2 * c1 + s3 * c2 * s1, 5);
00094 z = MathHelper::roundDecimal(s3 * c2 * c1 - c3 * s2 * s1, 5);
00095 break;
00096
00097 case TransformationTypes::EULER312:
00098
00099 w = MathHelper::roundDecimal(c3 * c1 * c2 - s3 * s1 * s2, 5);
00100 x = MathHelper::roundDecimal(s1 * c3 * c2 - s3 * c1 * s2, 5);
00101 y = MathHelper::roundDecimal(s3 * s1 * c2 + c3 * c1 * s2, 5);
00102 z = MathHelper::roundDecimal(s3 * c1 * c2 + c3 * s1 * s2, 5);
00103 break;
00104
00105 default:
00106 std::cerr << "This euler type is not implemented." << std::endl;
00107
00108 break;
00109 }
00110
00111 }
00112
00123 double *toArray() {
00124 double *quaternionArray = new double[4];
00125 quaternionArray[0] = MathHelper::roundDecimal(w, 5);
00126 quaternionArray[1] = MathHelper::roundDecimal(x, 5);
00127 quaternionArray[2] = MathHelper::roundDecimal(y, 5);
00128 quaternionArray[3] = MathHelper::roundDecimal(z, 5);
00129 return quaternionArray;
00130 }
00131
00132 friend std::ostream & operator <<(std::ostream & o, Quaternion q) {
00133 o << "Quaternion: [" << q.w << ", (" << q.x << ", " << q.y << ", "
00134 << q.z << ")]";
00135 return o;
00136 }
00137
00138 double getW() const {
00139 return w;
00140 }
00141
00142 double getX() const {
00143 return x;
00144 }
00145
00146 double getY() const {
00147 return y;
00148 }
00149
00150 double getZ() const {
00151 return z;
00152 }
00153
00154 };
00155 }
00156 #endif