TooN: so2.h Source File

TooN 2.0.0-beta8

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List

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