Robotsystem from Robot Group Leipzig: matrices.h Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# matrices.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: matrices.h,v $
    00023  *   Revision 1.3.4.1  2005/12/06 10:13:26  martius
    00024  *   openscenegraph integration started
    00025  *
    00026  *   Revision 1.3  2005/11/09 13:31:51  martius
    00027  *   GPL'ised
    00028  *
    00029  ***************************************************************************/
    00030 #ifndef MATRICES_H
    00031 #define MATRICES_H
    00032
    00033 #include <vector>
    00034 #include "vector.h"
    00035 #include "exceptions.h"
    00036
    00037 namespace lpzrobots {
    00038
    00039
    00040 template <typename T> class TriDiagonalMatrix;
    00041
    00042 template <typename T>
    00043 class TriDiagonalMatrixPair {
    00044  public:
    00045   TriDiagonalMatrixPair(const TriDiagonalMatrix<T> &r_first,
    00046                         const TriDiagonalMatrix<T> &r_second);
    00047
    00048   TriDiagonalMatrix<T> first;
    00049   TriDiagonalMatrix<T> second;
    00050 };
    00051
    00052
    00053 template<typename T>
    00054 class AbstractMatrix {
    00055  protected:
    00056  public:
    00057 };
    00058
    00059
    00060 template <typename T>
    00061 class Matrix : public AbstractMatrix<T> {
    00062  public:
    00063   typedef std::vector<T> Data;
    00064
    00065  protected:
    00066   Data data;
    00067   unsigned n;
    00068   unsigned m;
    00069
    00070  public:
    00071   Matrix(unsigned n = 0, unsigned m = 0);
    00072
    00073   unsigned get_column_count() const;
    00074   unsigned get_row_count() const;
    00075
    00076   T& operator() (unsigned i, unsigned j);
    00077   const T& operator() (unsigned i, unsigned j) const;
    00078 };
    00079
    00080
    00081
    00082 /**
    00083  * TriDiagonalMatrix
    00084  * N: number of rows
    00085  *
    00086  */
    00087
    00088 template<typename T>
    00089 class TriDiagonalMatrix : public AbstractMatrix<T> {
    00090  public:
    00091   // using class AbstractMatrix<T>;
    00092   typedef std::vector<T> Data;
    00093
    00094  protected:
    00095   Data data;
    00096
    00097  public:
    00098   TriDiagonalMatrix(unsigned dimension);
    00099
    00100   unsigned get_dimension() const;
    00101
    00102
    00103   T& operator() (unsigned i, unsigned j);
    00104   const T& operator() (unsigned i, unsigned j) const;
    00105
    00106   TriDiagonalMatrixPair<T> create_lu_decomposition() const;
    00107 };
    00108
    00109
    00110
    00111
    00112
    00113 template <typename T>
    00114 Vector<T> solve(const TriDiagonalMatrix<T> &r_mat, const Vector<T> &r_vector);
    00115
    00116
    00117 /*****************************************************************************/
    00118 /* IMPLEMENTATIONS                                                           */
    00119 /*****************************************************************************/
    00120
    00121
    00122
    00123 /*****************************************************************************/
    00124 /* Matrix                                                                    */
    00125 /*****************************************************************************/
    00126 template <typename T>
    00127 Matrix<T>::Matrix(unsigned _n, unsigned _m) :
    00128   data(_n * _m),
    00129   n(_n),
    00130   m(_m)
    00131 {
    00132 }
    00133
    00134
    00135 template <typename T>
    00136 unsigned Matrix<T>::get_row_count() const
    00137 {
    00138   return n;
    00139 }
    00140
    00141
    00142 template <typename T>
    00143 unsigned Matrix<T>::get_column_count() const
    00144 {
    00145   return m;
    00146 }
    00147
    00148
    00149 template <typename T>
    00150 T& Matrix<T>::operator() (unsigned i, unsigned j)
    00151 {
    00152   if(i >= n || j >= m)
    00153     IndexOutOfBoundsException().raise();
    00154
    00155   return data[i * m + j];
    00156 }
    00157
    00158
    00159 template <typename T>
    00160 const T& Matrix<T>::operator() (unsigned i, unsigned j) const
    00161 {
    00162   if(i >= n || j >= m)
    00163     IndexOutOfBoundsException().raise();
    00164
    00165   return data[i * m + j];
    00166 }
    00167
    00168 /*****************************************************************************/
    00169 /* TriDiagonalMatrix                                                         */
    00170 /*****************************************************************************/
    00171 // .) solve algorithm base on
    00172 //    http://www.d-fine.de/pool/bibliothek/vl_jra_stoch_6.pdf
    00173 //    unfortunately the formulas for the result vector are wrong >_<
    00174
    00175 template <typename T>
    00176 TriDiagonalMatrixPair<T>::TriDiagonalMatrixPair(const TriDiagonalMatrix<T> &r_first, const TriDiagonalMatrix<T> &r_second) :
    00177   first(r_first),
    00178   second(r_second)
    00179 {
    00180 }
    00181
    00182
    00183 template <typename T>
    00184 TriDiagonalMatrix<T>::TriDiagonalMatrix(unsigned dimension) :
    00185   data(3 * dimension - 2)
    00186 {
    00187   // each row has at most 3 non-zero elements
    00188   // the first and last row only have 2 non-zero elments
    00189 }
    00190
    00191
    00192 template <typename T>
    00193 unsigned TriDiagonalMatrix<T>::get_dimension() const
    00194 {
    00195   // since: size = 3 * dimension - 2
    00196   //        dimension = (size + 2) / 3
    00197   return (data.size() + 2) / 3;
    00198 }
    00199
    00200
    00201 template <typename T>
    00202 T& TriDiagonalMatrix<T>::operator() (unsigned i, unsigned j)
    00203 {
    00204   static T zero = 0;;
    00205
    00206   // formula for addressing an element: adr = 2 * i + j
    00207   // where |i - j| must be less equal 1
    00208
    00209   // make sure i and j are in bounds
    00210   if(i >= data.size() || j >= data.size())
    00211     IndexOutOfBoundsException().raise();
    00212
    00213   // check if a zero element is addressed
    00214   if((i > j) && ((i - j) > 1) ||
    00215      (j > i) && ((j - i) > 1))
    00216     return zero;
    00217
    00218   return data[2 * i + j];
    00219 }
    00220
    00221
    00222 template <typename T>
    00223 const T& TriDiagonalMatrix<T>::operator() (unsigned i, unsigned j) const
    00224 {
    00225   static T zero = 0;;
    00226
    00227   // formula for addressing an element: adr = 2 * i + j
    00228   // where |i - j| must be less equal 1
    00229
    00230   // make sure i and j are in bounds
    00231   if(i >= data.size() || j >= data.size())
    00232     IndexOutOfBoundsException().raise();
    00233
    00234   // check if a zero element is addressed
    00235   if((i > j) && ((i - j) > 1) ||
    00236      (j > i) && ((j - i) > 1))
    00237     return zero;
    00238
    00239   return data[2 * i + j];
    00240 }
    00241
    00242
    00243 /**
    00244  *
    00245  *
    00246  * status: tested! (high chance that this function yields correct results)
    00247  */
    00248 template<class T>
    00249 TriDiagonalMatrixPair<T> TriDiagonalMatrix<T>::create_lu_decomposition() const
    00250 {
    00251   unsigned n = get_dimension(); //data.size();
    00252
    00253   TriDiagonalMatrix<T> mat_l(n);
    00254   TriDiagonalMatrix<T> mat_u(n);
    00255
    00256   // init diagonal of mat_l to 1
    00257   for(unsigned i = 0; i < n; ++i)
    00258     mat_l(i, i) = static_cast<T>(1);
    00259
    00260   // copy c(i) to mat_u
    00261   for(unsigned i = 0; i < n - 1; ++i)
    00262     mat_u(i, i + 1) = (*this)(i, i + 1);
    00263
    00264   // set d0
    00265   mat_u(0, 0) = (*this)(0, 0);
    00266
    00267   for(unsigned i = 1; i < n; ++i) {
    00268     // calculate l(i)
    00269     mat_l(i, i - 1) = (*this)(i, i - 1) / mat_u(i - 1, i - 1);
    00270
    00271     // calculate d's
    00272     mat_u(i, i)     = (*this)(i, i) - mat_l(i, i - 1) * (*this)(i - 1, i);
    00273   }
    00274
    00275   return TriDiagonalMatrixPair<T>(mat_l, mat_u);
    00276 }
    00277
    00278
    00279
    00280 /**
    00281  * solve
    00282  *
    00283  * special solve function for tridiagonal matrices
    00284  *
    00285  * status: tested! (high chance that this function yields correct results)
    00286  */
    00287 template <typename T>
    00288 Vector<T> solve(const TriDiagonalMatrix<T> &r_mat, const Vector<T> &r_vector)
    00289 {
    00290   // check that the dimensions match
    00291   if(r_mat.get_dimension() != r_vector.get_dimension())
    00292     DimensionMismatchException().raise();
    00293
    00294   unsigned n = r_vector.get_dimension();
    00295
    00296   TriDiagonalMatrixPair<T> lu_pair(r_mat.create_lu_decomposition());
    00297
    00298   TriDiagonalMatrix<T> &r_mat_l = lu_pair.first;
    00299   TriDiagonalMatrix<T> &r_mat_u = lu_pair.second;
    00300
    00301
    00302   // determine the y's (the intermediate vector)
    00303   Vector<T> y(n);
    00304
    00305   y(0) = r_vector(0);
    00306   for(unsigned i = 1; i < n; ++i)
    00307     y(i) = r_vector(i) - r_mat_l(i, i - 1) * y(i - 1);
    00308
    00309   // determine the x's (the result vector)
    00310   Vector<T> x(n);
    00311
    00312   x(n - 1) = y(n - 1) / r_mat_u(n - 1, n - 1);
    00313   for(unsigned i = n - 2; i != static_cast<unsigned>(~0); --i)
    00314     x(i) = (y(i) - r_mat_u(i, i + 1) * x(i + 1)) / r_mat_u(i, i);
    00315
    00316   return x;
    00317 }
    00318
    00319
    00320 }
    00321
    00322
    00323 #endif