[Toon-members] TooN SymEigen.h

[Qi Pan]

CVSROOT:        /sources/toon
Module name:    TooN
Changes by:     Qi Pan <qpan>   09/12/03 18:27:27

Modified files:
        .              : SymEigen.h 

Log message:
        update to SymEigen.h with fast specialisation for eigendecomposition of 
symmetric 3x3 matrices

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/SymEigen.h?cvsroot=toon&r1=1.24&r2=1.25

Patches:
Index: SymEigen.h
===================================================================
RCS file: /sources/toon/TooN/SymEigen.h,v
retrieving revision 1.24
retrieving revision 1.25
diff -u -b -r1.24 -r1.25
--- SymEigen.h  28 Aug 2009 01:14:22 -0000      1.24
+++ SymEigen.h  3 Dec 2009 18:27:27 -0000       1.25
@@ -33,12 +33,13 @@
 #include <iostream>
 #include <cassert>
 #include <cmath>
+#include <complex>
 #include <TooN/lapack.h>
 
 #include <TooN/TooN.h>
 
 namespace TooN {
-
+static const double 
root3=1.73205080756887729352744634150587236694280525381038062805580;
 
 namespace Internal{
        ///Default condition number for SymEigen::backsub, SymEigen::get_pinv 
and SymEigen::get_inv_diag
@@ -118,6 +119,85 @@
                }
        };
 
+    ///@internal
+       ///@brief Compute 3x3 eigensystems
+       ///Helper struct for computing eigensystems, specialized on 3x3 
matrices.
+       ///@ingroup gInternal
+       template <> struct ComputeSymEigen<3> {
+
+               ///@internal
+               ///Compute an eigensystem.
+               ///@param m Input matrix (assumed to be symmetric)
+               ///@param eig Eigen vector output
+               ///@param ev Eigen values output
+               template<typename P, typename B>
+               static inline void compute(const Matrix<3,3,P,B>& m, 
Matrix<3,3,P>& eig, Vector<3, P>& ev) {
+            //method uses closed form solution of cubic equation to obtain 
roots of characteristic equation.
+
+            //Polynomial terms of |a - l * Identity|
+            //l^3 + a*l^2 + b*l + c
+
+            const double& a11 = m[0][0];
+            const double& a12 = m[0][1];
+            const double& a13 = m[0][2];
+
+            const double& a22 = m[1][1];
+            const double& a23 = m[1][2];
+
+            const double& a33 = m[2][2];
+
+            //From matlab:
+            double a = -a11-a22-a33;
+            double b = a11*a22+a11*a33+a22*a33-a12*a12-a13*a13-a23*a23;
+            double c = 
a11*(a23*a23)+(a13*a13)*a22+(a12*a12)*a33-a12*a13*a23*2.0-a11*a22*a33;
+
+            //Using Cardano's method:
+            double p = b - a*a/3;
+            double q = c + (2*a*a*a - 9*a*b)/27;
+
+            double alpha = -q/2;
+            double beta_descriminant = q*q/4 + p*p*p/27;
+
+            //beta_descriminant <= 0 for real roots!
+            double beta = sqrt(-beta_descriminant);
+            double r2 = alpha*alpha  - beta_descriminant;
+
+            ///Need A,B = cubert(alpha +- beta)
+            ///Turn in to r, theta
+            /// r^(1/3) * e^(i * theta/3)
+
+            double cuberoot_r = pow(r2, 1./6);
+            double theta3 = atan2(beta, alpha)/3;
+
+            double A_plus_B = 2*cuberoot_r*cos(theta3);
+            double A_minus_B = -2*cuberoot_r*sin(theta3);
+
+            //calculate eigenvalues
+            ev =  makeVector(A_plus_B, -A_plus_B/2 + A_minus_B * sqrt(3)/2, 
-A_plus_B/2 - A_minus_B * sqrt(3)/2) - Ones * a/3;
+
+            if(ev[0] > ev[1])
+                swap(ev[0], ev[1]);
+            if(ev[1] > ev[2])
+                swap(ev[1], ev[2]);
+            if(ev[0] > ev[1])
+                swap(ev[0], ev[1]);
+
+            //calculate the eigenvectors
+            eig[0][0]=a12 * a23 - a13 * (a22 - ev[0]);
+            eig[0][1]=a12 * a13 - a23 * (a11 - ev[0]);
+            eig[0][2]=(a11-ev[0])*(a22-ev[0]) - a12*a12;
+            normalize(eig[0]);
+            eig[1][0]=a12 * a23 - a13 * (a22 - ev[1]);
+            eig[1][1]=a12 * a13 - a23 * (a11 - ev[1]);
+            eig[1][2]=(a11-ev[1])*(a22-ev[1]) - a12*a12;
+            normalize(eig[1]);
+            eig[2][0]=a12 * a23 - a13 * (a22 - ev[2]);
+            eig[2][1]=a12 * a13 - a23 * (a11 - ev[2]);
+            eig[2][2]=(a11-ev[2])*(a22-ev[2]) - a12*a12;
+            normalize(eig[2]);
+               }
+       };
+
 };
 
 /**
@@ -333,3 +413,4 @@
 }
 
 #endif
+