rtcEigenSystem.h

Go to the documentation of this file.

/*
 * Copyright (C) 2009
 * Robert Bosch LLC
 * Research and Technology Center North America
 * Palo Alto, California
 *
 * All rights reserved.
 *
 *------------------------------------------------------------------------------
 * project ....: Autonomous Technologies
 * file .......: rtcEigenSystem.h
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 08/16/2006
 * modified ...: $Date: 2009-01-21 18:19:16 -0800 (Wed, 21 Jan 2009) $
 * changed by .: $Author: benjaminpitzer $
 * revision ...: $Revision: 14 $
 */
#ifndef RTC_EIGENSYSTEM_H
#define RTC_EIGENSYSTEM_H

// puma includes
#include <rtc/rtcException.h>
#include "rtc/rtcMath.h"
#include "rtc/rtcSMat.h"
#include "rtc/rtcVec.h"

#define PUMA_JACOBI_ROTATE(a,i,j,k,l) g=a.x[i*M+j];h=a.x[k*M+l];\
                                        a.x[i*M+j]=g-s*(h+g*tau);\
                                        a.x[k*M+l]=h+s*(g-h*tau);
#define PUMA_RADIX 2.0
#define PUMA_SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))

namespace rtc {

  // DECLARATIONS

  // Forward declarations
  template <class T, int M> class Vec;          // M-d vector
  template <class T, int M> class SMat;         // MxM Square Matrix
  template <class T, int M> class EigenSystem;  // Eigensystem


  template <class T, int M>
  class EigenSystem {
  public:
    // Constructors/Destructor
    EigenSystem(SMat<T,M>& a_);
    ~EigenSystem();

    // Eigen System routines for real symmetric matrices

    /*
     * Produces a vector of eigenvalues (D) and a matrix of eigenvectors (V) of
     * matrix A.
     */
    bool jacobi(Vec<T,M>& eigenvalues, SMat<T,M>& eigenvectors, int* nrot);
    bool jacobi(Vec<T,M>& eigenvalues, SMat<T,M>& eigenvectors);
    void eigenSort(Vec<T,M>& d, SMat<T,M>& v);

    // non-tested routines
    void householderReduce(Vec<T,M>& d, Vec<T,M>& e, bool vecs = true);
    void householderSolve(Vec<T,M>& d, Vec<T,M>& e, bool vecs = true);
    void balance();
    void hessenbergReduce();
    void qr(Vec<T,M>& wreal, Vec<T,M>& wimag);

  protected:
    void rotate(double& g,double& h,SMat<T,M>& a,
                const int i,const int j,const int k,const int l,
                const double s,const double tau) const;
    void rotateT(double& g,double& h,SMat<T,M>& a,
                 const int i,const int j,const int k,const int l,
                 const double s,const double tau) const;
    // Data
    SMat<T,M>& a;
  }; // end class EigenSystem<T,M>

  // DEFINITIONS


  // Constructor/Destructor

  template <class T, int M>
  inline EigenSystem<T,M>::EigenSystem(SMat<T,M>& a_) : a(a_) {
  }

  template <class T, int M>
  inline EigenSystem<T,M>::~EigenSystem() {
  }

  // Eigen System routines

  template <class T, int M>
  inline bool EigenSystem<T,M>::jacobi(Vec<T,M>& d, SMat<T,M>& v) {
    int nrot;
    return jacobi(d,v,&nrot);
  }

