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_SL_H
00032 #define TOON_INCLUDE_SL_H
00033
00034 #include <TooN/TooN.h>
00035 #include <TooN/helpers.h>
00036 #include <TooN/LU.h>
00037
00038 namespace TooN {
00039
00040 template <int N, typename P> class SL;
00041 template <int N, typename P> std::istream & operator>>(std::istream &, SL<N, P> &);
00042
00056 template <int N, typename Precision = double>
00057 class SL {
00058 friend std::istream & operator>> <N,Precision>(std::istream &, SL &);
00059 public:
00060 static const int size = N;
00061 static const int dim = N*N - 1;
00062
00064 SL() : my_matrix(Identity) {}
00065
00067 template <int S, typename P, typename B>
00068 SL( const Vector<S,P,B> & v ) { *this = exp(v); }
00069
00071 template <int R, int C, typename P, typename A>
00072 SL(Matrix<R,C,P,A>& M) : my_matrix(M) { coerce(); }
00073
00075 const Matrix<N,N,Precision> & get_matrix() const { return my_matrix; }
00077 SL inverse() const { return SL(*this, Invert()); }
00078
00080 SL operator*( const SL & rhs) const { return SL(*this, rhs); }
00082 SL operator*=( const SL & rhs) { *this = *this*rhs; return *this; }
00083
00086 template <int S, typename P, typename B>
00087 static inline SL exp( const Vector<S,P,B> &);
00088
00092 static inline Matrix<N,N,Precision> generator(int);
00093
00094 private:
00095 struct Invert {};
00096 SL( const SL & from, struct Invert ) : my_matrix(LU<N>(from.get_matrix()).get_inverse()) {}
00097 SL( const SL & a, const SL & b) : my_matrix(a.get_matrix() * b.get_matrix()) {}
00098
00099 void coerce(){
00100 using std::abs;
00101 Precision det = LU<N>(my_matrix).determinant();
00102 assert(abs(det) > 0);
00103 my_matrix /= det;
00104 }
00105
00109 static const int COUNT_DIAG = N - 1;
00110 static const int COUNT_SYMM = (dim - COUNT_DIAG)/2;
00111 static const int COUNT_ASYMM = COUNT_SYMM;
00112 static const int DIAG_LIMIT = COUNT_DIAG;
00113 static const int SYMM_LIMIT = COUNT_SYMM + DIAG_LIMIT;
00115
00116 Matrix<N,N,Precision> my_matrix;
00117 };
00118
00119 template <int N, typename Precision>
00120 template <int S, typename P, typename B>
00121 inline SL<N, Precision> SL<N, Precision>::exp( const Vector<S,P,B> & v){
00122 SizeMismatch<S,dim>::test(v.size(), dim);
00123 Matrix<N,N,Precision> t(Zeros);
00124 for(int i = 0; i < dim; ++i)
00125 t += generator(i) * v[i];
00126 SL<N, Precision> result;
00127 result.my_matrix = TooN::exp(t);
00128 return result;
00129 }
00130
00131 template <int N, typename Precision>
00132 inline Matrix<N,N,Precision> SL<N, Precision>::generator(int i){
00133 assert( i > -1 && i < dim );
00134 Matrix<N,N,Precision> result(Zeros);
00135 if(i < DIAG_LIMIT) {
00136 result(i,i) = 1;
00137 result(i+1,i+1) = -1;
00138 } else if(i < SYMM_LIMIT){
00139 int row = 0, col = i - DIAG_LIMIT + 1;
00140 while(col > (N - row - 1)){
00141 col -= (N - row - 1);
00142 ++row;
00143 }
00144 col += row;
00145 result(row, col) = result(col, row) = 1;
00146 } else {
00147 int row = 0, col = i - SYMM_LIMIT + 1;
00148 while(col > N - row - 1){
00149 col -= N - row - 1;
00150 ++row;
00151 }
00152 col += row;
00153 result(row, col) = -1;
00154 result(col, row) = 1;
00155 }
00156 return result;
00157 }
00158
00159 template <int S, typename PV, typename B, int N, typename P>
00160 Vector<N, typename Internal::MultiplyType<P, PV>::type> operator*( const SL<N, P> & lhs, const Vector<S,PV,B> & rhs ){
00161 return lhs.get_matrix() * rhs;
00162 }
00163
00164 template <int S, typename PV, typename B, int N, typename P>
00165 Vector<N, typename Internal::MultiplyType<PV, P>::type> operator*( const Vector<S,PV,B> & lhs, const SL<N,P> & rhs ){
00166 return lhs * rhs.get_matrix();
00167 }
00168
00169 template<int R, int C, typename PM, typename A, int N, typename P> inline
00170 Matrix<N, C, typename Internal::MultiplyType<P, PM>::type> operator*(const SL<N,P>& lhs, const Matrix<R, C, PM, A>& rhs){
00171 return lhs.get_matrix() * rhs;
00172 }
00173
00174 template<int R, int C, typename PM, typename A, int N, typename P> inline
00175 Matrix<R, N, typename Internal::MultiplyType<PM, P>::type> operator*(const Matrix<R, C, PM, A>& lhs, const SL<N,P>& rhs){
00176 return lhs * rhs.get_matrix();
00177 }
00178
00179 template <int N, typename P>
00180 std::ostream & operator<<(std::ostream & out, const SL<N, P> & h){
00181 out << h.get_matrix();
00182 return out;
00183 }
00184
00185 template <int N, typename P>
00186 std::istream & operator>>(std::istream & in, SL<N, P> & h){
00187 in >> h.my_matrix;
00188 h.coerce();
00189 return in;
00190 }
00191
00192 };
00193
00194 #endif