quaternion.hGo to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056 #ifndef QUATERNION_H
00057 #define QUATERNION_H
00058
00059
00060
00061
00062
00063
00064 #include "robot.h"
00065
00066 #ifdef use_namespace
00067 namespace ROBOOP {
00068 using namespace NEWMAT;
00069 #endif
00070
00071
00072 static const char header_quat_rcsid[] __UNUSED__ = "$Id: quaternion.h,v 1.5 2005/07/26 03:22:09 ejt Exp $";
00073
00074 #define BASE_FRAME 0
00075 #define BODY_FRAME 1
00076 #define EPSILON 0.0000001
00077
00078
00079
00080
00081
00082 class Quaternion
00083 {
00084 public:
00085 Quaternion();
00086 Quaternion(const Quaternion & q);
00087 Quaternion(const Real angle_in_rad, const ColumnVector & axis);
00088 Quaternion(const Real s, const Real v1, const Real v2,
00089 const Real v3);
00090 Quaternion(const Matrix & R);
00091
00092 Quaternion & operator=(const Quaternion & q);
00093 Quaternion operator+(const Quaternion & q)const;
00094 Quaternion operator-(const Quaternion & q)const;
00095 Quaternion operator*(const Quaternion & q)const;
00096 Quaternion operator*(const Real c)const;
00097 Quaternion operator/(const Quaternion & q)const;
00098 Quaternion operator/(const Real c)const;
00099 Quaternion conjugate()const;
00100
00101
00102 Quaternion i()const;
00103 Quaternion & unit();
00104 Quaternion exp() const;
00105 Quaternion power(const Real t) const;
00106 Quaternion Log() const;
00107
00108 Quaternion dot(const ColumnVector & w, const short sign)const;
00109 ReturnMatrix E(const short sign)const;
00110
00111 Real norm()const;
00112 Real dot_prod(const Quaternion & q)const;
00113 Real s()const { return s_; }
00114 void set_s(const Real s){ s_ = s; }
00115 ReturnMatrix v()const { return v_; }
00116 void set_v(const ColumnVector & v);
00117 ReturnMatrix R()const;
00118 ReturnMatrix T()const;
00119
00120 private:
00121 Real s_;
00122 ColumnVector v_;
00123 };
00124
00125
00126
00127 ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot);
00128
00129 short Integ_quat(Quaternion & dquat_present, Quaternion & dquat_past,
00130 Quaternion & quat, const Real dt);
00131 Real Integ_Trap_quat_s(const Quaternion & present, Quaternion & past,
00132 const Real dt);
00133 ReturnMatrix Integ_Trap_quat_v(const Quaternion & present, Quaternion & past,
00134 const Real dt);
00135
00136 Quaternion Slerp(const Quaternion & q0, const Quaternion & q1, const Real t);
00137 Quaternion Slerp_prime(const Quaternion & q0, const Quaternion & q1, const Real t);
00138
00139 Quaternion Squad(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00140 const Quaternion & q, const Real t);
00141 Quaternion Squad_prime(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00142 const Quaternion & q, const Real t);
00143
00144 #ifdef use_namespace
00145 }
00146 #endif
00147
00148 #endif
|