  template <class T, int M>
  inline bool EigenSystem<T,M>::jacobi(Vec<T,M>& d, SMat<T,M>& v, int* nrot) {
    int j,iq,ip,i;
    T tresh,theta,tau,t,sm,s,h,g,c;
    Vec<T,M> b;
    Vec<T,M> z;

    for (ip=0;ip<M;ip++) { // Initialize to the identity matrix.
      for (iq=0;iq<M;iq++) v.x[ip*M+iq]=0.0;
      v.x[ip*M+ip]=1.0;
    }
    for (ip=0;ip<M;ip++) { // Initialize b and d to the diagonal of a.
      b.x[ip]=d.x[ip]=a.x[ip*M+ip];
      z.x[ip]=0.0; // This vector will accumulate terms
    }
    *nrot=0;
    for (i=0;i<=50;i++) {
      sm=0.0;
      for (ip=0;ip<M-1;ip++) { // Sum off-diagonal elements
  for (iq=ip+1;iq<M;iq++) sm += fabs(a.x[ip*M+iq]);
      }
      if (sm == 0.0) return true;
      if (i < 4) tresh=0.2*sm/(M*M); // ...on the  rst three sweeps.
      else tresh=0.0; // ...thereafter.
      for (ip=0;ip<M-1;ip++) {
  for (iq=ip+1;iq<M;iq++) {
    g=100.0*fabs(a.x[ip*M+iq]);

    if (i > 4 && (T)(fabs(d.x[ip])+g) == (T)fabs(d.x[ip])
        && (T)(fabs(d.x[iq])+g) == (T)fabs(d.x[iq]))
      a.x[ip*M+iq]=0.0;
    else if (fabs(a.x[ip*M+iq]) > tresh) {
      h=d.x[iq]-d.x[ip];
      if ((T)(fabs(h)+g) == (T)fabs(h)) t=(a.x[ip*M+iq])/h;
      else {
        theta=0.5*h/(a.x[ip*M+iq]); // Equation (11.1.10).
        t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
        if (theta < 0.0) t = -t;
      }
      c=1.0/sqrt(1+t*t);
      s=t*c;
      tau=s/(1.0+c);
      h=t*a.x[ip*M+iq];
      z.x[ip] -= h;
      z.x[iq] += h;
      d.x[ip] -= h;
      d.x[iq] += h;
      a.x[ip*M+iq]=0.0;
      for (j=0;j<=ip-1;j++) { // Case of rotations 1 d j < p.
        PUMA_JACOBI_ROTATE(a,j,ip,j,iq)
    }
      for (j=ip+1;j<=iq-1;j++) { // Case of rotations p < j < q.
        PUMA_JACOBI_ROTATE(a,ip,j,j,iq)
    }
      for (j=iq+1;j<M;j++) { // Case of rotations q < j d n.
        PUMA_JACOBI_ROTATE(a,ip,j,iq,j)
    }
      for (j=0;j<M;j++) {
        PUMA_JACOBI_ROTATE(v,j,ip,j,iq)
    }
      ++(*nrot);
    }
  }
      }
      for (ip=0;ip<M;ip++) {
  b.x[ip] += z.x[ip];
  d.x[ip]=b.x[ip]; // Update d with the sum of tapq,
  z.x[ip]=0.0; // and reinitialize z.
      }
    }
    return false;
  }

  template <class T, int M>
  inline void EigenSystem<T,M>::rotate(double& g,double& h,SMat<T,M>& a,
                                       const int i,const int j,const int k,const int l,
                                       const double s,const double tau) const {
    g=a(i,j);
    h=a(k,l);
    a(i,j)=g-s*(h+g*tau);
    a(k,l)=h+s*(g-h*tau);
  }

  template <class T, int M>
  inline void EigenSystem<T,M>::rotateT(double& g,double& h,SMat<T,M>& a,
                                        const int i,const int j,const int k,const int l,
                                        const double s,const double tau) const {
    g=a(i,j);
    h=a(k,l);
    a(i,j)=static_cast<T>(g-s*(h+g*tau));
    a(k,l)=static_cast<T>(h+s*(g-h*tau));
  }


