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 #ifndef WFMATH_QUATERNION_H
00027 #define WFMATH_QUATERNION_H
00028
00029 #include <wfmath/const.h>
00030 #include <wfmath/vector.h>
00031 #include <wfmath/rotmatrix.h>
00032
00033 namespace WFMath {
00034
00036
00040 class Quaternion
00041 {
00042 public:
00044 Quaternion () : m_valid(false) {}
00046
00049 Quaternion (const CoordType w_in, const CoordType x_in, const CoordType y_in,
00050 const CoordType z_in);
00052 Quaternion (int axis, const CoordType angle) {rotation(axis, angle);}
00054 Quaternion (const Vector<3>& axis, const CoordType angle) {rotation(axis, angle);}
00056
00059 Quaternion (const Vector<3>& axis) {rotation(axis);}
00061
00063
00064
00065 ~Quaternion() {}
00066
00067 friend std::ostream& operator<<(std::ostream& os, const Quaternion& p);
00068 friend std::istream& operator>>(std::istream& is, Quaternion& p);
00069
00071 Atlas::Message::Object toAtlas() const;
00073 void fromAtlas(const Atlas::Message::Object& a);
00074
00075 Quaternion& operator= (const Quaternion& rhs)
00076 {m_w = rhs.m_w; m_vec = rhs.m_vec; return *this;}
00077
00078
00079
00080 bool isEqualTo(const Quaternion &q, double epsilon = WFMATH_EPSILON) const;
00081
00082 bool operator== (const Quaternion& rhs) const {return isEqualTo(rhs);}
00083 bool operator!= (const Quaternion& rhs) const {return !isEqualTo(rhs);}
00084
00085 bool isValid() const {return m_valid;}
00086
00088 Quaternion& identity() {m_w = 1; m_vec.zero(); return *this;}
00089
00090 bool operator< (const Quaternion& rhs) const;
00091
00092
00093
00095 Quaternion operator* (const Quaternion& rhs) const;
00097 Quaternion operator/ (const Quaternion& rhs) const;
00099 Quaternion& operator*= (const Quaternion& rhs)
00100 {return *this = operator*(rhs);}
00102 Quaternion& operator/= (const Quaternion& rhs)
00103 {return *this = operator/(rhs);}
00104
00105
00106
00107
00109
00118 bool fromRotMatrix(const RotMatrix<3>& m);
00119
00121 Quaternion inverse() const;
00122
00124 Quaternion& rotation(int axis, const CoordType angle);
00126 Quaternion& rotation(const Vector<3>& axis, const CoordType angle);
00128
00131 Quaternion& rotation(const Vector<3>& axis);
00132
00133
00134 template<const int dim>
00135 friend Vector<3>& Vector<dim>::rotate(const Quaternion& q);
00136 template<const int dim>
00137 friend RotMatrix<3>& RotMatrix<dim>::fromQuaternion(const Quaternion& q,
00138 const bool not_flip);
00139
00141 CoordType scalar() const {return m_w;}
00143 const Vector<3>& vector() const {return m_vec;}
00144
00145 private:
00146 Quaternion(bool valid) : m_valid(valid) {}
00147 CoordType m_w;
00148 Vector<3> m_vec;
00149 bool m_valid;
00150 };
00151
00152 }
00153
00154 #endif // WFMATH_QUATERNION_H