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
00045
00046
00047
00048
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:
00055
00056 SO2() : my_matrix(Identity) {}
00057
00058
00059 SO2(const Matrix<2,2,Precision>& rhs) {
00060 *this = rhs;
00061 coerce();
00062 }
00063
00064
00065 SO2(const Precision l) { *this = exp(l); }
00066
00067
00068
00069 template <int R, int C, typename P, typename A>
00070 SO2& operator=(const Matrix<R,C,P,A>& rhs){
00071 my_matrix = rhs;
00072 coerce();
00073 return *this;
00074 }
00075
00076
00077 void coerce(){
00078 my_matrix[0] = unit(my_matrix[0]);
00079 my_matrix[1] -= my_matrix[0] * (my_matrix[0]*my_matrix[1]);
00080 my_matrix[1] = unit(my_matrix[1]);
00081 }
00082
00083
00084 inline static SO2 exp(const Precision & d){
00085 SO2<Precision> result;
00086 result.my_matrix[0][0] = result.my_matrix[1][1] = cos(d);
00087 result.my_matrix[1][0] = sin(d);
00088 result.my_matrix[0][1] = -result.my_matrix[1][0];
00089 return result;
00090 }
00091
00092
00093 Precision ln() const { return atan2(my_matrix[1][0], my_matrix[0][0]); }
00094
00095
00096 SO2 inverse() const { return SO2(*this, Invert()); }
00097
00098
00099 template <typename P>
00100 SO2& operator *=(const SO2<P>& rhs){
00101 my_matrix=my_matrix*rhs.get_matrix();
00102 return *this;
00103 }
00104
00105
00106 template <typename P>
00107 SO2<typename Internal::MultiplyType<Precision, P>::type> operator *(const SO2<P>& rhs) const {
00108 return SO2<typename Internal::MultiplyType<Precision, P>::type>(*this,rhs);
00109 }
00110
00111
00112 const Matrix<2,2,Precision>& get_matrix() const {return my_matrix;}
00113
00114
00115 static Matrix<2,2,Precision> generator() {
00116 Matrix<2,2,Precision> result;
00117 result[0] = makeVector(0,-1);
00118 result[1] = makeVector(1,0);
00119 return result;
00120 }
00121
00122 private:
00123 struct Invert {};
00124 inline SO2(const SO2& so2, const Invert&) : my_matrix(so2.my_matrix.T()) {}
00125 template <typename PA, typename PB>
00126 inline SO2(const SO2<PA>& a, const SO2<PB>& b) : my_matrix(a.get_matrix()*b.get_matrix()) {}
00127
00128 Matrix<2,2,Precision> my_matrix;
00129 };
00130
00131
00132
00133 template <typename Precision>
00134 inline std::ostream& operator<< (std::ostream& os, const SO2<Precision> & rhs){
00135 return os << rhs.get_matrix();
00136 }
00137
00138
00139
00140 template <typename Precision>
00141 inline std::istream& operator>>(std::istream& is, SO2<Precision>& rhs){
00142 return is >> rhs.my_matrix;
00143 rhs.coerce();
00144 }
00145
00146
00147
00148 template<int D, typename P1, typename PV, typename Accessor>
00149 inline Vector<2, typename Internal::MultiplyType<P1, PV>::type> operator*(const SO2<P1> & lhs, const Vector<D, PV, Accessor> & rhs){
00150 return lhs.get_matrix() * rhs;
00151 }
00152
00153
00154
00155 template<int D, typename P1, typename PV, typename Accessor>
00156 inline Vector<2, typename Internal::MultiplyType<PV,P1>::type> operator*(const Vector<D, PV, Accessor>& lhs, const SO2<P1> & rhs){
00157 return lhs * rhs.get_matrix();
00158 }
00159
00160
00161
00162 template <int R, int C, typename P1, typename P2, typename Accessor>
00163 inline Matrix<2,C,typename Internal::MultiplyType<P1,P2>::type> operator*(const SO2<P1> & lhs, const Matrix<R,C,P2,Accessor>& rhs){
00164 return lhs.get_matrix() * rhs;
00165 }
00166
00167
00168
00169 template <int R, int C, typename P1, typename P2, typename Accessor>
00170 inline Matrix<R,2,typename Internal::MultiplyType<P1,P2>::type> operator*(const Matrix<R,C,P1,Accessor>& lhs, const SO2<P2>& rhs){
00171 return lhs * rhs.get_matrix();
00172 }
00173
00174 }
00175
00176 #endif