  template <class T, int M>
  inline void EigenSystem<T,M>::householderReduce(Vec<T,M>& d, Vec<T,M>& e,
              bool vecs) {
    int l,k,j,i;
    T scale,hh,h,g,f;

    // add wrapper to handle weird 1-based Numerical Recipes in C indexing
    int n = M;
    T* dd = d.x - 1;
    T* ee = e.x - 1;
    T** aa = new T*[M]; aa--;
    for (i=1;i<=M;i++) aa[i] = &(a.x[(i-1)*M-1]);

    for (i=n;i>=2;i--) {
      l=i-1;
      h=scale=0.0;
      if (l > 1) {
  for (k=1;k<=l;k++)
    scale += fabs(aa[i][k]);
  if (scale == 0.0)
    ee[i]=aa[i][l];
  else {
    for (k=1;k<=l;k++) {
      aa[i][k] /= scale;
      h += aa[i][k]*aa[i][k];
    }
    f=aa[i][l];
    g=(f >= 0.0 ? -sqrt(h) : sqrt(h));
    ee[i]=scale*g;
    h -= f*g;
    aa[i][l]=f-g;
    f=0.0;
    for (j=1;j<=l;j++) {
      /* Next statement can be omitted if eigenvectors not wanted */
      if (vecs) aa[j][i]=aa[i][j]/h;
      g=0.0;
      for (k=1;k<=j;k++)
        g += aa[j][k]*aa[i][k];
      for (k=j+1;k<=l;k++)
        g += aa[k][j]*aa[i][k];
      ee[j]=g/h;
      f += ee[j]*aa[i][j];
    }
    hh=f/(h+h);
    for (j=1;j<=l;j++) {
      f=aa[i][j];
      ee[j]=g=ee[j]-hh*f;
      for (k=1;k<=j;k++)
        aa[j][k] -= (f*ee[k]+g*aa[i][k]);
    }
  }
      } else
  ee[i]=aa[i][l];
      dd[i]=h;
    }
    /* Next statement can be omitted if eigenvectors not wanted */
    if (vecs) dd[1]=0.0;
    ee[1]=0.0;
    /* Contents of this loop can be omitted if eigenvectors not
       wanted except for statement dd[i]=aa[i][i]; */
    for (i=1;i<=n;i++) {
      if (vecs) {
  l=i-1;
  if (dd[i]) {
    for (j=1;j<=l;j++) {
      g=0.0;
      for (k=1;k<=l;k++)
        g += aa[i][k]*aa[k][j];
      for (k=1;k<=l;k++)
        aa[k][j] -= g*aa[k][i];
    }
  }
      }
      dd[i]=aa[i][i];
      if (vecs) {
  aa[i][i]=1.0;
  for (j=1;j<=l;j++) aa[j][i]=aa[i][j]=0.0;
      }
    }

    // free memory from stupid wrapper
    aa++; delete [] aa;
  }

  template <class T, int M>
  inline void EigenSystem<T,M>::householderSolve(Vec<T,M>& d, Vec<T,M>& e, bool vecs) {
    int m,l,iter,i,k;
    T s,r,p,g,f,dd,c,b;

    // add wrapper to handle weird 1-based Numerical Recipes in C indexing
    int n = M;
    T* ddd = d.x - 1;
    T* ee = e.x - 1;
    T** aa = new T*[M]; aa--;
    for (i=1;i<=M;i++) aa[i] = &(a.x[(i-1)*M-1]);

    for (i=2;i<=n;i++) ee[i-1]=ee[i];
    ee[n]=0.0;
    for (l=1;l<=n;l++) {
      iter=0;
      do {
  for (m=l;m<=n-1;m++) {
    dd=fabs(ddd[m])+fabs(ddd[m+1]);
    if ((T)(fabs(ee[m])+dd) == dd) break;
  }
  if (m != l) {
    if (iter++ == 30) {
      std::stringstream ss;
      ss << "EigenSystem<" << M << ">::householderSolve: error, "
         << "too many interaions." << std::endl << std::flush;
      throw Exception(ss.str());
    }
    g=(ddd[l+1]-ddd[l])/(2.0*ee[l]);
    r=pythag(g,T(1));
    g=ddd[m]-ddd[l]+ee[l]/(g+PUMA_SIGN(r,g));
    s=c=1.0;
    p=0.0;
    for (i=m-1;i>=l;i--) {
      f=s*ee[i];
      b=c*ee[i];
      ee[i+1]=(r=pythag(f,g));
      if (r == 0.0) {
        ddd[i+1] -= p;
        ee[m]=0.0;
        break;
      }
      s=f/r;
      c=g/r;
      g=ddd[i+1]-p;
      r=(ddd[i]-g)*s+2.0*c*b;
      ddd[i+1]=g+(p=s*r);
      g=c*r-b;
      /* Next loop can be omitted if eigenvectors not wanted*/
      if (vecs) for (k=1;k<=n;k++) {
        f=aa[k][i+1];
        aa[k][i+1]=s*aa[k][i]+c*f;
        aa[k][i]=c*aa[k][i]-s*f;
      }
    }
    if (r == 0.0 && i >= l) continue;
    ddd[l] -= p;
    ee[l]=g;
    ee[m]=0.0;
  }
      } while (m != l);
    }

    // free memory from stupid wrapper
    aa++; delete [] aa;
  }


