862 lines
16 KiB
C++
862 lines
16 KiB
C++
#pragma once
|
|
|
|
#include "functions.h"
|
|
#include "mat.h"
|
|
#include "diag_mat.h"
|
|
|
|
#include <limits>
|
|
#include <algorithm>
|
|
|
|
|
|
#include "vec.h"
|
|
#include <complex>
|
|
|
|
namespace cgv {
|
|
namespace math {
|
|
|
|
|
|
//if x is an eigen vector of A the rayleigh quotient returns the corresponding eigenvalue
|
|
template<typename T>
|
|
T rayleigh_quotient(mat<T>& A,vec<T>&x)
|
|
{
|
|
return dot(x,A*x)/dot(x,x);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
template <typename T>
|
|
void rot(mat<T> &a, const T s, const T tau, const int i,
|
|
|
|
const int j, const int k, const int l)
|
|
|
|
{
|
|
|
|
T g=a(i,j);
|
|
|
|
T h=a(k,l);
|
|
|
|
a(i,j)=g-s*(h+g*tau);
|
|
|
|
a(k,l)=h+s*(g-h*tau);
|
|
|
|
}
|
|
|
|
|
|
|
|
template <typename T>
|
|
void eigsrt(diag_mat<T> &d, mat<T> &v)
|
|
|
|
{
|
|
|
|
unsigned k;
|
|
|
|
unsigned n=d.size();
|
|
|
|
for (unsigned i=0;i<n-1;i++)
|
|
|
|
{
|
|
|
|
T p=d(k=i);
|
|
|
|
for (unsigned j=i;j<n;j++)
|
|
|
|
if (d(j) >= p) p=d(k=j);
|
|
|
|
if (k != i)
|
|
|
|
{
|
|
|
|
d(k)=d(i);
|
|
d(i)=p;
|
|
|
|
|
|
for (unsigned j=0;j<n;j++)
|
|
{
|
|
|
|
p=v(j,i);
|
|
v(j,i)=v(j,k);
|
|
v(j,k)=p;
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
///eigen decomposition of a symmetric matrix using the jacobi method
|
|
///v contains the eigenvectors
|
|
///d contains the eigenvalues
|
|
///a=v*d*transpose(v)
|
|
template <typename T>
|
|
bool eig_sym(const mat<T> &a, mat<T> &v, diag_mat<T> &d,bool ordering=true, unsigned maxiter=50)
|
|
{
|
|
mat<T>aa = a;
|
|
unsigned n =aa.nrows();
|
|
|
|
v.identity(n);
|
|
|
|
d.resize(n);
|
|
|
|
unsigned nrot=0;
|
|
|
|
const T eps = std::numeric_limits<T>::epsilon();
|
|
|
|
T tresh,theta,tau,t,sm,s,h,g,c;
|
|
|
|
vec<T> b(n),z;
|
|
z.zeros(n);
|
|
unsigned ip,iq;
|
|
|
|
for(unsigned i = 0; i < n; i++)
|
|
d(i)=b(i)=aa(i,i);
|
|
|
|
for (unsigned i=1;i<=maxiter;i++)
|
|
|
|
{
|
|
|
|
sm=(T)0.0;
|
|
|
|
|
|
|
|
for (ip=0;ip<n-1;ip++)
|
|
|
|
{
|
|
|
|
for (iq=ip+1;iq<n;iq++)
|
|
|
|
sm += std::abs(aa(ip,iq));
|
|
|
|
}
|
|
|
|
if (sm == (T)0.0)
|
|
|
|
{
|
|
|
|
if(ordering)
|
|
|
|
eigsrt(d,v);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
if (i < 4)
|
|
|
|
tresh=(T)(0.2*sm/(n*n));
|
|
|
|
else
|
|
|
|
tresh=(T)0.0;
|
|
|
|
for (ip=0;ip<n-1;ip++)
|
|
|
|
{
|
|
|
|
for (iq=ip+1;iq<n;iq++)
|
|
|
|
{
|
|
|
|
g=((T)100.0)*std::abs(aa(ip,iq));
|
|
|
|
if (i > 4 && g <= eps*std::abs(d(ip)) && g <= eps*std::abs(d(iq)))
|
|
|
|
aa(ip,iq)=(T)0.0;
|
|
|
|
else if (std::abs(aa(ip,iq)) > tresh)
|
|
|
|
{
|
|
|
|
h=d(iq)-d(ip);
|
|
|
|
if (g <= eps*std::abs(h))
|
|
|
|
t=(aa(ip,iq))/h;
|
|
|
|
else {
|
|
|
|
theta=(T)(0.5*h/(aa(ip,iq)));
|
|
|
|
t=(T)(1.0/(std::abs(theta)+sqrt(1.0+theta*theta)));
|
|
|
|
if (theta < 0.0) t = -t;
|
|
|
|
}
|
|
|
|
c=(T)(1.0/sqrt(1+t*t));
|
|
|
|
s=t*c;
|
|
|
|
tau=(T)(s/(1.0+c));
|
|
|
|
h=t*aa(ip,iq);
|
|
|
|
z(ip) -= h;
|
|
|
|
z(iq) += h;
|
|
|
|
d(ip) -= h;
|
|
|
|
d(iq) += h;
|
|
|
|
aa(ip,iq)=(T)0.0;
|
|
|
|
for (unsigned j=0;j<ip;j++)
|
|
|
|
rot(aa,s,tau,j,ip,j,iq);
|
|
|
|
for (unsigned j=ip+1;j<iq;j++)
|
|
|
|
rot(aa,s,tau,ip,j,j,iq);
|
|
|
|
for (unsigned j=iq+1;j<n;j++)
|
|
|
|
rot(aa,s,tau,ip,j,iq,j);
|
|
|
|
for (unsigned j=0;j<n;j++)
|
|
|
|
rot(v,s,tau,j,ip,j,iq);
|
|
|
|
++nrot;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
for (ip=0;ip<n;ip++) {
|
|
|
|
b(ip) += z(ip);
|
|
|
|
d(ip)=b(ip);
|
|
|
|
z(ip)=(T)0.0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
//Too many iterations in routine jacobi
|
|
|
|
}
|
|
|
|
|
|
|
|
template <typename T>
|
|
struct Unsymmeig
|
|
{
|
|
int n;
|
|
cgv::math::mat<T> a,zz;
|
|
cgv::math::diag_mat<std::complex<T> > wri;
|
|
cgv::math::vec<T> scale;
|
|
cgv::math::vec<int> perm;
|
|
bool yesvecs,hessen;
|
|
|
|
Unsymmeig(const cgv::math::mat<T>&aa, bool yesvec=true, bool hessenb=false) :
|
|
n(aa.nrows()), a(aa), zz(n,n,0.0), wri(n), scale(n), perm(n),
|
|
yesvecs(yesvec), hessen(hessenb)
|
|
{
|
|
scale.ones();
|
|
balance();
|
|
if (!hessen) elmhes();
|
|
if (yesvecs) {
|
|
for (int i=0;i<n;i++)
|
|
zz(i,i)=1.0;
|
|
if (!hessen) eltran();
|
|
hqr2();
|
|
balbak();
|
|
sortvecs();
|
|
} else {
|
|
hqr();
|
|
sort();
|
|
}
|
|
}
|
|
void balance();
|
|
void elmhes();
|
|
void eltran();
|
|
void hqr();
|
|
void hqr2();
|
|
void balbak();
|
|
void sort();
|
|
void sortvecs();
|
|
|
|
|
|
};
|
|
template <typename T>
|
|
T SIGN(const T &a, const T &b)
|
|
{return (T)(b >= 0 ? (a >= 0 ? a : -a) : (a >= 0 ? -a : a));}
|
|
template <typename T>
|
|
void Unsymmeig<T>::balance()
|
|
{
|
|
const T RADIX = std::numeric_limits<T>::radix;
|
|
bool done=false;
|
|
T sqrdx=RADIX*RADIX;
|
|
while (!done) {
|
|
done=true;
|
|
for (int i=0;i<n;i++) {
|
|
T r=0.0,c=0.0;
|
|
for (int j=0;j<n;j++)
|
|
if (j != i) {
|
|
c += std::abs(a(j,i));
|
|
r += std::abs(a(i,j));
|
|
}
|
|
if (c != 0.0 && r != 0.0) {
|
|
T g=r/RADIX;
|
|
T f=1.0;
|
|
T s=c+r;
|
|
while (c<g) {
|
|
f *= RADIX;
|
|
c *= sqrdx;
|
|
}
|
|
g=r*RADIX;
|
|
while (c>g) {
|
|
f /= RADIX;
|
|
c /= sqrdx;
|
|
}
|
|
if ((c+r)/f < 0.95*s)
|
|
{
|
|
done=false;
|
|
g=1.0/f;
|
|
scale[i] *= f;
|
|
for (int j=0;j<n;j++) a(i,j) *= g;
|
|
for (int j=0;j<n;j++) a(j,i) *= f;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
template <typename T>
|
|
void Unsymmeig<T>::balbak()
|
|
{
|
|
for (int i=0;i<n;i++)
|
|
for (int j=0;j<n;j++)
|
|
zz(i,j) *= scale[i];
|
|
}
|
|
|
|
template <typename T>
|
|
void Unsymmeig<T>::elmhes()
|
|
{
|
|
for (int m=1;m<n-1;m++) {
|
|
T x=0.0;
|
|
int i=m;
|
|
for (int j=m;j<n;j++)
|
|
{
|
|
if (std::abs(a(j,m-1)) > std::abs(x))
|
|
{
|
|
x=a(j,m-1);
|
|
i=j;
|
|
}
|
|
}
|
|
perm[m]=i;
|
|
if (i != m)
|
|
{
|
|
for (int j=m-1;j<n;j++) std::swap(a(i,j),a(m,j));
|
|
for (int j=0;j<n;j++) std::swap(a(j,i),a(j,m));
|
|
}
|
|
if (x != 0.0)
|
|
{
|
|
for (i=m+1;i<n;i++)
|
|
{
|
|
T y=a(i,m-1);
|
|
if (y != 0.0)
|
|
{
|
|
y /= x;
|
|
a(i,m-1)=y;
|
|
for (int j=m;j<n;j++) a(i,j) -= y*a(m,j);
|
|
for (int j=0;j<n;j++) a(j,m) += y*a(j,i);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
void Unsymmeig<T>::eltran()
|
|
{
|
|
for (int mp=n-2;mp>0;mp--)
|
|
{
|
|
for (int k=mp+1;k<n;k++)
|
|
zz(k,mp)=a(k,mp-1);
|
|
int i=perm[mp];
|
|
if (i != mp) {
|
|
for (int j=mp;j<n;j++) {
|
|
zz(mp,j)=zz(i,j);
|
|
zz(i,j)=0.0;
|
|
}
|
|
zz(i,mp)=1.0;
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
void Unsymmeig<T>::hqr()
|
|
{
|
|
int nn,m,l,k,j,its,i,mmin;
|
|
T z,y,x,w,v,u,t,s,r,q,p,anorm=0.0;
|
|
|
|
const T EPS=std::numeric_limits<T>::epsilon();
|
|
for (i=0;i<n;i++)
|
|
for (j=std::max(i-1,0);j<n;j++)
|
|
anorm += std::abs(a(i,j));
|
|
nn=n-1;
|
|
t=0.0;
|
|
while (nn >= 0) {
|
|
its=0;
|
|
do {
|
|
for (l=nn;l>0;l--) {
|
|
s=std::abs(a(l-1,l-1))+std::abs(a(l,l));
|
|
if (s == 0.0) s=anorm;
|
|
if (std::abs(a(l,l-1)) <= EPS*s)
|
|
{
|
|
a(l,l-1) = 0.0;
|
|
break;
|
|
}
|
|
}
|
|
x=a(nn,nn);
|
|
if (l == nn) {
|
|
wri[nn--]=x+t;
|
|
} else {
|
|
y=a(nn-1,nn-1);
|
|
w=a(nn,nn-1)*a(nn-1,nn);
|
|
if (l == nn-1)
|
|
{
|
|
p=0.5*(y-x);
|
|
q=p*p+w;
|
|
z=sqrt(std::abs(q));
|
|
x += t;
|
|
if (q >= 0.0) {
|
|
z=p+SIGN(z,p);
|
|
wri[nn-1]=wri[nn]=x+z;
|
|
if (z != 0.0) wri[nn]=x-w/z;
|
|
} else {
|
|
wri[nn]=std::complex<T>(x+p,-z);
|
|
wri[nn-1]=conj(wri[nn]);
|
|
}
|
|
nn -= 2;
|
|
} else {
|
|
if (its == 30) throw("Too many iterations in hqr");
|
|
if (its == 10 || its == 20) {
|
|
t += x;
|
|
for (i=0;i<nn+1;i++) a(i,i) -= x;
|
|
s=std::abs(a(nn,nn-1))+std::abs(a(nn-1,nn-2));
|
|
y=x=0.75*s;
|
|
w = -0.4375*s*s;
|
|
}
|
|
++its;
|
|
for (m=nn-2;m>=l;m--) {
|
|
z=a(m,m);
|
|
r=x-z;
|
|
s=y-z;
|
|
p=(r*s-w)/a(m+1,m)+a(m,m+1);
|
|
q=a(m+1,m+1)-z-r-s;
|
|
r=a(m+2,m+1);
|
|
s=std::abs(p)+std::abs(q)+std::abs(r);
|
|
p /= s;
|
|
q /= s;
|
|
r /= s;
|
|
if (m == l) break;
|
|
u=std::abs(a(m,m-1))*(std::abs(q)+std::abs(r));
|
|
v=std::abs(p)*(std::abs(a(m-1,m-1))+std::abs(z)+std::abs(a(m+1,m+1)));
|
|
if (u <= EPS*v) break;
|
|
}
|
|
for (i=m;i<nn-1;i++) {
|
|
a(i+2,i)=0.0;
|
|
if (i != m) a(i+2,i-1)=0.0;
|
|
}
|
|
for (k=m;k<nn;k++) {
|
|
if (k != m) {
|
|
p=a(k,k-1);
|
|
q=a(k+1,k-1);
|
|
r=0.0;
|
|
if (k+1 != nn) r=a(k+2,k-1);
|
|
if ((x=std::abs(p)+std::abs(q)+std::abs(r)) != 0.0) {
|
|
p /= x;
|
|
q /= x;
|
|
r /= x;
|
|
}
|
|
}
|
|
if ((s=SIGN(sqrt(p*p+q*q+r*r),p)) != 0.0) {
|
|
if (k == m) {
|
|
if (l != m)
|
|
a(k,k-1) = -a(k,k-1);
|
|
} else
|
|
a(k,k-1) = -s*x;
|
|
p += s;
|
|
x=p/s;
|
|
y=q/s;
|
|
z=r/s;
|
|
q /= p;
|
|
r /= p;
|
|
for (j=k;j<nn+1;j++)
|
|
{
|
|
p=a(k,j)+q*a(k+1,j);
|
|
if (k+1 != nn) {
|
|
p += r*a(k+2,j);
|
|
a(k+2,j) -= p*z;
|
|
}
|
|
a(k+1,j) -= p*y;
|
|
a(k,j) -= p*x;
|
|
}
|
|
mmin = nn < k+3 ? nn : k+3;
|
|
for (i=l;i<mmin+1;i++)
|
|
{
|
|
p=x*a(i,k)+y*a(i,k+1);
|
|
if (k+1 != nn)
|
|
{
|
|
p += z*a(i,k+2);
|
|
a(i,k+2) -= p*r;
|
|
}
|
|
a(i,k+1) -= p*q;
|
|
a(i,k) -= p;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} while (l+1 < nn);
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
void Unsymmeig<T>::hqr2()
|
|
{
|
|
int nn,m,l,k,j,its,i,mmin,na;
|
|
T z,y,x,w,v,u,t,s,r,q,p,anorm=0.0,ra,sa,vr,vi;
|
|
|
|
const T EPS=std::numeric_limits<T>::epsilon();
|
|
for (i=0;i<n;i++)
|
|
for (j=std::max(i-1,0);j<n;j++)
|
|
anorm += std::abs(a(i,j));
|
|
nn=n-1;
|
|
t=0.0;
|
|
while (nn >= 0) {
|
|
its=0;
|
|
do {
|
|
for (l=nn;l>0;l--) {
|
|
s=std::abs(a(l-1,l-1))+std::abs(a(l,l));
|
|
if (s == 0.0) s=anorm;
|
|
if (std::abs(a(l,l-1)) <= EPS*s) {
|
|
a(l,l-1) = 0.0;
|
|
break;
|
|
}
|
|
}
|
|
x=a(nn,nn);
|
|
if (l == nn) {
|
|
wri(nn)=a(nn,nn)=x+t;
|
|
nn--;
|
|
} else {
|
|
y=a(nn-1,nn-1);
|
|
w=a(nn,nn-1)*a(nn-1,nn);
|
|
if (l == nn-1) {
|
|
p=0.5*(y-x);
|
|
q=p*p+w;
|
|
z=sqrt(std::abs(q));
|
|
x += t;
|
|
a(nn,nn)=x;
|
|
a(nn-1,nn-1)=y+t;
|
|
if (q >= 0.0) {
|
|
z=p+SIGN(z,p);
|
|
wri[nn-1]=wri[nn]=x+z;
|
|
if (z != 0.0) wri[nn]=x-w/z;
|
|
x=a(nn,nn-1);
|
|
s=std::abs(x)+std::abs(z);
|
|
p=x/s;
|
|
q=z/s;
|
|
r=sqrt(p*p+q*q);
|
|
p /= r;
|
|
q /= r;
|
|
for (j=nn-1;j<n;j++) {
|
|
z=a(nn-1,j);
|
|
a(nn-1,j)=q*z+p*a(nn,j);
|
|
a(nn,j)=q*a(nn,j)-p*z;
|
|
}
|
|
for (i=0;i<=nn;i++) {
|
|
z=a(i,nn-1);
|
|
a(i,nn-1)=q*z+p*a(i,nn);
|
|
a(i,nn)=q*a(i,nn)-p*z;
|
|
}
|
|
for (i=0;i<n;i++) {
|
|
z=zz(i,nn-1);
|
|
zz(i,nn-1)=q*z+p*zz(i,nn);
|
|
zz(i,nn)=q*zz(i,nn)-p*z;
|
|
}
|
|
} else {
|
|
wri[nn]=std::complex<T>(x+p,-z);
|
|
wri[nn-1]=std::conj(wri[nn]);
|
|
}
|
|
nn -= 2;
|
|
} else {
|
|
if (its == 30) throw("Too many iterations in hqr");
|
|
if (its == 10 || its == 20)
|
|
{
|
|
t += x;
|
|
for (i=0;i<nn+1;i++) a(i,i) -= x;
|
|
s=std::abs(a(nn,nn-1))+std::abs(a(nn-1,nn-2));
|
|
y=x=0.75*s;
|
|
w = -0.4375*s*s;
|
|
}
|
|
++its;
|
|
for (m=nn-2;m>=l;m--)
|
|
{
|
|
z=a(m,m);
|
|
r=x-z;
|
|
s=y-z;
|
|
p=(r*s-w)/a(m+1,m)+a(m,m+1);
|
|
q=a(m+1,m+1)-z-r-s;
|
|
r=a(m+2,m+1);
|
|
s=std::abs(p)+std::abs(q)+std::abs(r);
|
|
p /= s;
|
|
q /= s;
|
|
r /= s;
|
|
if (m == l) break;
|
|
u=std::abs(a(m,m-1))*(std::abs(q)+std::abs(r));
|
|
v=std::abs(p)*(std::abs(a(m-1,m-1))+std::abs(z)+std::abs(a(m+1,m+1)));
|
|
if (u <= EPS*v) break;
|
|
}
|
|
for (i=m;i<nn-1;i++) {
|
|
a(i+2,i)=0.0;
|
|
if (i != m) a(i+2,i-1)=0.0;
|
|
}
|
|
for (k=m;k<nn;k++) {
|
|
if (k != m) {
|
|
p=a(k,k-1);
|
|
q=a(k+1,k-1);
|
|
r=0.0;
|
|
if (k+1 != nn) r=a(k+2,k-1);
|
|
if ((x=std::abs(p)+std::abs(q)+std::abs(r)) != 0.0) {
|
|
p /= x;
|
|
q /= x;
|
|
r /= x;
|
|
}
|
|
}
|
|
if ((s=SIGN(sqrt(p*p+q*q+r*r),p)) != 0.0) {
|
|
if (k == m) {
|
|
if (l != m)
|
|
a(k,k-1) = -a(k,k-1);
|
|
} else
|
|
a(k,k-1) = -s*x;
|
|
p += s;
|
|
x=p/s;
|
|
y=q/s;
|
|
z=r/s;
|
|
q /= p;
|
|
r /= p;
|
|
for (j=k;j<n;j++) {
|
|
p=a(k,j)+q*a(k+1,j);
|
|
if (k+1 != nn) {
|
|
p += r*a(k+2,j);
|
|
a(k+2,j) -= p*z;
|
|
}
|
|
a(k+1,j) -= p*y;
|
|
a(k,j) -= p*x;
|
|
}
|
|
mmin = nn < k+3 ? nn : k+3;
|
|
for (i=0;i<mmin+1;i++) {
|
|
p=x*a(i,k)+y*a(i,k+1);
|
|
if (k+1 != nn) {
|
|
p += z*a(i,k+2);
|
|
a(i,k+2) -= p*r;
|
|
}
|
|
a(i,k+1) -= p*q;
|
|
a(i,k) -= p;
|
|
}
|
|
for (i=0; i<n; i++) {
|
|
p=x*zz(i,k)+y*zz(i,k+1);
|
|
if (k+1 != nn) {
|
|
p += z*zz(i,k+2);
|
|
zz(i,k+2) -= p*r;
|
|
}
|
|
zz(i,k+1) -= p*q;
|
|
zz(i,k) -= p;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} while (l+1 < nn);
|
|
}
|
|
if (anorm != 0.0) {
|
|
for (nn=n-1;nn>=0;nn--) {
|
|
p=real(wri[nn]);
|
|
q=imag(wri[nn]);
|
|
na=nn-1;
|
|
if (q == 0.0) {
|
|
m=nn;
|
|
a(nn,nn)=1.0;
|
|
for (i=nn-1;i>=0;i--) {
|
|
w=a(i,i)-p;
|
|
r=0.0;
|
|
for (j=m;j<=nn;j++)
|
|
r += a(i,j)*a(j,nn);
|
|
if (imag(wri[i]) < 0.0) {
|
|
z=w;
|
|
s=r;
|
|
} else {
|
|
m=i;
|
|
|
|
if (imag(wri[i]) == 0.0) {
|
|
t=w;
|
|
if (t == 0.0)
|
|
t=EPS*anorm;
|
|
a(i,nn)=-r/t;
|
|
} else {
|
|
x=a(i,i+1);
|
|
y=a(i+1,i);
|
|
q=cgv::math::sqr(real(wri[i])-p)+cgv::math::sqr(imag(wri[i]));
|
|
t=(x*s-z*r)/q;
|
|
a(i,nn)=t;
|
|
if (std::abs(x) > std::abs(z))
|
|
a(i+1,nn)=(-r-w*t)/x;
|
|
else
|
|
a(i+1,nn)=(-s-y*t)/z;
|
|
}
|
|
t=std::abs(a(i,nn));
|
|
if (EPS*t*t > 1)
|
|
for (j=i;j<=nn;j++)
|
|
a(j,nn) /= t;
|
|
}
|
|
}
|
|
} else if (q < 0.0) {
|
|
m=na;
|
|
if (std::abs(a(nn,na)) > std::abs(a(na,nn))) {
|
|
a(na,na)=q/a(nn,na);
|
|
a(na,nn)=-(a(nn,nn)-p)/a(nn,na);
|
|
} else {
|
|
std::complex<T> temp=std::complex<T>(0.0,-a(na,nn))/std::complex<T>(a(na,na)-p,q);
|
|
a(na,na)=real(temp);
|
|
a(na,nn)=imag(temp);
|
|
}
|
|
a(nn,na)=0.0;
|
|
a(nn,nn)=1.0;
|
|
for (i=nn-2;i>=0;i--) {
|
|
w=a(i,i)-p;
|
|
ra=sa=0.0;
|
|
for (j=m;j<=nn;j++) {
|
|
ra += a(i,j)*a(j,na);
|
|
sa += a(i,j)*a(j,nn);
|
|
}
|
|
if (imag(wri[i]) < 0.0) {
|
|
z=w;
|
|
r=ra;
|
|
s=sa;
|
|
} else {
|
|
m=i;
|
|
if (imag(wri[i]) == 0.0) {
|
|
std::complex<T> temp = std::complex<T>(-ra,-sa)/std::complex<T>(w,q);
|
|
a(i,na)=real(temp);
|
|
a(i,nn)=imag(temp);
|
|
} else {
|
|
x=a(i,i+1);
|
|
y=a(i+1,i);
|
|
vr=cgv::math::sqr(real(wri(i))-p)+cgv::math::sqr(imag(wri(i)))-q*q;
|
|
vi=2.0*q*(real(wri(i))-p);
|
|
if (vr == 0.0 && vi == 0.0)
|
|
vr=EPS*anorm*(std::abs(w)+std::abs(q)+std::abs(x)+std::abs(y)+std::abs(z));
|
|
std::complex<T> temp=std::complex<T>(x*r-z*ra+q*sa,x*s-z*sa-q*ra)/
|
|
std::complex<T>(vr,vi);
|
|
a(i,na)=real(temp);
|
|
a(i,nn)=imag(temp);
|
|
if (std::abs(x) > std::abs(z)+std::abs(q)) {
|
|
a(i+1,na)=(-ra-w*a(i,na)+q*a(i,nn))/x;
|
|
a(i+1,nn)=(-sa-w*a(i,nn)-q*a(i,na))/x;
|
|
} else {
|
|
std::complex<T> temp=std::complex<T>(-r-y*a(i,na),-s-y*a(i,nn))/
|
|
std::complex<T>(z,q);
|
|
a(i+1,na)=real(temp);
|
|
a(i+1,nn)=imag(temp);
|
|
}
|
|
}
|
|
}
|
|
t=std::max(std::abs(a(i,na)),std::abs(a(i,nn)));
|
|
if (EPS*t*t > 1)
|
|
for (j=i;j<=nn;j++) {
|
|
a(j,na) /= t;
|
|
a(j,nn) /= t;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
for (j=n-1;j>=0;j--)
|
|
for (i=0;i<n;i++) {
|
|
z=0.0;
|
|
for (k=0;k<=j;k++)
|
|
z += zz(i,k)*a(k,j);
|
|
zz(i,j)=z;
|
|
}
|
|
}
|
|
}
|
|
template <typename T>
|
|
void Unsymmeig<T>::sort()
|
|
{
|
|
int i;
|
|
for (int j=1;j<n;j++)
|
|
{
|
|
std::complex<T> x=wri[j];
|
|
for (i=j-1;i>=0;i--) {
|
|
if (std::real(wri[i]) >= real(x)) break;
|
|
wri[i+1]=wri[i];
|
|
}
|
|
wri[i+1]=x;
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
void Unsymmeig<T>::sortvecs()
|
|
{
|
|
int i;
|
|
cgv::math::vec<T> temp(n);
|
|
for (int j=1;j<n;j++) {
|
|
std::complex<T> x=wri[j];
|
|
for (int k=0;k<n;k++)
|
|
temp[k]=zz(k,j);
|
|
for (i=j-1;i>=0;i--) {
|
|
if (real(wri[i]) >= std::real(x)) break;
|
|
wri[i+1]=wri[i];
|
|
for (int k=0;k<n;k++)
|
|
zz(k,i+1)=zz(k,i);
|
|
}
|
|
wri[i+1]=x;
|
|
for (int k=0;k<n;k++)
|
|
zz(k,i+1)=temp[k];
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//compute eigenvalues of a which is a real unsymmetric matrix
|
|
//if a is still in hessenberg form set hessenb to true (default is false)
|
|
template <typename T>
|
|
void eig_unsym(const cgv::math::mat<T> &a,cgv::math::diag_mat<std::complex<T> >& eigvals, bool hessenb=false)
|
|
{
|
|
Unsymmeig<T> eigsolver((cgv::math::mat<T>)a,false,hessenb);
|
|
eigvals = eigsolver.wri;
|
|
|
|
}
|
|
|
|
|
|
//compute eigenvectors and eigenvalues of matrix a which is a real unsymmetric matrix
|
|
//if a is still in hessenberg form set hessenb to true (default is false)
|
|
//eigenvectors are not normalized
|
|
template <typename T>
|
|
void eig_unsym(const cgv::math::mat<T>& a,cgv::math::mat<T>& eigvecs,cgv::math::diag_mat<std::complex<T> >& eigvals,bool normalize=true, bool hessenb=false)
|
|
{
|
|
cgv::math::mat<T> A=a;
|
|
Unsymmeig<T> eigsolver(A,true,hessenb);
|
|
eigvals = eigsolver.wri;
|
|
if(normalize)
|
|
{
|
|
for(unsigned i = 0; i < eigsolver.zz.ncols();i++)
|
|
eigsolver.zz.set_col(i,cgv::math::normalize(eigsolver.zz.col(i)));
|
|
}
|
|
eigvecs=eigsolver.zz;
|
|
|
|
}
|
|
|
|
}}
|