rtc: rtc: rtcLinearSystem.h Source File

00001 /*
00002  * Copyright (C) 2009
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: rtcLinearSystem.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-03-19 11:10:08 -0700 (Thu, 19 Mar 2009) $
00016  * changed by .: $Author: wg75pal $
00017  * revision ...: $Revision: 101 $
00018  */
00019 #ifndef RTC_LINEARSYSTEM_H
00020 #define RTC_LINEARSYSTEM_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcSMat.h"
00025 #include "rtc/rtcVec.h"
00026 
00027 #define LSC_PO 0
00028 
00029 //== NAMESPACES ================================================================
00030 namespace rtc {
00031 
00035 template <class T, int M>
00036 class LinearSystem {
00037 public:
00038   // Constructors/Destructor
00039   LinearSystem(SMat<T,M>& a_, bool overwrite_ = true);
00040   ~LinearSystem();
00041 
00042   // Decompositions and Solve routines
00043   int forceRefresh(int decomp = 0);
00044   int cholesky(const Vec<T,M>& b, Vec<T,M>& sol);
00045   int lu(const Vec<T,M>& b, Vec<T,M>& sol);
00046   int gaussianElim(const Vec<T,M>& b, Vec<T,M>& sol);
00047 protected:
00048   // Data
00049   SMat<T,M>& a;
00050   SMat<T,M>* d;
00051   Vec<int,M> indx;
00052   int haveDecomp;
00053   bool overwrite;
00054 }; // end class LinearSystem<T,M>
00055 
00056 // Some common typedefs
00057 typedef LinearSystem<float,2> LinearSystem2f;
00058 typedef LinearSystem<double,2> LinearSystem2d;
00059 typedef LinearSystem<float,3> LinearSystem3f;
00060 typedef LinearSystem<double,3> LinearSystem3d;
00061 typedef LinearSystem<float,4> LinearSystem4f;
00062 typedef LinearSystem<double,4> LinearSystem4d;
00063 typedef LinearSystem<float,5> LinearSystem5f;
00064 typedef LinearSystem<double,5> LinearSystem5d;
00065 typedef LinearSystem<float,6> LinearSystem6f;
00066 typedef LinearSystem<double,6> LinearSystem6d;
00067 
00068 // Constructor/Destructor
00069 
00078 template <class T, int M>
00079 inline LinearSystem<T,M>::LinearSystem(SMat<T,M>& a_, bool overwrite_) :
00080   a(a_), overwrite(overwrite_) {
00081   haveDecomp = 0;
00082   if (!overwrite) d = new SMat<T,M>(a);
00083   else d = &a;
00084   if (LSC_PO>1) std::cout << "Built LinearSystem class instance with ";
00085   if (LSC_PO>1) {
00086     if (overwrite) std::cout << "overwrite enabled.\n";
00087     else std::cout << "overwrite disabled.\n";
00088   }
00089 }
00090 
00093 template <class T, int M>
00094 inline LinearSystem<T,M>::~LinearSystem() {
00095   if (!overwrite) {
00096     delete d;
00097   }
00098   if (LSC_PO>1) std::cout << "Deleted LinearSystem.\n";
00099 }
00100 
00101 // Solve routines
00102 
00113 template <class T, int M>
00114 inline int LinearSystem<T,M>::forceRefresh(int decomp) {
00115   if (decomp == 0 && !haveDecomp) {
00116     std::cerr << "No decomposition specified and none active.\n";
00117     return 1;
00118   }
00119   if (decomp == 0 && haveDecomp) {
00120     decomp = haveDecomp;
00121     haveDecomp = 0;
00122   }
00123   switch (decomp) {
00124   case 1:
00125     if (d->luDecomp(indx)) return 1; else haveDecomp = 1; break;
00126   case 2:
00127     if (d->choleskyDecomp()) return 1; else haveDecomp = 2; break;
00128   default:
00129     std::cerr << "Decomposition number " << decomp << " is not a valid choice.\n"
00130    << "Please choose from 1: LU, 2: Cholesky\n";
00131     return 1;
00132   }
00133 }
00134 
00139 template <class T, int M>
00140 inline int LinearSystem<T,M>::lu(const Vec<T,M>& b, Vec<T,M>& sol) {
00141   if (haveDecomp != 1) {
00142     if (haveDecomp != 0) {
00143 if (!overwrite) { d->set(a); } else {
00144   std::cerr << "Can't perform LU Decomposition because A already "
00145        << "contains ";
00146   switch (haveDecomp) {
00147   case 2: std::cerr << "a Cholesky Decomposition.\n"; break;
00148   case 3: std::cerr << "a Gauss-Jordan Decomposition.\n"; break;
00149   default: std::cerr << "an Unknown decomposition.\n" << haveDecomp; break;
00150   }
00151   return 1;
00152 }
00153     }
00154     if (LSC_PO>1) std::cout << "Performing LU Decomposition...";
00155     if (d->luDecomp(indx)) return 1;
00156     else haveDecomp = 1;
00157     if (LSC_PO>1) std::cout << "Done.\n";
00158   }
00159   if (LSC_PO>1) std::cout << "Performing LU Solve...";
00160   sol.set(b);
00161   d->luSolve(indx,sol);
00162   if (LSC_PO>1) std::cout << "Done.\n";
00163   return 0;
00164 }
00165 
00170 template <class T, int M>
00171 inline int LinearSystem<T,M>::cholesky(const Vec<T,M>& b, Vec<T,M>& sol) {
00172   if (haveDecomp != 2) {
00173     if (haveDecomp != 0) {
00174       if (!overwrite) { d->set(a); } else {
00175         std::cerr << "Can't perform Cholesky Decomposition because A already " << "contains ";
00176         switch (haveDecomp) {
00177           case 1: std::cerr << "an LU Decomposition.\n"; break;
00178           case 3: std::cerr << "a Gauss-Jordan Decomposition.\n"; break;
00179           default: std::cerr << "an Unknown decomposition.\n" << haveDecomp; break;
00180         }
00181         return 1;
00182       }
00183     }
00184     if (LSC_PO>1) std::cout << "Performing Cholesky Decomposition...";
00185     if (d->choleskyDecomp()) return 1;
00186     else haveDecomp = 2;
00187     if (LSC_PO>1) std::cout << "Done.\n";
00188   }
00189   if (LSC_PO>1) std::cout << "Performing Cholesky Solve...";
00190   sol.set(b);
00191   d->choleskySolve(sol);
00192   if (LSC_PO>1) std::cout << "Done.\n";
00193   return 0;
00194 }
00195 
00201 template<class T, int M>
00202 inline int LinearSystem<T,M>::gaussianElim(const Vec<T,M>& b, Vec<T,M>& sol)
00203 {
00204   if (haveDecomp != 0) {
00205     if (!overwrite) {
00206       d->set(a);
00207     } else {
00208       std::cerr << "Can't perform Gauss-Jordan Decomposition because A already "<< "contains ";
00209       switch (haveDecomp) {
00210       case 1:
00211         std::cerr << "an LU Decomposition.\n";
00212         break;
00213       case 2:
00214         std::cerr << "a Cholesky Decomposition.\n";
00215         break;
00216       case 3:
00217         std::cerr << "a different Gauss-Jordan Decomposition\n";
00218         break;
00219       default:
00220         std::cerr << "an Unknown decomposition.\n"<< haveDecomp;
00221         break;
00222       }
00223       return 1;
00224     }
00225   }
00226   haveDecomp = 3;
00227   if (LSC_PO>1)
00228     std::cout << "Performing Gauss-Jordan Decomposition/Solve...";
00229 
00230   T tmp, pvt;
00231   T *t;
00232   T* aa[M];
00233   T bb[M];
00234   int i, j, k;
00235 
00236   // Do decomposition
00237   for (i=0; i<M; i++) {
00238     aa[i] = &((*d)(i, 0));
00239     bb[i] = b.x[i];
00240   }
00241   for (i=0; i<M; i++) {
00242     pvt = aa[i][i];
00243     if (!pvt) {
00244       for (j=i+1; j<M; j++) {
00245         if ((pvt = a(j, i)) != 0.0)
00246           break;
00247       }
00248       if (!pvt)
00249         return 1;
00250       t=aa[j];
00251       aa[j]=aa[i];
00252       aa[i]=t;
00253       tmp=bb[j];
00254       bb[j]=bb[i];
00255       bb[i]=tmp;
00256     }
00257     for (k=i+1; k<M; k++) {
00258       tmp = aa[k][i]/pvt;
00259       for (j=i+1; j<M; j++) {
00260         aa[k][j] -= tmp*aa[i][j];
00261       }
00262       bb[k] -= tmp*bb[i];
00263     }
00264   }
00265   // Do back substitution
00266   for (i=M-1; i>=0; i--) {
00267     sol.x[i]=bb[i];
00268     for (j=M-1; j>i; j--) {
00269       sol.x[i] -= aa[i][j]*sol.x[j];
00270     }
00271     sol.x[i] /= aa[i][i];
00272   }
00273   if (LSC_PO>1)
00274     std::cout << "Done.\n";
00275   return 0;
00276 }
00277 
00278 //==============================================================================
00279 } // NAMESPACE rtc
00280 //==============================================================================
00281 #endif // RTC_LINEARSYSTEM_H defined
00282 //==============================================================================
00283