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 #include <cassert>
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
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
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
00063
00064 SL() : my_matrix(Identity) {}
00065
00066
00067 template <int S, typename P, typename B>
00068 SL( const Vector<S,P,B> & v ) { *this = exp(v); }
00069
00070
00071 template <int R, int C, typename P, typename A>
00072 SL(const Matrix<R,C,P,A>& M) : my_matrix(M) { coerce(); }
00073
00074
00075 const Matrix<N,N,Precision> & get_matrix() const { return my_matrix; }
00076
00077 SL inverse() const { return SL(*this, Invert()); }
00078
00079
00080 SL operator*( const SL & rhs) const { return SL(*this, rhs); }
00081
00082 SL operator*=( const SL & rhs) { *this = *this*rhs; return *this; }
00083
00084
00085
00086 template <int S, typename P, typename B>
00087 static inline SL exp( const Vector<S,P,B> &);
00088
00089
00090
00091
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 using std::pow;
00104 my_matrix /= pow(det, 1.0/N);
00105 }
00106
00107
00108
00109
00110 static const int COUNT_DIAG = N - 1;
00111 static const int COUNT_SYMM = (dim - COUNT_DIAG)/2;
00112 static const int COUNT_ASYMM = COUNT_SYMM;
00113 static const int DIAG_LIMIT = COUNT_DIAG;
00114 static const int SYMM_LIMIT = COUNT_SYMM + DIAG_LIMIT;
00115
00116
00117 Matrix<N,N,Precision> my_matrix;
00118 };
00119
00120 template <int N, typename Precision>
00121 template <int S, typename P, typename B>
00122 inline SL<N, Precision> SL<N, Precision>::exp( const Vector<S,P,B> & v){
00123 SizeMismatch<S,dim>::test(v.size(), dim);
00124 Matrix<N,N,Precision> t(Zeros);
00125 for(int i = 0; i < dim; ++i)
00126 t += generator(i) * v[i];
00127 SL<N, Precision> result;
00128 result.my_matrix = TooN::exp(t);
00129 return result;
00130 }
00131
00132 template <int N, typename Precision>
00133 inline Matrix<N,N,Precision> SL<N, Precision>::generator(int i){
00134 assert( i > -1 && i < dim );
00135 Matrix<N,N,Precision> result(Zeros);
00136 if(i < DIAG_LIMIT) {
00137 result(i,i) = 1;
00138 result(i+1,i+1) = -1;
00139 } else if(i < SYMM_LIMIT){
00140 int row = 0, col = i - DIAG_LIMIT + 1;
00141 while(col > (N - row - 1)){
00142 col -= (N - row - 1);
00143 ++row;
00144 }
00145 col += row;
00146 result(row, col) = result(col, row) = 1;
00147 } else {
00148 int row = 0, col = i - SYMM_LIMIT + 1;
00149 while(col > N - row - 1){
00150 col -= N - row - 1;
00151 ++row;
00152 }
00153 col += row;
00154 result(row, col) = -1;
00155 result(col, row) = 1;
00156 }
00157 return result;
00158 }
00159
00160 template <int S, typename PV, typename B, int N, typename P>
00161 Vector<N, typename Internal::MultiplyType<P, PV>::type> operator*( const SL<N, P> & lhs, const Vector<S,PV,B> & rhs ){
00162 return lhs.get_matrix() * rhs;
00163 }
00164
00165 template <int S, typename PV, typename B, int N, typename P>
00166 Vector<N, typename Internal::MultiplyType<PV, P>::type> operator*( const Vector<S,PV,B> & lhs, const SL<N,P> & rhs ){
00167 return lhs * rhs.get_matrix();
00168 }
00169
00170 template<int R, int C, typename PM, typename A, int N, typename P> inline
00171 Matrix<N, C, typename Internal::MultiplyType<P, PM>::type> operator*(const SL<N,P>& lhs, const Matrix<R, C, PM, A>& rhs){
00172 return lhs.get_matrix() * rhs;
00173 }
00174
00175 template<int R, int C, typename PM, typename A, int N, typename P> inline
00176 Matrix<R, N, typename Internal::MultiplyType<PM, P>::type> operator*(const Matrix<R, C, PM, A>& lhs, const SL<N,P>& rhs){
00177 return lhs * rhs.get_matrix();
00178 }
00179
00180 template <int N, typename P>
00181 std::ostream & operator<<(std::ostream & out, const SL<N, P> & h){
00182 out << h.get_matrix();
00183 return out;
00184 }
00185
00186 template <int N, typename P>
00187 std::istream & operator>>(std::istream & in, SL<N, P> & h){
00188 in >> h.my_matrix;
00189 h.coerce();
00190 return in;
00191 }
00192
00193 };
00194
00195 #endif