  template<class T, int M>
  inline void EigenSystem<T, M>::eigenSort(Vec<T, M>& d, SMat<T, M>& v)
  {
    int k, j, i;
    T p;
    for(i = 0; i<M-1; i++) {
      p = rtc_abs(d.x[k = i]);
      for(j = i+1; j<M; j++)
        if (rtc_abs(d.x[j])>=p)
          p = d.x[k = j];
      if (k!=i) {
        d.x[k] = d.x[i];
        d.x[i] = p;
        for(j = 0; j<M; j++) {
          p = v.x[j*M+i];
          v.x[j*M+i] = v.x[j*M+k];
          v.x[j*M+k] = p;
        }
      }
    }
  }

  template <class T, int M>
  inline void EigenSystem<T,M>::balance() {
    int last,j,i;
    T s,r,g,f,c,sqrdx;
    sqrdx=PUMA_RADIX*PUMA_RADIX;
    last=0;
    while (last == 0) {
      last=1;
      for (i=0;i<M;i++) { // Calculate row and column norms.
  r=c=0.0;
  for (j=0;j<M;j++) if (j != i) {
    c += fabs(a.x[j*M+i]);
    r += fabs(a.x[i*M+j]);
  }
  if (c && r) { //If both are nonzero,
    g=r/PUMA_RADIX;
    f=1.0;
    s=c+r;
    while (c<g) {
      f *= PUMA_RADIX;
      c *= sqrdx;
    }
    g=r*PUMA_RADIX;
    while (c>g) {
      f /= PUMA_RADIX;
      c /= sqrdx;
    }
    if ((c+r)/f < 0.95*s) {
      last=0;
      g=1.0/f;
      for (j=0;j<M;j++) a.x[i*M+j] *= g;
      for (j=0;j<M;j++) a.x[j*M+i] *= f;
    }
  }
      }
    }
  }

  template <class T, int M>
  inline void EigenSystem<T,M>::hessenbergReduce() {
    int m,j,i;
    T y,x;

    for (m=1;m<M-1;m++) { // m is called r + 1 in the text.
      x=0.0;
      i=m;
      for (j=m;j<M;j++) { // Find the pivot.
  if (fabs(a.x[j*M+m-1]) > fabs(x)) {
    x=a.x[j*M+m-1];
    i=j;
  }
      }
      if (i != m) { // Interchange rows and columns.
  for (j=m-1;j<M;j++) puma_swap(a.x[i*M+j],a.x[m*M+j]);
  for (j=0;j<M;j++) puma_swap(a.x[j*M+i],a.x[j*M+m]);
      }
      if (x) { // Carry out the elimination.
  for (i=m+1;i<M;i++) {
    if ((y=a.x[i*M+m-1]) != 0.0) {
      y /= x;
      a.x[i*M+m-1]=y;
      for (j=m;j<M;j++) a.x[i*M+j] -= y*a.x[m*M+j];
      for (j=0;j<M;j++) a.x[j*M+m] += y*a.x[j*M+i];
    }
  }
      }
    }
  }

