GCOP  1.0
quat.h
Go to the documentation of this file.
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