Robotsystem from Robot Group Leipzig: cubic_spline.h Source File

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

- File List
- File Members

# cubic_spline.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: cubic_spline.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 CUBIC_SPLINE_H
    00031 #define CUBIC_SPLINE_H
    00032
    00033
    00034 #include <vector>
    00035
    00036 #include "exceptions.h"
    00037 #include "vector.h"
    00038 #include "matrices.h"
    00039
    00040 namespace lpzrobots {
    00041
    00042
    00043 template<typename T>
    00044 class CubicSpline {
    00045  protected:
    00046  typedef std::vector< Matrix<T> > MatrixSet;
    00047
    00048  protected:
    00049   MatrixSet coefficient_matrix_set; // one matrix for each component
    00050   Vector<T> parameter_vector;       // size of this is col_count of matrix + 1
    00051
    00052  public:
    00053   CubicSpline();
    00054
    00055
    00056   void create(const Matrix<T> &r_matrix);
    00057
    00058   Vector<T> get_point(T parameter);
    00059
    00060
    00061   T get_distant_point_parameter(T parameter, T distance);
    00062 };
    00063
    00064
    00065 template<class T>
    00066 CubicSpline<T>::CubicSpline()
    00067 {
    00068 }
    00069
    00070 /**
    00071  * .) each component is processed individually
    00072  *
    00073  * .) calculate the delta_t s:
    00074  *      delta_t[0] = t[2] - t[0]
    00075  *      delta_t[i] = t[i + 1] - t[i] | i >= 1 .. n
    00076  *
    00077  * .) calculate the ks:
    00078  *      k[i] = (f[i] - f[i - 1]) / delta_t
    00079  *
    00080  *
    00081  * result matrix: 4 x (n - 1)
    00082  * 0: a = f
    00083  * 1: b
    00084  * 2: c
    00085  * 3: d
    00086  * 4: t
    00087  *
    00088  *
    00089  * input : a matrix, first row must be the t parameters
    00090  * output: is stored internally,
    00091  *         for each component a matrix consisting of a, b, c and d is created
    00092  *         the t parameter is stored seperately
    00093  */
    00094
    00095 template<class T>
    00096 void CubicSpline<T>::create(const Matrix<T> &r_matrix)
    00097 {
    00098   unsigned n = r_matrix.get_column_count();
    00099   unsigned m = r_matrix.get_row_count();
    00100
    00101   // create the matrix for the c coefficents
    00102   TriDiagonalMatrix<T> mat_c(n - 1);
    00103
    00104   coefficient_matrix_set.resize(m - 1);
    00105
    00106   parameter_vector.resize(n);
    00107
    00108   // copy the parameters
    00109   for(unsigned i = 0; i < n; ++i)
    00110     parameter_vector(i) = r_matrix(0, i);
    00111
    00112
    00113   // for each component...
    00114   for(unsigned m = 1; m < r_matrix.get_row_count(); ++m) {
    00115
    00116     // temporarily this x vector is created
    00117     // its simply holds the values of a specific component
    00118     Vector<T> x(n);
    00119     for(unsigned i = 0; i < n; ++i)
    00120       x(i) = r_matrix(m, i);
    00121
    00122
    00123     Matrix<T> coefficient_matrix(4, n - 1);
    00124     Vector<T> h(n - 1);
    00125     TriDiagonalMatrix<T> mat_c(n);
    00126
    00127
    00128     // copy the function values into the coefficient matrix
    00129     for(unsigned i = 0; i < n - 1; ++i)
    00130       coefficient_matrix(0, i) = r_matrix(m, i);
    00131
    00132
    00133     // calculate the h's
    00134     for(unsigned i = 0; i < n - 1; ++i)
    00135       h(i) = parameter_vector(i + 1) - parameter_vector(i);
    00136
    00137
    00138     Vector<T>               r(n);
    00139
    00140     //mat_c(0, n - 1) = h(n - 1);
    00141     mat_c(0, 0)     = 2 * h(0);
    00142     mat_c(0, 1)     = h(0);
    00143     r(0)            = 3 * (x(1) - x(0)) / h(0);
    00144
    00145     for(unsigned i = 1; i < n - 1; ++i) {
    00146       mat_c(i, i - 1) = h(i - 1);
    00147       mat_c(i, i    ) = 2 * (h(i - 1) + h(i));
    00148       mat_c(i, i + 1) = h(i);
    00149
    00150       r(i) = 3 * (x(i + 1) - x(i)) / h(i) - 3 * (x(i) - x(i - 1)) / h(i - 1);
    00151     }
    00152
    00153     mat_c(n - 1, n - 2) = h(n - 2);
    00154     mat_c(n - 1, n - 1) = 2 * h(n - 2);
    00155
    00156     r(n - 1) = - 3 * (x(n - 1) - x(n - 2)) / h(n - 2);
    00157
    00158     // calculate the c's
    00159     Vector<T> c(solve(mat_c, r));
    00160
    00161     // copy the c's into the coefficient matrix
    00162     // of course it would be nice getting rid of having to copy here
    00163     for(unsigned i = 0; i < n - 1; ++i)
    00164       coefficient_matrix(2, i) = c(i);
    00165
    00166
    00167     // calculate the b's
    00168     for(unsigned i = 0; i < n - 1; ++i)
    00169       coefficient_matrix(1, i) = (x(i + 1) - x(i)) / h(i) - h(i) * (c(i + 1) + 2 * c(i)) / 3;
    00170
    00171     // coefficient_matrix(3, n - 2) = (x(0) - x(n - 1)) / h(n - 1) - h(n) * (c(0) + 2 * c(n - 1)) / 3;
    00172
    00173
    00174     // calculate the d's
    00175     for(unsigned i = 0; i < n - 1; ++i)
    00176       coefficient_matrix(3, i) = (c(i + 1) - c(i)) / 3 * h(i);
    00177     //coefficient_matrix(4, n - 1) = (c(0) - c(n - 1)) / (3 * h(n - 1));
    00178
    00179
    00180     *(coefficient_matrix_set.begin() + (m - 1)) = coefficient_matrix;
    00181   }
    00182
    00183 }
    00184
    00185
    00186 template<class T>
    00187 Vector<T> CubicSpline<T>::get_point(T parameter)
    00188 {
    00189   if(parameter < parameter_vector(0))
    00190     IndexOutOfBoundsException().raise();
    00191
    00192   if(parameter > parameter_vector( parameter_vector.get_dimension() - 1 ))
    00193     IndexOutOfBoundsException().raise();
    00194
    00195   unsigned index = 0;
    00196   while(parameter >= parameter_vector(index))
    00197     ++index;
    00198
    00199   --index;
    00200
    00201   Vector<T> result( coefficient_matrix_set.size() );
    00202   T delta_t = parameter - parameter_vector(index);
    00203
    00204   for(unsigned i = 0; i < coefficient_matrix_set.size(); ++i) {
    00205     Matrix<T> &r_coefficient_matrix = coefficient_matrix_set[i];
    00206
    00207     result(i) = r_coefficient_matrix(0, index) +
    00208                 r_coefficient_matrix(1, index) * delta_t +
    00209                 r_coefficient_matrix(2, index) * delta_t * delta_t +
    00210                 r_coefficient_matrix(3, index) * delta_t * delta_t * delta_t;
    00211   }
    00212
    00213   return result;
    00214 }
    00215
    00216
    00217
    00218 template<class T>
    00219 T CubicSpline<T>::get_distant_point_parameter(T parameter, T distance)
    00220 {
    00221   Vector<T> start_point(get_point(parameter));
    00222   Vector<T> end_point(coefficient_matrix_set.size());
    00223
    00224   T square_distance = distance * distance;
    00225
    00226   T s = parameter;
    00227   for(;;) {
    00228     end_point = get_point(s);
    00229
    00230     if((end_point - start_point).square_length() >= square_distance)
    00231       return s;
    00232
    00233     s += 0.001;
    00234     if(s > parameter_vector( parameter_vector.get_dimension() - 1))
    00235       return s;
    00236   }
    00237
    00238 }
    00239
    00240
    00241
    00242 }
    00243
    00244
    00245 #endif