TooN 2.1
|
00001 // -*- c++ -*- 00002 00003 // Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk), 00004 // Ed Rosten (er258@cam.ac.uk), Gerhard Reitmayr (gr281@cam.ac.uk) 00005 // 00006 // This file is part of the TooN Library. This library is free 00007 // software; you can redistribute it and/or modify it under the 00008 // terms of the GNU General Public License as published by the 00009 // Free Software Foundation; either version 2, or (at your option) 00010 // any later version. 00011 00012 // This library is distributed in the hope that it will be useful, 00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00015 // GNU General Public License for more details. 00016 00017 // You should have received a copy of the GNU General Public License along 00018 // with this library; see the file COPYING. If not, write to the Free 00019 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, 00020 // USA. 00021 00022 // As a special exception, you may use this file as part of a free software 00023 // library without restriction. Specifically, if other files instantiate 00024 // templates or use macros or inline functions from this file, or you compile 00025 // this file and link it with other files to produce an executable, this 00026 // file does not by itself cause the resulting executable to be covered by 00027 // the GNU General Public License. This exception does not however 00028 // invalidate any other reasons why the executable file might be covered by 00029 // the GNU General Public License. 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 template <typename Precision> class SIM2; 00042 00043 template<typename Precision> inline std::istream & operator>>(std::istream &, SO2<Precision> & ); 00044 template<typename Precision> inline std::istream & operator>>(std::istream &, SE2<Precision> & ); 00045 template<typename Precision> inline std::istream & operator>>(std::istream &, SIM2<Precision> & ); 00046 00047 /// Class to represent a two-dimensional rotation matrix. Two-dimensional rotation 00048 /// matrices are members of the Special Orthogonal Lie group SO2. This group can be parameterised with 00049 /// one number (the rotation angle). 00050 /// @ingroup gTransforms 00051 template<typename Precision = DefaultPrecision> 00052 class SO2 { 00053 friend std::istream& operator>> <Precision>(std::istream&, SO2& ); 00054 friend std::istream& operator>> <Precision>(std::istream&, SE2<Precision>& ); 00055 friend std::istream& operator>> <Precision>(std::istream&, SIM2<Precision>& ); 00056 00057 public: 00058 /// Default constructor. Initialises the matrix to the identity (no rotation) 00059 SO2() : my_matrix(Identity) {} 00060 00061 /// Construct from a rotation matrix. 00062 SO2(const Matrix<2,2,Precision>& rhs) { 00063 *this = rhs; 00064 coerce(); 00065 } 00066 00067 /// Construct from an angle. 00068 SO2(const Precision l) { *this = exp(l); } 00069 00070 /// Assigment operator from a general matrix. This also calls coerce() 00071 /// to make sure that the matrix is a valid rotation matrix. 00072 template <int R, int C, typename P, typename A> 00073 SO2& operator=(const Matrix<R,C,P,A>& rhs){ 00074 my_matrix = rhs; 00075 coerce(); 00076 return *this; 00077 } 00078 00079 /// Modifies the matrix to make sure it is a valid rotation matrix. 00080 void coerce(){ 00081 my_matrix[0] = unit(my_matrix[0]); 00082 my_matrix[1] -= my_matrix[0] * (my_matrix[0]*my_matrix[1]); 00083 my_matrix[1] = unit(my_matrix[1]); 00084 } 00085 00086 /// Exponentiate an angle in the Lie algebra to generate a new SO2. 00087 inline static SO2 exp(const Precision & d){ 00088 SO2<Precision> result; 00089 result.my_matrix[0][0] = result.my_matrix[1][1] = cos(d); 00090 result.my_matrix[1][0] = sin(d); 00091 result.my_matrix[0][1] = -result.my_matrix[1][0]; 00092 return result; 00093 } 00094 00095 /// extracts the rotation angle from the SO2 00096 Precision ln() const { return atan2(my_matrix[1][0], my_matrix[0][0]); } 00097 00098 /// Returns the inverse of this matrix (=the transpose, so this is a fast operation) 00099 SO2 inverse() const { return SO2(*this, Invert()); } 00100 00101 /// Self right-multiply by another rotation matrix 00102 template <typename P> 00103 SO2& operator *=(const SO2<P>& rhs){ 00104 my_matrix=my_matrix*rhs.get_matrix(); 00105 return *this; 00106 } 00107 00108 /// Right-multiply by another rotation matrix 00109 template <typename P> 00110 SO2<typename Internal::MultiplyType<Precision, P>::type> operator *(const SO2<P>& rhs) const { 00111 return SO2<typename Internal::MultiplyType<Precision, P>::type>(*this,rhs); 00112 } 00113 00114 /// Returns the SO2 as a Matrix<2> 00115 const Matrix<2,2,Precision>& get_matrix() const {return my_matrix;} 00116 00117 /// returns generator matrix 00118 static Matrix<2,2,Precision> generator() { 00119 Matrix<2,2,Precision> result; 00120 result[0] = makeVector(0,-1); 00121 result[1] = makeVector(1,0); 00122 return result; 00123 } 00124 00125 private: 00126 struct Invert {}; 00127 inline SO2(const SO2& so2, const Invert&) : my_matrix(so2.my_matrix.T()) {} 00128 template <typename PA, typename PB> 00129 inline SO2(const SO2<PA>& a, const SO2<PB>& b) : my_matrix(a.get_matrix()*b.get_matrix()) {} 00130 00131 Matrix<2,2,Precision> my_matrix; 00132 }; 00133 00134 /// Write an SO2 to a stream 00135 /// @relates SO2 00136 template <typename Precision> 00137 inline std::ostream& operator<< (std::ostream& os, const SO2<Precision> & rhs){ 00138 return os << rhs.get_matrix(); 00139 } 00140 00141 /// Read from SO2 to a stream 00142 /// @relates SO2 00143 template <typename Precision> 00144 inline std::istream& operator>>(std::istream& is, SO2<Precision>& rhs){ 00145 is >> rhs.my_matrix; 00146 rhs.coerce(); 00147 return is; 00148 } 00149 00150 /// Right-multiply by a Vector 00151 /// @relates SO2 00152 template<int D, typename P1, typename PV, typename Accessor> 00153 inline Vector<2, typename Internal::MultiplyType<P1, PV>::type> operator*(const SO2<P1> & lhs, const Vector<D, PV, Accessor> & rhs){ 00154 return lhs.get_matrix() * rhs; 00155 } 00156 00157 /// Left-multiply by a Vector 00158 /// @relates SO2 00159 template<int D, typename P1, typename PV, typename Accessor> 00160 inline Vector<2, typename Internal::MultiplyType<PV,P1>::type> operator*(const Vector<D, PV, Accessor>& lhs, const SO2<P1> & rhs){ 00161 return lhs * rhs.get_matrix(); 00162 } 00163 00164 /// Right-multiply by a Matrix 00165 /// @relates SO2 00166 template <int R, int C, typename P1, typename P2, typename Accessor> 00167 inline Matrix<2,C,typename Internal::MultiplyType<P1,P2>::type> operator*(const SO2<P1> & lhs, const Matrix<R,C,P2,Accessor>& rhs){ 00168 return lhs.get_matrix() * rhs; 00169 } 00170 00171 /// Left-multiply by a Matrix 00172 /// @relates SO2 00173 template <int R, int C, typename P1, typename P2, typename Accessor> 00174 inline Matrix<R,2,typename Internal::MultiplyType<P1,P2>::type> operator*(const Matrix<R,C,P1,Accessor>& lhs, const SO2<P2>& rhs){ 00175 return lhs * rhs.get_matrix(); 00176 } 00177 00178 } 00179 00180 #endif