00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #pragma once
00014
00015 #ifndef __LIEMATRIX__
00016 #define __LIEMATRIX__
00017
00018
00019 #include <mtl/matrix.h>
00020 #include <mtl/mtl.h>
00021 #include <mtl/utils.h>
00022 #include <mtl/lu.h>
00023 #include "GroupElement.h"
00024 #include "libalgebra.h"
00025
00026
00028
00030 template <typename my_matrix_alg_type>
00031 class LieMatrix : public my_matrix_alg_type::mtlMatrix
00032 {
00033 typedef typename my_matrix_alg_type::SCA SCA;
00034 typedef typename my_matrix_alg_type::RAT RAT;
00035 typedef typename my_matrix_alg_type::mtlVector mtlVector;
00036 typedef typename my_matrix_alg_type::mtlMatrix mtlMatrix;
00037 typedef typename my_matrix_alg_type::mtlDiagMat mtlDiagMat;
00038
00039 static const unsigned myDIM = my_matrix_alg_type::myDIM;
00040
00041 public:
00043 LieMatrix() : mtlMatrix(myDIM,myDIM)
00044 {
00045 mtl::set_value((mtlMatrix &) *this, RAT(0));
00046 };
00047
00049 LieMatrix(const LieMatrix<my_matrix_alg_type> & m) : mtlMatrix(myDIM,myDIM)
00050 {
00051 mtl::copy(m,(mtlMatrix &)*this);
00052 };
00053
00055 ~LieMatrix(void) {};
00056
00058 LieMatrix<my_matrix_alg_type> & operator+=(const mtlMatrix & m)
00059 {
00060 mtl::add(m,(mtlMatrix &)*this);
00061 return *this;
00062 };
00063
00065 LieMatrix<my_matrix_alg_type> & operator *=(const SCA a)
00066 {
00067 mtl::scale((mtlMatrix &)*this,a);
00068 return *this;
00069 };
00070
00072 LieMatrix<my_matrix_alg_type> operator+(const LieMatrix<my_matrix_alg_type> & n) const
00073 {
00074 return (LieMatrix<my_matrix_alg_type>(*this)+=n);
00075 };
00076
00078 GroupElement<my_matrix_alg_type> exp(const SCA t = SCA(1)) const
00079 {
00080 SCA s = mtl::infinity_norm(*this) + mtl::one_norm(*this);
00081
00082
00083
00084 int mantissa;
00085 frexp( s*t, &mantissa );
00086
00087 if (mantissa < 0) mantissa=0;
00088 mantissa=mantissa+1;
00089
00090 SCA r1,r2;
00091 r1=ldexp(1.,mantissa);
00092 r2=ldexp(1.,-mantissa);
00093
00094 LieMatrix<my_matrix_alg_type> z(*this);
00095 z*=(t*r2);
00096 LieMatrix<my_matrix_alg_type> z_squared;
00097 mtl::mult(z,z,z_squared);
00098
00099
00100
00101 GroupElement<my_matrix_alg_type> numerator, denominator;
00102 numerator=pade_recurse(z,z_squared,4,top);
00103 denominator=pade_recurse(z,z_squared,4,bottom);
00104
00105
00106 mtlMatrix temp(myDIM,myDIM),padeexp(myDIM,myDIM),denom(myDIM,myDIM);
00107 mtl::copy((mtlMatrix &) denominator,denom);
00108 mtl::set_value(temp,RAT(0));
00109 mtl::set_value(padeexp,RAT(0));
00110 mtl::dense1D<int> pvector(myDIM);
00111 mtl::lu_factor(denom, pvector);
00112 mtl::lu_inverse(denom, pvector, temp);
00113 mtl::mult((mtlMatrix &) numerator,temp,padeexp);
00114 GroupElement<my_matrix_alg_type> exp;
00115 mtl::set_diagonal((mtlMatrix &) exp,RAT(0));
00116 mtl::copy(padeexp,(mtlMatrix &) exp);
00117
00118
00119 while (mantissa--)
00120 {
00121 GroupElement<my_matrix_alg_type> tem;
00122 tem=exp*exp;
00123 mtl::swap((mtlMatrix &) exp,(mtlMatrix &) tem);
00124 };
00125 return exp;
00126 };
00127
00128 static mtlMatrix& scaled_add ( mtlMatrix& B, SCA l, mtlMatrix & result)
00129 {
00130 mtl::add(mtl::scaled( B, l), result);
00131 return result;
00132 };
00133
00135 static LieMatrix<my_matrix_alg_type> Lie(const LieMatrix<my_matrix_alg_type> & A, const LieMatrix<my_matrix_alg_type> & B)
00136 {
00137 LieMatrix<my_matrix_alg_type> temp;
00138 mtl::mult((mtlMatrix &) A, (mtlMatrix &) B,(mtlMatrix &) temp);
00139 mtl::mult(mtl::scaled((mtlMatrix &) B,SCA(-1)), (mtlMatrix &) A, (mtlMatrix &) temp);
00140 return temp;
00141 };
00142
00143 private:
00144 enum flip{top=1,bottom=-1};
00145
00147 GroupElement<my_matrix_alg_type> pade_recurse(const LieMatrix<my_matrix_alg_type> & zz,const LieMatrix<my_matrix_alg_type> & zz_squared, unsigned int c,flip position) const
00148 {
00149 GroupElement<my_matrix_alg_type> flow0,flow1;
00150 mtl::add(mtl::scaled((mtlMatrix &)zz, SCA(position) * SCA(SCA(1)/SCA(2))) ,(mtlMatrix &) flow1);
00151 for (unsigned int count(1);count<c;count++)
00152 {
00153 GroupElement<my_matrix_alg_type> flow2;
00154 for(unsigned int i1(0); i1<myDIM; i1++)
00155 for(unsigned int i2(0); i2<myDIM; i2++)
00156 {
00157 flow2[i1][i2]=flow1[i1][i2];
00158 }
00159 mtl::mult(
00160 mtl::scaled(
00161 zz_squared
00162 ,SCA(1)/(SCA(16)*SCA(count)*SCA(count)- SCA(4))
00163 )
00164 ,(mtlMatrix &) flow0
00165 ,(mtlMatrix &) flow2
00166 );
00167
00168 for(unsigned int i1(0); i1<myDIM; i1++)
00169 for(unsigned int i2(0); i2<myDIM; i2++)
00170 {
00171 flow0[i1][i2]=flow1[i1][i2];
00172 flow1[i1][i2]=flow2[i1][i2];
00173 }
00174 }
00175 return flow1;
00176 };
00177
00178 };
00179
00180 #endif // __LIEMATRIX__