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 #ifndef TOON_INCLUDE_SO2_H
00032 #define TOON_INCLUDE_SO2_H
00033
00034 #include <TooN/TooN.h>
00035 #include <TooN/helpers.h>
00036
00037 namespace TooN {
00038
00039 template<typename Precision> class SO2;
00040 template <typename Precision> class SE2;
00041
00042 template<typename Precision> inline std::istream & operator>>(std::istream &, SO2<Precision> & );
00043 template<typename Precision> inline std::istream & operator>>(std::istream &, SE2<Precision> & );
00044
00049 template<typename Precision = double>
00050 class SO2 {
00051 friend std::istream& operator>> <Precision>(std::istream&, SO2& );
00052 friend std::istream& operator>> <Precision>(std::istream&, SE2<Precision>& );
00053
00054 public:
00056 SO2() : my_matrix(Identity) {}
00057
00058 SO2(const Matrix<2,2,Precision>& rhs) { *this = rhs; }
00059
00060 SO2(const Precision l) { *this = exp(l); }
00061
00064 template <int R, int C, typename P, typename A>
00065 inline SO2& operator=(const Matrix<R,C,P,A>& rhs){
00066 my_matrix = rhs;
00067 coerce();
00068 return *this;
00069 }
00070
00072 void coerce(){
00073 my_matrix[0] = unit(my_matrix[0]);
00074 my_matrix[1] -= my_matrix[0] * (my_matrix[0]*my_matrix[1]);
00075 my_matrix[1] = unit(my_matrix[1]);
00076 }
00077
00079 inline static SO2 exp(const Precision & d){
00080 SO2<Precision> result;
00081 result.my_matrix[0][0] = result.my_matrix[1][1] = cos(d);
00082 result.my_matrix[1][0] = sin(d);
00083 result.my_matrix[0][1] = -result.my_matrix[1][0];
00084 return result;
00085 }
00086
00088 Precision ln() const { return atan2(my_matrix[1][0], my_matrix[0][0]); }
00089
00091 SO2 inverse() const { return SO2(*this, Invert()); }
00092
00094 SO2& operator *=(const SO2& rhs){
00095 my_matrix=my_matrix*rhs.my_matrix;
00096 return *this;
00097 }
00098
00100 SO2 operator *(const SO2& rhs) const { return SO2(*this,rhs); }
00101
00103 const Matrix<2,2,Precision>& get_matrix() const {return my_matrix;}
00104
00106 static Matrix<2,2,Precision> generator() {
00107 Matrix<2,2,Precision> result;
00108 result[0] = makeVector(0,-1);
00109 result[1] = makeVector(1,0);
00110 return result;
00111 }
00112
00113 private:
00114 struct Invert {};
00115 inline SO2(const SO2& so2, const Invert&) : my_matrix(so2.my_matrix.T()) {}
00116 inline SO2(const SO2& a, const SO2& b) : my_matrix(a.my_matrix*b.my_matrix) {}
00117
00118 Matrix<2,2,Precision> my_matrix;
00119 };
00120
00123 template <typename Precision>
00124 inline std::ostream& operator<< (std::ostream& os, const SO2<Precision> & rhs){
00125 return os << rhs.get_matrix();
00126 }
00127
00130 template <typename Precision>
00131 inline std::istream& operator>>(std::istream& is, SO2<Precision>& rhs){
00132 return is >> rhs.my_matrix;
00133 rhs.coerce();
00134 }
00135
00138 template<int D, typename P1, typename PV, typename Accessor>
00139 inline Vector<2, typename Internal::MultiplyType<P1, PV>::type> operator*(const SO2<P1> & lhs, const Vector<D, PV, Accessor> & rhs){
00140 return lhs.get_matrix() * rhs;
00141 }
00142
00145 template<int D, typename P1, typename PV, typename Accessor>
00146 inline Vector<2, typename Internal::MultiplyType<PV,P1>::type> operator*(const Vector<D, PV, Accessor>& lhs, const SO2<P1> & rhs){
00147 return lhs * rhs.get_matrix();
00148 }
00149
00152 template <int R, int C, typename P1, typename P2, typename Accessor>
00153 inline Matrix<2,C,typename Internal::MultiplyType<P1,P2>::type> operator*(const SO2<P1> & lhs, const Matrix<R,C,P2,Accessor>& rhs){
00154 return lhs.get_matrix() * rhs;
00155 }
00156
00159 template <int R, int C, typename P1, typename P2, typename Accessor>
00160 inline Matrix<R,2,typename Internal::MultiplyType<P1,P2>::type> operator*(const Matrix<R,C,P1,Accessor>& lhs, const SO2<P2>& rhs){
00161 return lhs * rhs.get_matrix();
00162 }
00163
00164 }
00165
00166 #endif