BSplineND.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
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 Thu Jun 6 2019 21:19:22