SmoothCurve.cc
Go to the documentation of this file.
00001 #include <art_map/SmoothCurve.h>
00002 
00003 
00004 SmoothCurve::SmoothCurve(const std::vector<Point2f>& ctrl, 
00005                          float starttheta, float startspeed,
00006                          float endtheta, float endspeed, bool use_pats)
00007 {
00008   // use_pats uses Patrick's change the curves to use derivative
00009   // based on one neighboring waypoint instead of two.  The one
00010   // neighboring waypoint is chosen by looking for the one that is
00011   // part of the straightest path with the current waypoint.
00012 
00013   startspeed = fmax(1,startspeed);
00014   endspeed = fmax(1,endspeed);
00015 
00016   degree=3;
00017   
00018   dataPoints=ctrl;
00019   //CONTROLPOINTS
00020   int n =(dataPoints.size()-1)*2;
00021   controlPoints.resize(n);
00022 
00023   for (int i = 0; i < (int)controlPoints.size(); i++)
00024     {
00025       controlPoints[i] = dataPoints[(i + 1 )/ 2];
00026     }
00027     
00028 
00029   n = dataPoints.size();
00030   float speed = (startspeed+endspeed) / 2.0;
00031   std::vector<Point2f> M;
00032   M.resize(n);
00033   M[0] = Vec2f(cosf(starttheta),sinf(starttheta))*startspeed;
00034   M[n-1] = Vec2f(cosf(endtheta),sinf(endtheta))*endspeed;
00035   for (int i = 1; i < n-1;i++)
00036     {
00037       Vec2f a = dataPoints[i] - dataPoints[i-1];
00038       Vec2f b = dataPoints[i+1] - dataPoints[i];
00039       a.normalize();
00040       b.normalize();
00041       Vec2f top=a+b;
00042       if (use_pats && i >1 && i <n-2) {
00043         Vec2f a2 = dataPoints[i] - dataPoints[i-2];
00044         Vec2f b2 = dataPoints[i+2] - dataPoints[i];
00045         a2.normalize();
00046         b2.normalize();
00047         float adot=a.dot(a2);
00048         float bdot=b.dot(b2);
00049         float ratio=adot/bdot;
00050         if (Epsilon::lte(ratio,.9775))
00051           top=b;
00052         else if (Epsilon::gte(ratio,1/.9775))
00053           top=a;
00054         /*
00055           if (Epsilon::equal(adot,bdot))
00056           top = a+b;
00057           else if(adot < bdot)
00058           top=b;
00059           else top=a;
00060         */
00061       }
00062       top.normalize();
00063       M[i] = top*speed;
00064     }
00065   
00066   //Really Estimate Path Length And Calculate Knots
00067   knots.resize(ctrl.size());
00068   knots[0]=0;
00069   for(unsigned int i=1;i<knots.size();i++){
00070     Vec2f a = dataPoints[i] - dataPoints[i-1];
00071     float alen=a.length();
00072 
00073     Vec2f v1 = M[i-1]; 
00074     Vec2f v2 = M[i];
00075 
00076     a.normalize();
00077     v1.normalize();
00078     v2.normalize();
00079     
00080     float theta1 = acosf(fmax(-1.0,fmin(1.0,a.dot(v1))));
00081     float theta2 = acosf(fmax(-1.0,fmin(1.0,a.dot(v2))));
00082 
00083     float l1 = 0, l2 = 0;
00084     float sinth1=sinf(theta1);
00085     if (Epsilon::equal(sinth1,0.0)) 
00086       l1 = alen;
00087     else
00088       l1 = fabs(alen*theta1/sinth1);
00089 
00090     
00091     float sinth2=sinf(theta2);
00092     if (Epsilon::equal(sinth2,0.0)) 
00093       l2 = alen;
00094     else
00095       l2 = fabs(alen*theta2/sinth2);
00096 
00097 
00098     float r= l1 + l2;
00099     
00100     if(i==1){
00101       r/=(startspeed+speed);
00102     }
00103     else if(i==(knots.size()-1)){
00104       r/=(endspeed+speed);
00105     }
00106     else{
00107       r/=(2.0*speed);
00108     }
00109     
00110     
00111     knots[i]=knots[i-1]+r;
00112   }
00113   
00114   //for(int i=0;i<(int)knots.size();i++){
00115   //  printf("Knots %d %f",i,knots[i]);
00116   //}
00117   
00118   //Convert from Hermite to Bezier
00119   for(int i=0;i< (int)controlPoints.size();i++){
00120     Vec2f tempVec1 = M[(i+1)/2];
00121     float del;
00122     //printf("Vector for Derivative: %f, %f\n", tempVec1[0], tempVec1[1]);
00123     if(i%2==0){
00124       del = (DeltaU(i/2)/((float)degree));
00125       controlPoints[i]+= tempVec1 * del;
00126     }           
00127     else{
00128       del = (DeltaU((i-1)/2)/((float)degree));
00129       controlPoints[i]-= tempVec1 * del;
00130     }
00131   }
00132   
00133 }
00134   
00135 SmoothCurve::~SmoothCurve() {}
00136 
00137 float SmoothCurve::DeltaU (int i) 
00138 { 
00139   if (i >= (int)knots.size()-1)
00140     {
00141       std::cerr << "Delta() - array out of bounds" << std::endl;
00142       exit(1);
00143     }
00144   else  
00145     return knots[i+1] - knots[i];
00146 }
00147 
00148 Point2f SmoothCurve::bezierPoint(int i)
00149 {
00150   Point2f temp;
00151   switch(i%3){
00152   case 0:
00153     temp=dataPoints[i/3];
00154     break;
00155   case 1:
00156     temp=controlPoints[2*(i/3)];
00157     break;
00158   default:
00159     temp=controlPoints[2*(i/3)+1];    
00160   }
00161   return temp;
00162 }
00163   
00164 Vec2f SmoothCurve::Delta(int i)
00165 {
00166   return bezierPoint(i+1)-bezierPoint(i);
00167 }
00168   
00169 
00170 int SmoothCurve::knotCount() const 
00171 {
00172   return knots.size();
00173 }
00174     
00175 int SmoothCurve::controlPointsCount() const 
00176 {
00177   return controlPoints.size();
00178 }    
00179     
00180 int SmoothCurve::dataPointsCount() const 
00181 {
00182   return dataPoints.size();
00183 }
00184 
00185 Point2f SmoothCurve::getDataPoint(int index) const 
00186 {
00187   return dataPoints[index];
00188 }
00189         
00190 Point2f SmoothCurve::getControlPoint(int index) const 
00191 {
00192   return controlPoints[index];
00193 }
00194 
00195 float SmoothCurve::getKnot(int index) const 
00196 {
00197   return knots[index];
00198 }
00199     
00200 void SmoothCurve::clear()
00201 {
00202   dataPoints.clear();
00203   knots.clear();
00204   controlPoints.clear();
00205 }
00206     
00207 
00208 float SmoothCurve::curveLength() 
00209 {
00210   return knots.empty() ? 0 : knots.back();
00211 }
00212     
00213 Point2f SmoothCurve::evaluatePoint(float time)
00214 {
00215 
00216   if (dataPoints.empty())
00217     return Point2f();
00218 
00219   unsigned int i=0;
00220   
00221   while (i < knots.size() &&
00222          knots[i] <= time)
00223     i++;
00224 
00225   if (i == 0)
00226     return dataPoints[0];
00227   if (i >= knots.size())
00228     return dataPoints.back();
00229       
00230   // Curves and Surfaces for Computer Aided Geometric Design 1st Edition
00231   // Gerald Farin 
00232   // Chapter 4 p 34
00233   Point2f sum(0,0);
00234   std::vector<float> b(degree+1,0);
00235   float u = (time-knots[i-1])/(knots[i]-knots[i-1]);
00236   
00237   b[0]=powf(1-u,3);
00238   b[1]=3*(u*powf(1-u,2));
00239   b[2]=3*(powf(u,2)*powf(1-u,1));
00240   b[3]=powf(u,3);
00241   
00242   i=(i-1)*3;
00243   for(int j=0;j<degree+1;j++)
00244     sum+=bezierPoint(i+j)*b[j];
00245   return sum;
00246 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:41:51