BSplineND.h
Go to the documentation of this file.
00001 //-----------------------------------------------
00002 // Neobotix 
00003 /***********************************************************************
00004  *                                                                     *
00005  *  written by Felix Geibel May 2009                                   *
00006  *  at Fraunhofer IPA                                                  *
00007  *                                                                     *
00008  *  based on BSpline2d by Neobotix (see below)                         *
00009  *                                                                     *
00010  ***********************************************************************/
00011 
00012 // www.neobotix.de
00013 // Copyright (c) 2003. All rights reserved.
00014 
00015 // author: Bertram Rohrmoser
00016 //-----------------------------------------------
00017 
00018 #ifndef BSPLINE_ND_H
00019 #define BSPLINE_ND_H
00020 //-----------------------------------------------
00021 
00022 #include <vector>
00023 #include <cmath>
00024 #include <assert.h>
00025 
00026 #define BSPLINE_TINY 1e-20
00027 
00028 
00036 template <class PointND>
00037 class BSplineND
00038 {
00039 public:
00040         //----------------------- Interface
00041         BSplineND();
00042 
00043         ~BSplineND();
00044 
00045         void setCtrlPoints(const std::vector<PointND>& ctrlPointVec );
00046 
00047         // main functions
00048         bool ipoWithConstSampleDist(double dIpoDist, std::vector<PointND>& ipoVec);
00049 
00050         bool ipoWithNumSamples(int iNumPts, std::vector<PointND>& ipoVec);
00051         
00052         void eval(double dPos, PointND& point);
00053         
00054         double getMaxdPos() const { return m_dLength; }
00055 
00056 private:
00057         //----------------------- Parameters
00058         double m_iGrad;
00059 
00060         //----------------------- Variables
00061         std::vector<PointND> m_CtrlPointVec;
00062 
00063         std::vector<double> m_KnotVec;
00064 
00065         double m_dLength;
00066 
00067         //----------------------- Member functions
00068         double evalBasis(double t, unsigned int i, int n);
00069 
00070 };
00071 
00072 
00073 /*****************************************************************************
00074  *                                                                           *
00075  *  Implementierung bei TemplateKlassen im Headerfile                        *
00076  *                                                                           *
00077  *****************************************************************************/
00078 
00079 template <class PointND>
00080 inline BSplineND<PointND>::BSplineND()
00081 {
00082         m_iGrad = 3;
00083         m_dLength = 0;
00084 }
00085 
00086 //-----------------------------------------------
00087 template <class PointND>
00088 BSplineND<PointND>::~BSplineND()
00089 {
00090 }
00091 
00092 //-----------------------------------------------
00093 template <class PointND>
00094 void BSplineND<PointND>::setCtrlPoints(const std::vector<PointND>& ctrlPointVec )
00095 {
00096         int iNumCtrlPoint;
00097         int i;
00098         double d;
00099 
00100         m_CtrlPointVec = ctrlPointVec;
00101         iNumCtrlPoint = m_CtrlPointVec.size();
00102 
00103         if (iNumCtrlPoint < m_iGrad)
00104     {
00105         return;
00106     }
00107 
00108         m_KnotVec.resize( iNumCtrlPoint + m_iGrad );
00109 
00110         // Calculate knots
00111         for(i = 0; i< m_iGrad; i++)
00112         {
00113                 m_KnotVec[i] = 0;
00114         }
00115 
00116         int iNumInternalKnots = iNumCtrlPoint - m_iGrad;
00117         for(i=0; i<iNumInternalKnots; i++)
00118         {
00119                 double Distance1 = 0.0;
00120                 for(unsigned int k = 0; k < m_CtrlPointVec[i+1].size(); k++)
00121                 {
00122                         Distance1 += (m_CtrlPointVec[i+1].at(k) - m_CtrlPointVec[i+2].at(k)) * (m_CtrlPointVec[i+1].at(k) - m_CtrlPointVec[i+2].at(k));
00123                 }
00124                 d= m_KnotVec[i+m_iGrad-1] + sqrt(Distance1);
00125                 // OLD WITH JOINTD d= m_KnotVec[i+m_iGrad-1] + Distance( m_CtrlPointVec[i+1], m_CtrlPointVec[i+2] );
00126                 m_KnotVec[i+m_iGrad] = d;
00127         }
00128         double Distance2 = 0.0;
00129         for(unsigned int k = 0; k <  m_CtrlPointVec[iNumInternalKnots+1].size(); k++)
00130         {
00131                 Distance2 += ( m_CtrlPointVec[iNumInternalKnots+1].at(k) - m_CtrlPointVec[iNumInternalKnots+2].at(k)) * ( m_CtrlPointVec[iNumInternalKnots+1].at(k) - m_CtrlPointVec[iNumInternalKnots+2].at(k));
00132         }
00133         d = m_KnotVec[iNumCtrlPoint-1] + sqrt(Distance2);
00134         // OLD WITH JOINTD d = m_KnotVec[iNumCtrlPoint-1] + Distance( m_CtrlPointVec[iNumInternalKnots+1], m_CtrlPointVec[iNumInternalKnots+2] );
00135 
00136         for(i = 0; i< m_iGrad; i++)
00137         {
00138                 m_KnotVec[i+iNumCtrlPoint] = d;
00139         }
00140         
00141         // This is not the arc-length but the maximum of the spline parameter.
00142         // eval(m_dLength, Point) should return the last point of the spline
00143         m_dLength = d;
00144 
00145 }
00146 
00147 
00148 //-----------------------------------------------
00149 template <class PointND>
00150 void BSplineND<PointND>::eval(double dPos, PointND& point)
00151 {
00152         double dFak;
00153 
00154         for(unsigned int i = 0; i<point.size(); i++)
00155                 point.at(i) = 0.0;
00156         for(unsigned int i = 0; i < m_CtrlPointVec.size(); i++)
00157         {
00158                 dFak = evalBasis(dPos, i, m_iGrad);
00159                 for(unsigned int j = 0; j<point.size(); j++)
00160                         point.at(j) += m_CtrlPointVec[i][j] * dFak;  // !!!! TODO this might be wrong due to change from JointD to std::vector
00161         }
00162 }
00163 
00164 
00165 //-----------------------------------------------
00166 template <class PointND>
00167 bool BSplineND<PointND>::ipoWithConstSampleDist(double dIpoDist, std::vector<PointND >& ipoVec)
00168 {
00169         PointND buf = m_CtrlPointVec.front();
00170         int iNumOfPoints;
00171         double dPos;
00172         //int iStart, iNextStart;
00173 
00174         if (m_CtrlPointVec.size() < m_iGrad)
00175         {
00176                 ipoVec = m_CtrlPointVec;
00177                 return false;
00178         }
00179         
00180         // Felix: Dies ist falsch? m_KnotVec.back() enthält nicht die tatsächliche 
00181         // Bogenlänge entlang des Splines. Folge: Ergebnisvektor ist am Ende abge-
00182         // schnitten.
00183         iNumOfPoints = m_KnotVec.back() / dIpoDist + 2;
00184         ipoVec.resize(iNumOfPoints);
00185 
00186         // Calculate x- and y-coordinates
00187         dPos = 0;
00188         for(int i=0; i < iNumOfPoints -1 ; i++)
00189         {
00190                 eval(dPos, buf);
00191                 ipoVec[i] = buf;
00192                 dPos += dIpoDist;
00193         }
00194 
00195         ipoVec.back() = m_CtrlPointVec.back();
00196 
00197         return true;
00198 }
00199 
00200 /*
00201 //-----------------------------------------------
00202 //Florian
00203 bool BSplineND<PointND>::ipoWithConstSampleDist(double dIpoDist, OmniPath& ipoVec)
00204 {
00205         Neo::Vector2D buf;
00206         int iNumOfPoints;
00207         int iNumOfCtrlPoints;
00208         double dPos, viewang;
00209         double dx, dy, da;
00210         int i;
00211         bool bRet = true;
00212         int iStart, iNextStart;
00213 
00214         iNumOfCtrlPoints = m_OmniCtrlPointVec.getSize();
00215 
00216         if ( iNumOfCtrlPoints < m_iGrad )
00217         {
00218                 bRet = false;
00219 
00220                 iNumOfPoints = m_OmniCtrlPointVec.getSize();
00221                 ipoVec.clearVec();
00222                 for(i=0; i<iNumOfPoints; i++)
00223                 {
00224                         ipoVec.addFrame(m_OmniCtrlPointVec.m_VecPathPnt[i].Frm,m_OmniCtrlPointVec.m_VecPathPnt[i].ViewAng);
00225                 }
00226 
00227                 if ( iNumOfCtrlPoints < 2 )
00228                 {
00229                         return bRet;
00230                 }
00231         }
00232         else
00233         {
00234                 iNumOfPoints = m_KnotVec.back() / dIpoDist + 2;
00235                 ipoVec.m_VecPathPnt.resize(iNumOfPoints);
00236 
00237                 // Calculate x- and y-coordinates
00238                 dPos = 0;
00239                 iStart = 0;
00240                 for(i=0; i < iNumOfPoints -1 ; i++)
00241                 {
00242 //                      eval(dPos, buf);
00243                         eval(dPos, buf, iStart, iNextStart, viewang);
00244                         iStart = iNextStart;
00245 
00246                         ipoVec.m_VecPathPnt[i].Frm.x() = buf.x();
00247                         ipoVec.m_VecPathPnt[i].Frm.y() = buf.y();
00248                         ipoVec.m_VecPathPnt[i].ViewAng=viewang;
00249                         dPos += dIpoDist;
00250                 }
00251 
00252                 ipoVec.m_VecPathPnt.back().Frm.x() = m_OmniCtrlPointVec.m_VecPathPnt.back().Frm.x();
00253                 ipoVec.m_VecPathPnt.back().Frm.y() = m_OmniCtrlPointVec.m_VecPathPnt.back().Frm.y();
00254                 ipoVec.m_VecPathPnt.back().ViewAng = m_OmniCtrlPointVec.m_VecPathPnt.back().ViewAng;
00255         }
00256 
00257         // Calculate angle
00258         for(i=0; i < iNumOfPoints-1; i++)
00259         {
00260                 dx = ipoVec.m_VecPathPnt[i+1].Frm.x() - ipoVec.m_VecPathPnt[i].Frm.x();
00261                 dy = ipoVec.m_VecPathPnt[i+1].Frm.y() - ipoVec.m_VecPathPnt[i].Frm.y();
00262                 da = atan2(dy, dx);
00263                 MathSup::normalizePi(da);
00264                 ipoVec.m_VecPathPnt[i].Frm.a() = da;
00265         }
00266         ipoVec.m_VecPathPnt.back().Frm.a() = da;
00267 
00268         return bRet;
00269 }
00270 */
00271 
00272 //-----------------------------------------------
00273 template <class PointND>
00274 bool BSplineND<PointND>::ipoWithNumSamples(int iNumOfPoints, std::vector<PointND >& ipoVec)
00275 {
00276         PointND buf;
00277         double dPos, dInc;
00278 
00279         if (m_CtrlPointVec.size() < m_iGrad)
00280         {
00281                 ipoVec = m_CtrlPointVec;
00282                 return false;
00283         }
00284 
00285         dInc = m_KnotVec.back() / (double)(iNumOfPoints-1);
00286         ipoVec.resize(iNumOfPoints);
00287 
00288         // Calculate x- and y-coordinates
00289         dPos = 0;
00290         for(int i=0; i < iNumOfPoints -1 ; i++)
00291         {
00292                 eval(dPos, buf);
00293 
00294                 ipoVec[i] = buf;
00295                 dPos += dInc;
00296         }
00297 
00298         ipoVec.back() = m_CtrlPointVec.back();
00299         return true;
00300 }
00301 
00302 
00303 template <class PointND>
00304 double BSplineND<PointND>::evalBasis(double u, unsigned int i, int n)
00305 {
00306         assert(i >= 0);
00307         assert(i < m_KnotVec.size() - 1 );
00308 
00309         if (n==1)
00310         {
00311                 if ( (m_KnotVec[i]<=u) && ( u<m_KnotVec[i+1]) )
00312                 {
00313                         return 1.0;
00314                 }
00315                 else
00316                 {
00317                         return 0.0;
00318                 }
00319         }
00320         else
00321         {
00322                 double N;
00323                 double dNum1, dNum2;
00324                 double dDen1, dDen2;
00325                 
00326                 dDen1 = u - m_KnotVec[i];
00327                 dNum1 = m_KnotVec[i+n-1] - m_KnotVec[i];
00328 
00329                 dDen2 = m_KnotVec[i+n] - u;
00330                 dNum2 = m_KnotVec[i+n] - m_KnotVec[i+1];
00331 
00332                 if ( (fabs(dNum1) > BSPLINE_TINY)&&(fabs(dNum2) > BSPLINE_TINY) )
00333                 {
00334                         N = dDen1 / dNum1 * evalBasis(u, i , n-1);
00335                         N += dDen2 / dNum2 * evalBasis(u, i+1 , n-1);
00336                 }
00337                 else if ( (fabs(dNum1) < BSPLINE_TINY)&&(fabs(dNum2) > BSPLINE_TINY) )
00338                 {
00339                         N = dDen2 / dNum2 * evalBasis(u, i+1 , n-1);
00340                 }
00341                 else if ((fabs(dNum1) > BSPLINE_TINY)&&(fabs(dNum2) < BSPLINE_TINY))
00342                 {
00343                         N = dDen1 / dNum1 * evalBasis(u, i , n-1);
00344                 }
00345                 else
00346                 {
00347                         N = 0;
00348                 }
00349 
00350                 return N;
00351         }
00352 }
00353 
00354 
00355 
00356 #endif


cob_trajectory_controller
Author(s): Alexander Bubeck
autogenerated on Tue Mar 3 2015 15:12:41