so2.h

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 
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

Generated on Thu May 7 20:28:41 2009 for TooN by  doxygen 1.5.3