Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #ifndef EULER_H_
00009 #define EULER_H_
00010 #include <math.h>
00011 #include <iostream>
00012 #include <TransformationTypes.h>
00013 #include <Quaternion.h>
00014
00015 namespace rotateOp {
00024 class Euler {
00025 private:
00026 double roll;
00027 double pitch;
00028 double yaw;
00029
00030 TransformationTypes::EulerType eulerType;
00031
00032 public:
00033
00038 Euler(TransformationTypes::EulerType eulerType) {
00039 this->eulerType = eulerType;
00040 this->roll = 0;
00041 this->pitch = 0;
00042 this->yaw = 0;
00043 }
00051 Euler(double roll, double pitch, double yaw,
00052 TransformationTypes::EulerType eulerType) {
00053 this->eulerType = eulerType;
00054 this->roll = MathHelper::roundDecimal(roll,5);
00055 this->pitch = MathHelper::roundDecimal(pitch,5);
00056 this->yaw = MathHelper::roundDecimal(yaw,5);
00057 }
00058
00065 void fromQuaternion(Quaternion quaternion) {
00066
00067 switch (eulerType) {
00068 case TransformationTypes::EULER123:
00069 roll = MathHelper::roundDecimal(atan2(-2
00070 * (quaternion.getY() * quaternion.getZ()
00071 - quaternion.getW() * quaternion.getX()),
00072 quaternion.getW() * quaternion.getW() - quaternion.getX()
00073 * quaternion.getX() - quaternion.getY()
00074 * quaternion.getY() + quaternion.getZ()
00075 * quaternion.getZ()), 5);
00076 pitch = MathHelper::roundDecimal(asin(2
00077 * (quaternion.getX() * quaternion.getZ()
00078 + quaternion.getW() * quaternion.getY())), 5);
00079 yaw = MathHelper::roundDecimal(atan2(-2
00080 * (quaternion.getX() * quaternion.getY()
00081 - quaternion.getW() * quaternion.getZ()),
00082 quaternion.getW() * quaternion.getW() + quaternion.getX()
00083 * quaternion.getX() - quaternion.getY()
00084 * quaternion.getY() - quaternion.getZ()
00085 * quaternion.getZ()), 5);
00086
00087 break;
00088 case TransformationTypes::EULER321:
00089 yaw = MathHelper::roundDecimal(atan2(2
00090 * (quaternion.getY() * quaternion.getX()
00091 - quaternion.getZ() * quaternion.getW()),
00092 quaternion.getW() * quaternion.getW() + quaternion.getX()
00093 * quaternion.getX() - quaternion.getY()
00094 * quaternion.getY() - quaternion.getZ()
00095 * quaternion.getZ()), 5);
00096 pitch = MathHelper::roundDecimal(asin(-2
00097 * (quaternion.getX() * quaternion.getZ()
00098 - quaternion.getW() * quaternion.getY())), 5);
00099 roll = MathHelper::roundDecimal(atan2(2
00100 * (quaternion.getZ() * quaternion.getY()
00101 - quaternion.getW() * quaternion.getX()),
00102 quaternion.getW() * quaternion.getW() - quaternion.getX()
00103 * quaternion.getX() - quaternion.getY()
00104 * quaternion.getY() + quaternion.getZ()
00105 * quaternion.getZ()), 5);
00106
00107 break;
00108 case TransformationTypes::EULER312:
00109 yaw = MathHelper::roundDecimal(asin(2
00110 * (quaternion.getY() * quaternion.getZ()
00111 + quaternion.getX() * quaternion.getW())), 5);
00112 roll = MathHelper::roundDecimal(atan2(2
00113 * (-quaternion.getX() * quaternion.getZ()
00114 + quaternion.getY() * quaternion.getW()),
00115 quaternion.getZ() * quaternion.getZ() - quaternion.getY()
00116 * quaternion.getY() - quaternion.getX()
00117 * quaternion.getX() + quaternion.getW()
00118 * quaternion.getW()), 5);
00119 pitch = MathHelper::roundDecimal(atan2(2
00120 * (-quaternion.getX() * quaternion.getY()
00121 + quaternion.getZ() * quaternion.getW()),
00122 quaternion.getY() * quaternion.getY() - quaternion.getZ()
00123 * quaternion.getZ() + quaternion.getW()
00124 * quaternion.getW() - quaternion.getX()
00125 * quaternion.getX()), 5);
00126
00127 break;
00128 default:
00129 break;
00130 }
00131
00132 }
00133
00134 bool operator==(const Euler& aux) const {
00135 if (roll == aux.roll && yaw == aux.yaw && pitch == aux.pitch
00136 && eulerType == aux.eulerType)
00137 return true;
00138 else
00139 return false;
00140 }
00141
00142 Euler& operator=(const Euler& aux) {
00143 roll = aux.roll;
00144 pitch = aux.pitch;
00145 yaw = aux.yaw;
00146 eulerType = aux.eulerType;
00147
00148 return *this;
00149 }
00150
00151 friend std::ostream & operator <<(std::ostream & o, Euler e) {
00152 switch (e.eulerType) {
00153 case TransformationTypes::EULER123:
00154 o << "Euler123: [Roll: " << e.roll << ", Pitch: " << e.pitch
00155 << ", Yaw: " << e.yaw << "]";
00156 break;
00157 case TransformationTypes::EULER321:
00158 o << "Euler321: [Yaw: " << e.yaw << ", Pitch: " << e.pitch
00159 << ", Roll: " << e.roll << "]";
00160 break;
00161 case TransformationTypes::EULER312:
00162 o << "Euler312: [Yaw: " << e.yaw << ", roll: " << e.roll
00163 << ", Pich: " << e.pitch << "]";
00164 break;
00165 default:
00166 o << "Euler: [Roll: " << e.roll << ", Pitch: " << e.pitch
00167 << ", Yaw: " << e.yaw << "]";
00168 break;
00169 }
00170
00171 return o;
00172 }
00173
00174 double getPitch() const {
00175 return pitch;
00176 }
00177
00178 double getRoll() const {
00179 return roll;
00180 }
00181
00182 double getYaw() const {
00183 return yaw;
00184 }
00185
00186 TransformationTypes::EulerType getEulerType() const {
00187 return eulerType;
00188 }
00189 };
00190 }
00191
00192 #endif