GCOP
1.0
|
00001 #ifndef GCOP_QUAT_H 00002 #define GCOP_QUAT_H 00003 00004 #include <stdio.h> 00005 #include <iostream> 00006 00007 namespace gcop { 00008 00020 class Quat { 00021 public: 00025 Quat(); 00026 00027 00032 Quat(const Quat &q); 00033 00034 00039 Quat(const double q[4]); 00040 00041 00049 Quat(double w, double x, double y, double z); 00050 00051 00052 virtual ~Quat(); 00053 00054 00064 static double* Rotate(double c[3], 00065 const double v[3], 00066 const double u[3], 00067 double w); 00068 00069 double* Rotate2(double c[3], const double a[3]) const; 00070 double* Rotate2(double c[3]) const; 00071 00072 00079 double* Rotate(double c[3], const double v[3]) const; 00080 00081 00087 double* Rotate(double v[3]) const; 00088 00089 00095 void Transform(const double e[3]); 00096 00097 00103 void Transform(const double u[3], double v); 00104 00105 00109 void Invert(); 00110 00111 00116 void Invert(Quat &q) const; 00117 00118 00122 void Identity(); 00123 00124 00128 void Normalize(); 00129 00130 00136 Quat& operator*=(const Quat &q); 00137 00138 00143 void FromExp(const double e[3]); 00144 00145 00150 void ToExp(double e[3]) const; 00151 00152 00158 void FromAxis(const double u[3], const double v); 00159 00160 00166 void ToAxis(double u[3], double &v) const; 00167 00168 00173 void FromSE3(const double m[16]); 00174 00175 00180 void ToSE3(double m[16]) const; 00181 00182 00187 void FromRpy(const double rpy[3]); 00188 00189 00194 void ToRpy(double rpy[3]) const; 00195 00196 00202 double* Q(double q[4]) const; 00203 00204 00208 bool IsIdentity() const; 00209 00210 00215 void SetTol(double tol); 00216 00217 double qw, qx, qy, qz; 00218 00219 protected: 00220 double tol; 00221 00222 private: 00223 friend const Quat operator*(const Quat& q0, const Quat& q1); 00224 friend std::ostream& operator<<(std::ostream &os, const Quat &q); 00225 friend std::istream& operator>>(std::istream &is, Quat &q); 00226 }; 00227 00234 const Quat operator*(const Quat& q0, const Quat& q1); 00235 00242 std::ostream& operator<<(std::ostream &os, const gcop::Quat &q); 00243 00250 std::istream& operator>>(std::istream &is, gcop::Quat &q); 00251 00252 } 00253 00254 #endif