  template <class T, int M>
  inline void EigenSystem<T,M>::qr(Vec<T,M>& wreal, Vec<T,M>& wimag) {
    int nn,m,l,k,j,its,i,mmin;
    T z,y,x,w,v,u,t,s,r,q,p,anorm;

    T* wr = wreal.x - 1;
    T* wi = wimag.x - 1;
    T** aa = new T*[M]; aa--;
    for (i=1;i<=M;i++) aa[i] = &(a.x[(i-1)*M-1]);

    anorm=0.0;
    for (i=1;i<=M;i++)
      for (j=rtc_max(i-1,1);j<=M;j++)
  anorm += fabs(aa[i][j]);
    nn=M;
    t=0.0;
    while (nn >= 1) {
      its=0;
      do {
  for (l=nn;l>=2;l--) {
    s=fabs(aa[l-1][l-1])+fabs(aa[l][l]);
    if (s == 0.0) s=anorm;
    if ((T)(fabs(aa[l][l-1]) + s) == s) {
      aa[l][l-1]=0.0;
      break;
    }
  }
  x=aa[nn][nn];
  if (l == nn) {
    wr[nn]=x+t;
    wi[nn--]=0.0;
  } else {
    y=aa[nn-1][nn-1];
    w=aa[nn][nn-1]*aa[nn-1][nn];
    if (l == (nn-1)) {
      p=0.5*(y-x);
      q=p*p+w;
      z=sqrt(fabs(q));
      x += t;
      if (q >= 0.0) { // ...a real pair.
        z=p+PUMA_SIGN(z,p);
        wr[nn-1]=wr[nn]=x+z;
        if (z) wr[nn]=x-w/z;
        wi[nn-1]=wi[nn]=0.0;
      } else { //...a complex pair.
        wr[nn-1]=wr[nn]=x+p;
        wi[nn-1]= -(wi[nn]=z);
      }
      nn -= 2;
    } else { // No roots found. Continue iteration.
      if (its == 30) {
        std::stringstream ss;
        ss << "EigenSystem<" << M << ">::qr: error, " << "too many iterations in hqr\n";
        throw Exception(ss.str());
      }
      if (its == 10 || its == 20) { // Form exceptional shift.
        t += x;
        for (i=1;i<=nn;i++) aa[i][i] -= x;
        s=fabs(aa[nn][nn-1])+fabs(aa[nn-1][nn-2]);
        y=x=0.75*s;
        w = -0.4375*s*s;
      }
      ++its;
      for (m=(nn-2);m>=l;m--) {
        z=aa[m][m];
        r=x-z;
        s=y-z;
        p=(r*s-w)/aa[m+1][m]+aa[m][m+1]; // Equation (11.6.23).
        q=aa[m+1][m+1]-z-r-s;
        r=aa[m+2][m+1];
        s=fabs(p)+fabs(q)+fabs(r);
        p /= s; q /= s; r /= s;
        if (m == l) break;

        u=fabs(aa[m][m-1])*(fabs(q)+fabs(r));
        v=fabs(p)*(fabs(aa[m-1][m-1])+fabs(z)+fabs(aa[m+1][m+1]));
        if ((T)(u+v) == v) break;
      }
      for (i=m+2;i<=nn;i++) {
        aa[i][i-2]=0.0;
        if (i != (m+2)) aa[i][i-3]=0.0;
      }
      for (k=m;k<=nn-1;k++) {
        if (k != m) {
    p=aa[k][k-1];
    q=aa[k+1][k-1];
    r=0.0;
    if (k != (nn-1)) r=aa[k+2][k-1];
    if ((x=fabs(p)+fabs(q)+fabs(r)) != 0.0) {
      p /= x;
      q /= x;
      r /= x;
    }
        }
        if ((s=PUMA_SIGN(sqrt(p*p+q*q+r*r),p)) != 0.0) {
    if (k == m) {
      if (l != m)
        aa[k][k-1] = -aa[k][k-1];
    } else
      aa[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;j++) {
      p=aa[k][j]+q*aa[k+1][j];
      if (k != (nn-1)) {
        p += r*aa[k+2][j];
        aa[k+2][j] -= p*z;
      }
      aa[k+1][j] -= p*y;
      aa[k][j] -= p*x;
    }
    mmin = nn<k+3 ? nn : k+3;
    for (i=l;i<=mmin;i++) {
      p=x*aa[i][k]+y*aa[i][k+1];
      if (k != (nn-1)) {
        p += z*aa[i][k+2];
        aa[i][k+2] -= p*r;
      }
      aa[i][k+1] -= p*q;
      aa[i][k] -= p;
    }
        }
      }
    }
        }
      } while (l < nn-1);
    }
    aa++; delete [] aa;
  }

}; // end namspace puma

#endif