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 #ifndef EIGEN_MATRIX_LOGARITHM
00027 #define EIGEN_MATRIX_LOGARITHM
00028
00029 #ifndef M_PI
00030 #define M_PI 3.141592653589793238462643383279503L
00031 #endif
00032
00033 namespace Eigen {
00034
00045 template <typename MatrixType>
00046 class MatrixLogarithmAtomic
00047 {
00048 public:
00049
00050 typedef typename MatrixType::Scalar Scalar;
00051
00052 typedef typename NumTraits<Scalar>::Real RealScalar;
00053
00054
00055
00057 MatrixLogarithmAtomic() { }
00058
00063 MatrixType compute(const MatrixType& A);
00064
00065 private:
00066
00067 void compute2x2(const MatrixType& A, MatrixType& result);
00068 void computeBig(const MatrixType& A, MatrixType& result);
00069 static Scalar atanh(Scalar x);
00070 int getPadeDegree(float normTminusI);
00071 int getPadeDegree(double normTminusI);
00072 int getPadeDegree(long double normTminusI);
00073 void computePade(MatrixType& result, const MatrixType& T, int degree);
00074 void computePade3(MatrixType& result, const MatrixType& T);
00075 void computePade4(MatrixType& result, const MatrixType& T);
00076 void computePade5(MatrixType& result, const MatrixType& T);
00077 void computePade6(MatrixType& result, const MatrixType& T);
00078 void computePade7(MatrixType& result, const MatrixType& T);
00079 void computePade8(MatrixType& result, const MatrixType& T);
00080 void computePade9(MatrixType& result, const MatrixType& T);
00081 void computePade10(MatrixType& result, const MatrixType& T);
00082 void computePade11(MatrixType& result, const MatrixType& T);
00083
00084 static const int minPadeDegree = 3;
00085 static const int maxPadeDegree = std::numeric_limits<RealScalar>::digits<= 24? 5:
00086 std::numeric_limits<RealScalar>::digits<= 53? 7:
00087 std::numeric_limits<RealScalar>::digits<= 64? 8:
00088 std::numeric_limits<RealScalar>::digits<=106? 10: 11;
00089
00090
00091 MatrixLogarithmAtomic(const MatrixLogarithmAtomic&);
00092 MatrixLogarithmAtomic& operator=(const MatrixLogarithmAtomic&);
00093 };
00094
00096 template <typename MatrixType>
00097 MatrixType MatrixLogarithmAtomic<MatrixType>::compute(const MatrixType& A)
00098 {
00099 using std::log;
00100 MatrixType result(A.rows(), A.rows());
00101 if (A.rows() == 1)
00102 result(0,0) = log(A(0,0));
00103 else if (A.rows() == 2)
00104 compute2x2(A, result);
00105 else
00106 computeBig(A, result);
00107 return result;
00108 }
00109
00111 template <typename MatrixType>
00112 typename MatrixType::Scalar MatrixLogarithmAtomic<MatrixType>::atanh(typename MatrixType::Scalar x)
00113 {
00114 using std::abs;
00115 using std::sqrt;
00116 if (abs(x) > sqrt(NumTraits<Scalar>::epsilon()))
00117 return Scalar(0.5) * log((Scalar(1) + x) / (Scalar(1) - x));
00118 else
00119 return x + x*x*x / Scalar(3);
00120 }
00121
00123 template <typename MatrixType>
00124 void MatrixLogarithmAtomic<MatrixType>::compute2x2(const MatrixType& A, MatrixType& result)
00125 {
00126 using std::abs;
00127 using std::ceil;
00128 using std::imag;
00129 using std::log;
00130
00131 Scalar logA00 = log(A(0,0));
00132 Scalar logA11 = log(A(1,1));
00133
00134 result(0,0) = logA00;
00135 result(1,0) = Scalar(0);
00136 result(1,1) = logA11;
00137
00138 if (A(0,0) == A(1,1)) {
00139 result(0,1) = A(0,1) / A(0,0);
00140 } else if ((abs(A(0,0)) < 0.5*abs(A(1,1))) || (abs(A(0,0)) > 2*abs(A(1,1)))) {
00141 result(0,1) = A(0,1) * (logA11 - logA00) / (A(1,1) - A(0,0));
00142 } else {
00143
00144 int unwindingNumber = static_cast<int>(ceil((imag(logA11 - logA00) - M_PI) / (2*M_PI)));
00145 Scalar z = (A(1,1) - A(0,0)) / (A(1,1) + A(0,0));
00146 result(0,1) = A(0,1) * (Scalar(2) * atanh(z) + Scalar(0,2*M_PI*unwindingNumber)) / (A(1,1) - A(0,0));
00147 }
00148 }
00149
00152 template <typename MatrixType>
00153 void MatrixLogarithmAtomic<MatrixType>::computeBig(const MatrixType& A, MatrixType& result)
00154 {
00155 int numberOfSquareRoots = 0;
00156 int numberOfExtraSquareRoots = 0;
00157 int degree;
00158 MatrixType T = A;
00159 const RealScalar maxNormForPade = maxPadeDegree<= 5? 5.3149729967117310e-1:
00160 maxPadeDegree<= 7? 2.6429608311114350e-1:
00161 maxPadeDegree<= 8? 2.32777776523703892094e-1L:
00162 maxPadeDegree<=10? 1.05026503471351080481093652651105e-1L:
00163 1.1880960220216759245467951592883642e-1L;
00164
00165 while (true) {
00166 RealScalar normTminusI = (T - MatrixType::Identity(T.rows(), T.rows())).cwiseAbs().colwise().sum().maxCoeff();
00167 if (normTminusI < maxNormForPade) {
00168 degree = getPadeDegree(normTminusI);
00169 int degree2 = getPadeDegree(normTminusI / RealScalar(2));
00170 if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1))
00171 break;
00172 ++numberOfExtraSquareRoots;
00173 }
00174 MatrixType sqrtT;
00175 MatrixSquareRootTriangular<MatrixType>(T).compute(sqrtT);
00176 T = sqrtT;
00177 ++numberOfSquareRoots;
00178 }
00179
00180 computePade(result, T, degree);
00181 result *= pow(RealScalar(2), numberOfSquareRoots);
00182 }
00183
00184
00185 template <typename MatrixType>
00186 int MatrixLogarithmAtomic<MatrixType>::getPadeDegree(float normTminusI)
00187 {
00188 const float maxNormForPade[] = { 2.5111573934555054e-1 , 4.0535837411880493e-1,
00189 5.3149729967117310e-1 };
00190 for (int degree = 3; degree <= maxPadeDegree; ++degree)
00191 if (normTminusI <= maxNormForPade[degree - minPadeDegree])
00192 return degree;
00193 assert(false);
00194 }
00195
00196
00197 template <typename MatrixType>
00198 int MatrixLogarithmAtomic<MatrixType>::getPadeDegree(double normTminusI)
00199 {
00200 const double maxNormForPade[] = { 1.6206284795015624e-2 , 5.3873532631381171e-2,
00201 1.1352802267628681e-1, 1.8662860613541288e-1, 2.642960831111435e-1 };
00202 for (int degree = 3; degree <= maxPadeDegree; ++degree)
00203 if (normTminusI <= maxNormForPade[degree - minPadeDegree])
00204 return degree;
00205 assert(false);
00206 }
00207
00208
00209 template <typename MatrixType>
00210 int MatrixLogarithmAtomic<MatrixType>::getPadeDegree(long double normTminusI)
00211 {
00212 #if LDBL_MANT_DIG == 53 // double precision
00213 const double maxNormForPade[] = { 1.6206284795015624e-2 , 5.3873532631381171e-2,
00214 1.1352802267628681e-1, 1.8662860613541288e-1, 2.642960831111435e-1 };
00215 #elif LDBL_MANT_DIG <= 64 // extended precision
00216 const double maxNormForPade[] = { 5.48256690357782863103e-3 , 2.34559162387971167321e-2,
00217 5.84603923897347449857e-2, 1.08486423756725170223e-1, 1.68385767881294446649e-1,
00218 2.32777776523703892094e-1 };
00219 #elif LDBL_MANT_DIG <= 106 // double-double
00220 const double maxNormForPade[] = { 8.58970550342939562202529664318890e-5 ,
00221 9.34074328446359654039446552677759e-4, 4.26117194647672175773064114582860e-3,
00222 1.21546224740281848743149666560464e-2, 2.61100544998339436713088248557444e-2,
00223 4.66170074627052749243018566390567e-2, 7.32585144444135027565872014932387e-2,
00224 1.05026503471351080481093652651105e-1 };
00225 #else // quadruple precision
00226 const double maxNormForPade[] = { 4.7419931187193005048501568167858103e-5 ,
00227 5.8853168473544560470387769480192666e-4, 2.9216120366601315391789493628113520e-3,
00228 8.8415758124319434347116734705174308e-3, 1.9850836029449446668518049562565291e-2,
00229 3.6688019729653446926585242192447447e-2, 5.9290962294020186998954055264528393e-2,
00230 8.6998436081634343903250580992127677e-2, 1.1880960220216759245467951592883642e-1 };
00231 #endif
00232 for (int degree = 3; degree <= maxPadeDegree; ++degree)
00233 if (normTminusI <= maxNormForPade[degree - minPadeDegree])
00234 return degree;
00235 assert(false);
00236 }
00237
00238
00239 template <typename MatrixType>
00240 void MatrixLogarithmAtomic<MatrixType>::computePade(MatrixType& result, const MatrixType& T, int degree)
00241 {
00242 switch (degree) {
00243 case 3: computePade3(result, T); break;
00244 case 4: computePade4(result, T); break;
00245 case 5: computePade5(result, T); break;
00246 case 6: computePade6(result, T); break;
00247 case 7: computePade7(result, T); break;
00248 case 8: computePade8(result, T); break;
00249 case 9: computePade9(result, T); break;
00250 case 10: computePade10(result, T); break;
00251 case 11: computePade11(result, T); break;
00252 default: assert(false);
00253 }
00254 }
00255
00256 template <typename MatrixType>
00257 void MatrixLogarithmAtomic<MatrixType>::computePade3(MatrixType& result, const MatrixType& T)
00258 {
00259 const int degree = 3;
00260 const RealScalar nodes[] = { 0.1127016653792583114820734600217600L, 0.5000000000000000000000000000000000L,
00261 0.8872983346207416885179265399782400L };
00262 const RealScalar weights[] = { 0.2777777777777777777777777777777778L, 0.4444444444444444444444444444444444L,
00263 0.2777777777777777777777777777777778L };
00264 assert(degree <= maxPadeDegree);
00265 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
00266 result.setZero(T.rows(), T.rows());
00267 for (int k = 0; k < degree; ++k)
00268 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
00269 .template triangularView<Upper>().solve(TminusI);
00270 }
00271
00272 template <typename MatrixType>
00273 void MatrixLogarithmAtomic<MatrixType>::computePade4(MatrixType& result, const MatrixType& T)
00274 {
00275 const int degree = 4;
00276 const RealScalar nodes[] = { 0.0694318442029737123880267555535953L, 0.3300094782075718675986671204483777L,
00277 0.6699905217924281324013328795516223L, 0.9305681557970262876119732444464048L };
00278 const RealScalar weights[] = { 0.1739274225687269286865319746109997L, 0.3260725774312730713134680253890003L,
00279 0.3260725774312730713134680253890003L, 0.1739274225687269286865319746109997L };
00280 assert(degree <= maxPadeDegree);
00281 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
00282 result.setZero(T.rows(), T.rows());
00283 for (int k = 0; k < degree; ++k)
00284 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
00285 .template triangularView<Upper>().solve(TminusI);
00286 }
00287
00288 template <typename MatrixType>
00289 void MatrixLogarithmAtomic<MatrixType>::computePade5(MatrixType& result, const MatrixType& T)
00290 {
00291 const int degree = 5;
00292 const RealScalar nodes[] = { 0.0469100770306680036011865608503035L, 0.2307653449471584544818427896498956L,
00293 0.5000000000000000000000000000000000L, 0.7692346550528415455181572103501044L,
00294 0.9530899229693319963988134391496965L };
00295 const RealScalar weights[] = { 0.1184634425280945437571320203599587L, 0.2393143352496832340206457574178191L,
00296 0.2844444444444444444444444444444444L, 0.2393143352496832340206457574178191L,
00297 0.1184634425280945437571320203599587L };
00298 assert(degree <= maxPadeDegree);
00299 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
00300 result.setZero(T.rows(), T.rows());
00301 for (int k = 0; k < degree; ++k)
00302 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
00303 .template triangularView<Upper>().solve(TminusI);
00304 }
00305
00306 template <typename MatrixType>
00307 void MatrixLogarithmAtomic<MatrixType>::computePade6(MatrixType& result, const MatrixType& T)
00308 {
00309 const int degree = 6;
00310 const RealScalar nodes[] = { 0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L,
00311 0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L,
00312 0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L };
00313 const RealScalar weights[] = { 0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L,
00314 0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L,
00315 0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L };
00316 assert(degree <= maxPadeDegree);
00317 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
00318 result.setZero(T.rows(), T.rows());
00319 for (int k = 0; k < degree; ++k)
00320 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
00321 .template triangularView<Upper>().solve(TminusI);
00322 }
00323
00324 template <typename MatrixType>
00325 void MatrixLogarithmAtomic<MatrixType>::computePade7(MatrixType& result, const MatrixType& T)
00326 {
00327 const int degree = 7;
00328 const RealScalar nodes[] = { 0.0254460438286207377369051579760744L, 0.1292344072003027800680676133596058L,
00329 0.2970774243113014165466967939615193L, 0.5000000000000000000000000000000000L,
00330 0.7029225756886985834533032060384807L, 0.8707655927996972199319323866403942L,
00331 0.9745539561713792622630948420239256L };
00332 const RealScalar weights[] = { 0.0647424830844348466353057163395410L, 0.1398526957446383339507338857118898L,
00333 0.1909150252525594724751848877444876L, 0.2089795918367346938775510204081633L,
00334 0.1909150252525594724751848877444876L, 0.1398526957446383339507338857118898L,
00335 0.0647424830844348466353057163395410L };
00336 assert(degree <= maxPadeDegree);
00337 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
00338 result.setZero(T.rows(), T.rows());
00339 for (int k = 0; k < degree; ++k)
00340 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
00341 .template triangularView<Upper>().solve(TminusI);
00342 }
00343
00344 template <typename MatrixType>
00345 void MatrixLogarithmAtomic<MatrixType>::computePade8(MatrixType& result, const MatrixType& T)
00346 {
00347 const int degree = 8;
00348 const RealScalar nodes[] = { 0.0198550717512318841582195657152635L, 0.1016667612931866302042230317620848L,
00349 0.2372337950418355070911304754053768L, 0.4082826787521750975302619288199080L,
00350 0.5917173212478249024697380711800920L, 0.7627662049581644929088695245946232L,
00351 0.8983332387068133697957769682379152L, 0.9801449282487681158417804342847365L };
00352 const RealScalar weights[] = { 0.0506142681451881295762656771549811L, 0.1111905172266872352721779972131204L,
00353 0.1568533229389436436689811009933007L, 0.1813418916891809914825752246385978L,
00354 0.1813418916891809914825752246385978L, 0.1568533229389436436689811009933007L,
00355 0.1111905172266872352721779972131204L, 0.0506142681451881295762656771549811L };
00356 assert(degree <= maxPadeDegree);
00357 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
00358 result.setZero(T.rows(), T.rows());
00359 for (int k = 0; k < degree; ++k)
00360 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
00361 .template triangularView<Upper>().solve(TminusI);
00362 }
00363
00364 template <typename MatrixType>
00365 void MatrixLogarithmAtomic<MatrixType>::computePade9(MatrixType& result, const MatrixType& T)
00366 {
00367 const int degree = 9;
00368 const RealScalar nodes[] = { 0.0159198802461869550822118985481636L, 0.0819844463366821028502851059651326L,
00369 0.1933142836497048013456489803292629L, 0.3378732882980955354807309926783317L,
00370 0.5000000000000000000000000000000000L, 0.6621267117019044645192690073216683L,
00371 0.8066857163502951986543510196707371L, 0.9180155536633178971497148940348674L,
00372 0.9840801197538130449177881014518364L };
00373 const RealScalar weights[] = { 0.0406371941807872059859460790552618L, 0.0903240803474287020292360156214564L,
00374 0.1303053482014677311593714347093164L, 0.1561735385200014200343152032922218L,
00375 0.1651196775006298815822625346434870L, 0.1561735385200014200343152032922218L,
00376 0.1303053482014677311593714347093164L, 0.0903240803474287020292360156214564L,
00377 0.0406371941807872059859460790552618L };
00378 assert(degree <= maxPadeDegree);
00379 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
00380 result.setZero(T.rows(), T.rows());
00381 for (int k = 0; k < degree; ++k)
00382 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
00383 .template triangularView<Upper>().solve(TminusI);
00384 }
00385
00386 template <typename MatrixType>
00387 void MatrixLogarithmAtomic<MatrixType>::computePade10(MatrixType& result, const MatrixType& T)
00388 {
00389 const int degree = 10;
00390 const RealScalar nodes[] = { 0.0130467357414141399610179939577740L, 0.0674683166555077446339516557882535L,
00391 0.1602952158504877968828363174425632L, 0.2833023029353764046003670284171079L,
00392 0.4255628305091843945575869994351400L, 0.5744371694908156054424130005648600L,
00393 0.7166976970646235953996329715828921L, 0.8397047841495122031171636825574368L,
00394 0.9325316833444922553660483442117465L, 0.9869532642585858600389820060422260L };
00395 const RealScalar weights[] = { 0.0333356721543440687967844049466659L, 0.0747256745752902965728881698288487L,
00396 0.1095431812579910219977674671140816L, 0.1346333596549981775456134607847347L,
00397 0.1477621123573764350869464973256692L, 0.1477621123573764350869464973256692L,
00398 0.1346333596549981775456134607847347L, 0.1095431812579910219977674671140816L,
00399 0.0747256745752902965728881698288487L, 0.0333356721543440687967844049466659L };
00400 assert(degree <= maxPadeDegree);
00401 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
00402 result.setZero(T.rows(), T.rows());
00403 for (int k = 0; k < degree; ++k)
00404 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
00405 .template triangularView<Upper>().solve(TminusI);
00406 }
00407
00408 template <typename MatrixType>
00409 void MatrixLogarithmAtomic<MatrixType>::computePade11(MatrixType& result, const MatrixType& T)
00410 {
00411 const int degree = 11;
00412 const RealScalar nodes[] = { 0.0108856709269715035980309994385713L, 0.0564687001159523504624211153480364L,
00413 0.1349239972129753379532918739844233L, 0.2404519353965940920371371652706952L,
00414 0.3652284220238275138342340072995692L, 0.5000000000000000000000000000000000L,
00415 0.6347715779761724861657659927004308L, 0.7595480646034059079628628347293048L,
00416 0.8650760027870246620467081260155767L, 0.9435312998840476495375788846519636L,
00417 0.9891143290730284964019690005614287L };
00418 const RealScalar weights[] = { 0.0278342835580868332413768602212743L, 0.0627901847324523123173471496119701L,
00419 0.0931451054638671257130488207158280L, 0.1165968822959952399592618524215876L,
00420 0.1314022722551233310903444349452546L, 0.1364625433889503153572417641681711L,
00421 0.1314022722551233310903444349452546L, 0.1165968822959952399592618524215876L,
00422 0.0931451054638671257130488207158280L, 0.0627901847324523123173471496119701L,
00423 0.0278342835580868332413768602212743L };
00424 assert(degree <= maxPadeDegree);
00425 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
00426 result.setZero(T.rows(), T.rows());
00427 for (int k = 0; k < degree; ++k)
00428 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
00429 .template triangularView<Upper>().solve(TminusI);
00430 }
00431
00444 template<typename Derived> class MatrixLogarithmReturnValue
00445 : public ReturnByValue<MatrixLogarithmReturnValue<Derived> >
00446 {
00447 public:
00448
00449 typedef typename Derived::Scalar Scalar;
00450 typedef typename Derived::Index Index;
00451
00456 MatrixLogarithmReturnValue(const Derived& A) : m_A(A) { }
00457
00462 template <typename ResultType>
00463 inline void evalTo(ResultType& result) const
00464 {
00465 typedef typename Derived::PlainObject PlainObject;
00466 typedef internal::traits<PlainObject> Traits;
00467 static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
00468 static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
00469 static const int Options = PlainObject::Options;
00470 typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
00471 typedef Matrix<ComplexScalar, Dynamic, Dynamic, Options, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType;
00472 typedef MatrixLogarithmAtomic<DynMatrixType> AtomicType;
00473 AtomicType atomic;
00474
00475 const PlainObject Aevaluated = m_A.eval();
00476 MatrixFunction<PlainObject, AtomicType> mf(Aevaluated, atomic);
00477 mf.compute(result);
00478 }
00479
00480 Index rows() const { return m_A.rows(); }
00481 Index cols() const { return m_A.cols(); }
00482
00483 private:
00484 typename internal::nested<Derived>::type m_A;
00485
00486 MatrixLogarithmReturnValue& operator=(const MatrixLogarithmReturnValue&);
00487 };
00488
00489 namespace internal {
00490 template<typename Derived>
00491 struct traits<MatrixLogarithmReturnValue<Derived> >
00492 {
00493 typedef typename Derived::PlainObject ReturnType;
00494 };
00495 }
00496
00497
00498
00499
00500
00501 template <typename Derived>
00502 const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const
00503 {
00504 eigen_assert(rows() == cols());
00505 return MatrixLogarithmReturnValue<Derived>(derived());
00506 }
00507
00508 }
00509
00510 #endif // EIGEN_MATRIX_LOGARITHM