rtcLinearSystem.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 .......: rtcLinearSystem.h
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 08/16/2006
 * modified ...: $Date: 2009-03-19 11:10:08 -0700 (Thu, 19 Mar 2009) $
 * changed by .: $Author: wg75pal $
 * revision ...: $Revision: 101 $
 */
#ifndef RTC_LINEARSYSTEM_H
#define RTC_LINEARSYSTEM_H

//== INCLUDES ==================================================================
#include "rtc/rtcMath.h"
#include "rtc/rtcSMat.h"
#include "rtc/rtcVec.h"

#define LSC_PO 0

//== NAMESPACES ================================================================
namespace rtc {

template <class T, int M>
class LinearSystem {
public:
  // Constructors/Destructor
  LinearSystem(SMat<T,M>& a_, bool overwrite_ = true);
  ~LinearSystem();

  // Decompositions and Solve routines
  int forceRefresh(int decomp = 0);
  int cholesky(const Vec<T,M>& b, Vec<T,M>& sol);
  int lu(const Vec<T,M>& b, Vec<T,M>& sol);
  int gaussianElim(const Vec<T,M>& b, Vec<T,M>& sol);
protected:
  // Data
  SMat<T,M>& a;
  SMat<T,M>* d;
  Vec<int,M> indx;
  int haveDecomp;
  bool overwrite;
}; // end class LinearSystem<T,M>

// Some common typedefs
typedef LinearSystem<float,2> LinearSystem2f;
typedef LinearSystem<double,2> LinearSystem2d;
typedef LinearSystem<float,3> LinearSystem3f;
typedef LinearSystem<double,3> LinearSystem3d;
typedef LinearSystem<float,4> LinearSystem4f;
typedef LinearSystem<double,4> LinearSystem4d;
typedef LinearSystem<float,5> LinearSystem5f;
typedef LinearSystem<double,5> LinearSystem5d;
typedef LinearSystem<float,6> LinearSystem6f;
typedef LinearSystem<double,6> LinearSystem6d;

// Constructor/Destructor

template <class T, int M>
inline LinearSystem<T,M>::LinearSystem(SMat<T,M>& a_, bool overwrite_) :
  a(a_), overwrite(overwrite_) {
  haveDecomp = 0;
  if (!overwrite) d = new SMat<T,M>(a);
  else d = &a;
  if (LSC_PO>1) std::cout << "Built LinearSystem class instance with ";
  if (LSC_PO>1) {
    if (overwrite) std::cout << "overwrite enabled.\n";
    else std::cout << "overwrite disabled.\n";
  }
}

template <class T, int M>
inline LinearSystem<T,M>::~LinearSystem() {
  if (!overwrite) {
    delete d;
  }
  if (LSC_PO>1) std::cout << "Deleted LinearSystem.\n";
}

// Solve routines

template <class T, int M>
inline int LinearSystem<T,M>::forceRefresh(int decomp) {
  if (decomp == 0 && !haveDecomp) {
    std::cerr << "No decomposition specified and none active.\n";
    return 1;
  }
  if (decomp == 0 && haveDecomp) {
    decomp = haveDecomp;
    haveDecomp = 0;
  }
  switch (decomp) {
  case 1:
    if (d->luDecomp(indx)) return 1; else haveDecomp = 1; break;
  case 2:
    if (d->choleskyDecomp()) return 1; else haveDecomp = 2; break;
  default:
    std::cerr << "Decomposition number " << decomp << " is not a valid choice.\n"
   << "Please choose from 1: LU, 2: Cholesky\n";
    return 1;
  }
}

template <class T, int M>
inline int LinearSystem<T,M>::lu(const Vec<T,M>& b, Vec<T,M>& sol) {
  if (haveDecomp != 1) {
    if (haveDecomp != 0) {
if (!overwrite) { d->set(a); } else {
  std::cerr << "Can't perform LU Decomposition because A already "
       << "contains ";
  switch (haveDecomp) {
  case 2: std::cerr << "a Cholesky Decomposition.\n"; break;
  case 3: std::cerr << "a Gauss-Jordan Decomposition.\n"; break;
  default: std::cerr << "an Unknown decomposition.\n" << haveDecomp; break;
  }
  return 1;
}
    }
    if (LSC_PO>1) std::cout << "Performing LU Decomposition...";
    if (d->luDecomp(indx)) return 1;
    else haveDecomp = 1;
    if (LSC_PO>1) std::cout << "Done.\n";
  }
  if (LSC_PO>1) std::cout << "Performing LU Solve...";
  sol.set(b);
  d->luSolve(indx,sol);
  if (LSC_PO>1) std::cout << "Done.\n";
  return 0;
}

template <class T, int M>
inline int LinearSystem<T,M>::cholesky(const Vec<T,M>& b, Vec<T,M>& sol) {
  if (haveDecomp != 2) {
    if (haveDecomp != 0) {
      if (!overwrite) { d->set(a); } else {
        std::cerr << "Can't perform Cholesky Decomposition because A already " << "contains ";
        switch (haveDecomp) {
          case 1: std::cerr << "an LU Decomposition.\n"; break;
          case 3: std::cerr << "a Gauss-Jordan Decomposition.\n"; break;
          default: std::cerr << "an Unknown decomposition.\n" << haveDecomp; break;
        }
        return 1;
      }
    }
    if (LSC_PO>1) std::cout << "Performing Cholesky Decomposition...";
    if (d->choleskyDecomp()) return 1;
    else haveDecomp = 2;
    if (LSC_PO>1) std::cout << "Done.\n";
  }
  if (LSC_PO>1) std::cout << "Performing Cholesky Solve...";
  sol.set(b);
  d->choleskySolve(sol);
  if (LSC_PO>1) std::cout << "Done.\n";
  return 0;
}

template<class T, int M>
inline int LinearSystem<T,M>::gaussianElim(const Vec<T,M>& b, Vec<T,M>& sol)
{
  if (haveDecomp != 0) {
    if (!overwrite) {
      d->set(a);
    } else {
      std::cerr << "Can't perform Gauss-Jordan Decomposition because A already "<< "contains ";
      switch (haveDecomp) {
      case 1:
        std::cerr << "an LU Decomposition.\n";
        break;
      case 2:
        std::cerr << "a Cholesky Decomposition.\n";
        break;
      case 3:
        std::cerr << "a different Gauss-Jordan Decomposition\n";
        break;
      default:
        std::cerr << "an Unknown decomposition.\n"<< haveDecomp;
        break;
      }
      return 1;
    }
  }
  haveDecomp = 3;
  if (LSC_PO>1)
    std::cout << "Performing Gauss-Jordan Decomposition/Solve...";

  T tmp, pvt;
  T *t;
  T* aa[M];
  T bb[M];
  int i, j, k;

  // Do decomposition
  for (i=0; i<M; i++) {
    aa[i] = &((*d)(i, 0));
    bb[i] = b.x[i];
  }
  for (i=0; i<M; i++) {
    pvt = aa[i][i];
    if (!pvt) {
      for (j=i+1; j<M; j++) {
        if ((pvt = a(j, i)) != 0.0)
          break;
      }
      if (!pvt)
        return 1;
      t=aa[j];
      aa[j]=aa[i];
      aa[i]=t;
      tmp=bb[j];
      bb[j]=bb[i];
      bb[i]=tmp;
    }
    for (k=i+1; k<M; k++) {
      tmp = aa[k][i]/pvt;
      for (j=i+1; j<M; j++) {
        aa[k][j] -= tmp*aa[i][j];
      }
      bb[k] -= tmp*bb[i];
    }
  }
  // Do back substitution
  for (i=M-1; i>=0; i--) {
    sol.x[i]=bb[i];
    for (j=M-1; j>i; j--) {
      sol.x[i] -= aa[i][j]*sol.x[j];
    }
    sol.x[i] /= aa[i][i];
  }
  if (LSC_PO>1)
    std::cout << "Done.\n";
  return 0;
}

//==============================================================================
} // NAMESPACE rtc
//==============================================================================
#endif // RTC_LINEARSYSTEM_H defined
//==============================================================================