_bezier.h

Go to the documentation of this file.
00001 
00025 /* === S T A R T =========================================================== */
00026 
00027 #ifndef __ETL_BEZIER_H
00028 #define __ETL_BEZIER_H
00029 
00030 /* === H E A D E R S ======================================================= */
00031 
00032 #include "_curve_func.h"
00033 #include <ETL/fixed>
00034 
00035 /* === M A C R O S ========================================================= */
00036 
00037 /* === T Y P E D E F S ===================================================== */
00038 
00039 /* === C L A S S E S & S T R U C T S ======================================= */
00040 
00041 _ETL_BEGIN_NAMESPACE
00042 
00043 template<typename V,typename T> class bezier;
00044 
00046 // This generic implementation uses the DeCasteljau algorithm.
00047 // Works for just about anything that has an affine combination function
00048 template <typename V,typename T=float>
00049 class bezier_base : public std::unary_function<T,V>
00050 {
00051 public:
00052     typedef V value_type;
00053     typedef T time_type;
00054 
00055 private:
00056     value_type a,b,c,d;
00057     time_type r,s;
00058 
00059 protected:
00060     affine_combo<value_type,time_type> affine_func; 
00061 
00062 public:
00063     bezier_base():r(0.0),s(1.0) { }
00064     bezier_base(
00065         const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00066         const time_type &r=0.0, const time_type &s=1.0):
00067         a(a),b(b),c(c),d(d),r(r),s(s) { sync(); }
00068         
00069     void sync()
00070     {
00071     }
00072 
00073     value_type
00074     operator()(time_type t)const
00075     {
00076         t=(t-r)/(s-r);
00077         return
00078         affine_func(
00079             affine_func(
00080                 affine_func(a,b,t),
00081                 affine_func(b,c,t)
00082             ,t),
00083             affine_func(
00084                 affine_func(b,c,t),
00085                 affine_func(c,d,t)
00086             ,t)
00087         ,t);
00088     }
00089     
00090     /*
00091     void evaluate(time_type t, value_type &f, value_type &df) const
00092     {
00093         t=(t-r)/(s-r);
00094         
00095         value_type p1 = affine_func(
00096                             affine_func(a,b,t),
00097                             affine_func(b,c,t)
00098                             ,t);
00099         value_type p2 = affine_func(
00100                             affine_func(b,c,t),
00101                             affine_func(c,d,t)
00102                         ,t);
00103         
00104         f = affine_func(p1,p2,t);
00105         df = (p2-p1)*3;
00106     }
00107     */
00108     
00109     void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; }
00110     void set_r(time_type new_r) { r=new_r; }
00111     void set_s(time_type new_s) { s=new_s; }
00112     const time_type &get_r()const { return r; }
00113     const time_type &get_s()const { return s; }
00114     time_type get_dt()const { return s-r; }
00115 
00116     bool intersect_hull(const bezier_base<value_type,time_type> &x)const
00117     {
00118         return 0;
00119     }
00120 
00122 
00142     time_type intersect(const bezier_base<value_type,time_type> &x, time_type near=0.0)const
00143     {
00144         return 0;
00145     }
00146     
00147     /* subdivide at some time t into 2 separate curves left and right
00148     
00149         b0 l1
00150         *       0+1 l2
00151         b1      *       1+2*1+2 l3
00152         *       1+2     *           0+3*1+3*2+3 l4,r1
00153         b2      *       1+2*2+2 r2  *
00154         *       2+3 r3  *
00155         b3 r4   *
00156         *       
00157         
00158         0.1 2.3 ->  0.1 2 3 4 5.6
00159     */
00160 /*  void subdivide(bezier_base *left, bezier_base *right, const time_type &time = (time_type)0.5) const
00161     {
00162         time_type t = (time-r)/(s-r);
00163         bezier_base lt,rt;
00164         
00165         value_type temp;
00166 
00167         //1st stage points to keep      
00168         lt.a = a;
00169         rt.d = d;
00170 
00171         //2nd stage calc
00172         lt.b = affine_func(a,b,t);
00173         temp = affine_func(b,c,t);
00174         rt.c = affine_func(c,d,t);
00175                 
00176         //3rd stage calc
00177         lt.c = affine_func(lt.b,temp,t);
00178         rt.b = affine_func(temp,rt.c,t);
00179         
00180         //last stage calc
00181         lt.d = rt.a = affine_func(lt.c,rt.b,t);
00182         
00183         //set the time range for l,r (the inside values should be 1, 0 respectively)
00184         lt.r = r;
00185         rt.s = s;
00186         
00187         //give back the curves
00188         if(left) *left = lt;
00189         if(right) *right = rt; 
00190     }
00191     */  
00192     value_type &
00193     operator[](int i)
00194     { return (&a)[i]; }
00195 
00196     const value_type &
00197     operator[](int i) const
00198     { return (&a)[i]; }
00199 };
00200 
00201 
00202 #if 1
00203 // Fast float implementation of a cubic bezier curve
00204 template <>
00205 class bezier_base<float,float> : public std::unary_function<float,float>
00206 {
00207 public:
00208     typedef float value_type;
00209     typedef float time_type;
00210 private:
00211     affine_combo<value_type,time_type> affine_func; 
00212     value_type a,b,c,d;
00213     time_type r,s;
00214 
00215     value_type _coeff[4];
00216     time_type drs; // reciprocal of (s-r)
00217 public:
00218     bezier_base():r(0.0),s(1.0),drs(1.0) { }
00219     bezier_base(
00220         const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00221         const time_type &r=0.0, const time_type &s=1.0):
00222         a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
00223 
00224     void sync()
00225     {
00226 //      drs=1.0/(s-r);
00227         _coeff[0]=                 a;
00228         _coeff[1]=           b*3 - a*3;
00229         _coeff[2]=     c*3 - b*6 + a*3;
00230         _coeff[3]= d - c*3 + b*3 - a;
00231     }
00232     
00233     // Cost Summary: 4 products, 3 sums, and 1 difference.
00234     inline value_type
00235     operator()(time_type t)const
00236     { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
00237 
00238     void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
00239     void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
00240     void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
00241     const time_type &get_r()const { return r; }
00242     const time_type &get_s()const { return s; }
00243     time_type get_dt()const { return s-r; }
00244 
00246 
00249     time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
00250     {
00251         //BROKEN - the time values of the 2 curves should be independent
00252         value_type system[4];
00253         system[0]=_coeff[0]-x._coeff[0];
00254         system[1]=_coeff[1]-x._coeff[1];
00255         system[2]=_coeff[2]-x._coeff[2];
00256         system[3]=_coeff[3]-x._coeff[3];
00257 
00258         t-=r;
00259         t*=drs;
00260 
00261         // Newton's method
00262         // Inner loop cost summary: 7 products, 5 sums, 1 difference
00263         for(;i;i--)
00264             t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
00265                 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
00266 
00267         t*=(s-r);
00268         t+=r;
00269 
00270         return t;
00271     }
00272 
00273     value_type &
00274     operator[](int i)
00275     { return (&a)[i]; }
00276 
00277     const value_type &
00278     operator[](int i) const
00279     { return (&a)[i]; }
00280 };
00281 
00282 
00283 // Fast double implementation of a cubic bezier curve
00284 template <>
00285 class bezier_base<double,float> : public std::unary_function<float,double>
00286 {
00287 public:
00288     typedef double value_type;
00289     typedef float time_type;
00290 private:
00291     affine_combo<value_type,time_type> affine_func;
00292     value_type a,b,c,d;
00293     time_type r,s;
00294 
00295     value_type _coeff[4];
00296     time_type drs; // reciprocal of (s-r)
00297 public:
00298     bezier_base():r(0.0),s(1.0),drs(1.0) { }
00299     bezier_base(
00300         const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00301         const time_type &r=0.0, const time_type &s=1.0):
00302         a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
00303 
00304     void sync()
00305     {
00306 //      drs=1.0/(s-r);
00307         _coeff[0]=                 a;
00308         _coeff[1]=           b*3 - a*3;
00309         _coeff[2]=     c*3 - b*6 + a*3;
00310         _coeff[3]= d - c*3 + b*3 - a;
00311     }
00312 
00313     // 4 products, 3 sums, and 1 difference.
00314     inline value_type
00315     operator()(time_type t)const
00316     { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
00317 
00318     void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
00319     void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
00320     void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
00321     const time_type &get_r()const { return r; }
00322     const time_type &get_s()const { return s; }
00323     time_type get_dt()const { return s-r; }
00324 
00326 
00329     time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
00330     {
00331         //BROKEN - the time values of the 2 curves should be independent
00332         value_type system[4];
00333         system[0]=_coeff[0]-x._coeff[0];
00334         system[1]=_coeff[1]-x._coeff[1];
00335         system[2]=_coeff[2]-x._coeff[2];
00336         system[3]=_coeff[3]-x._coeff[3];
00337 
00338         t-=r;
00339         t*=drs;
00340 
00341         // Newton's method
00342         // Inner loop: 7 products, 5 sums, 1 difference
00343         for(;i;i--)
00344             t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
00345                 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
00346 
00347         t*=(s-r);
00348         t+=r;
00349 
00350         return t;
00351     }
00352 
00353     value_type &
00354     operator[](int i)
00355     { return (&a)[i]; }
00356 
00357     const value_type &
00358     operator[](int i) const
00359     { return (&a)[i]; }
00360 };
00361 
00362 //#ifdef __FIXED__
00363 
00364 // Fast double implementation of a cubic bezier curve
00365 /*
00366 template <>
00367 template <class T,unsigned int FIXED_BITS>
00368 class bezier_base<fixed_base<T,FIXED_BITS> > : std::unary_function<fixed_base<T,FIXED_BITS>,fixed_base<T,FIXED_BITS> >
00369 {
00370 public:
00371     typedef fixed_base<T,FIXED_BITS> value_type;
00372     typedef fixed_base<T,FIXED_BITS> time_type;
00373 
00374 private:
00375     affine_combo<value_type,time_type> affine_func;
00376     value_type a,b,c,d;
00377     time_type r,s;
00378 
00379     value_type _coeff[4];
00380     time_type drs; // reciprocal of (s-r)
00381 public:
00382     bezier_base():r(0.0),s(1.0),drs(1.0) { }
00383     bezier_base(
00384         const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00385         const time_type &r=0, const time_type &s=1):
00386         a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
00387 
00388     void sync()
00389     {
00390         drs=time_type(1)/(s-r);
00391         _coeff[0]=                 a;
00392         _coeff[1]=           b*3 - a*3;
00393         _coeff[2]=     c*3 - b*6 + a*3;
00394         _coeff[3]= d - c*3 + b*3 - a;
00395     }
00396 
00397     // 4 products, 3 sums, and 1 difference.
00398     inline value_type
00399     operator()(time_type t)const
00400     { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
00401 
00402     void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=time_type(1)/(s-r); }
00403     void set_r(time_type new_r) { r=new_r; drs=time_type(1)/(s-r); }
00404     void set_s(time_type new_s) { s=new_s; drs=time_type(1)/(s-r); }
00405     const time_type &get_r()const { return r; }
00406     const time_type &get_s()const { return s; }
00407     time_type get_dt()const { return s-r; }
00408 
00411     //  for the calling curve.
00412     //
00413     time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0,int i=15)const
00414     {
00415         value_type system[4];
00416         system[0]=_coeff[0]-x._coeff[0];
00417         system[1]=_coeff[1]-x._coeff[1];
00418         system[2]=_coeff[2]-x._coeff[2];
00419         system[3]=_coeff[3]-x._coeff[3];
00420 
00421         t-=r;
00422         t*=drs;
00423 
00424         // Newton's method
00425         // Inner loop: 7 products, 5 sums, 1 difference
00426         for(;i;i--)
00427             t-=(time_type) ( (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
00428                 (system[1]+(system[2]*2+(system[3]*3)*t)*t) );
00429 
00430         t*=(s-r);
00431         t+=r;
00432 
00433         return t;
00434     }
00435 
00436     value_type &
00437     operator[](int i)
00438     { return (&a)[i]; }
00439 
00440     const value_type &
00441     operator[](int i) const
00442     { return (&a)[i]; }
00443 };
00444 */
00445 //#endif
00446 
00447 #endif
00448 
00449 
00450 
00451 template <typename V, typename T>
00452 class bezier_iterator
00453 {
00454 public:
00455 
00456     struct iterator_category {};
00457     typedef V value_type;
00458     typedef T difference_type;
00459     typedef V reference;
00460     
00461 private:
00462     difference_type t;
00463     difference_type dt;
00464     bezier_base<V,T>    curve;
00465 
00466 public:
00467 
00468 /*
00469     reference
00470     operator*(void)const { return curve(t); }
00471     const surface_iterator&
00472 
00473     operator++(void)
00474     { t+=dt; return &this; }
00475 
00476     const surface_iterator&
00477     operator++(int)
00478     { hermite_iterator _tmp=*this; t+=dt; return _tmp; }
00479 
00480     const surface_iterator&
00481     operator--(void)
00482     { t-=dt; return &this; }
00483 
00484     const surface_iterator&
00485     operator--(int)
00486     { hermite_iterator _tmp=*this; t-=dt; return _tmp; }
00487 
00488     
00489     surface_iterator
00490     operator+(difference_type __n) const
00491     { return surface_iterator(data+__n[0]+__n[1]*pitch,pitch); }
00492 
00493     surface_iterator
00494     operator-(difference_type __n) const
00495     { return surface_iterator(data-__n[0]-__n[1]*pitch,pitch); }
00496 */
00497     
00498 };
00499 
00500 template <typename V,typename T=float>
00501 class bezier : public bezier_base<V,T>
00502 {
00503 public:
00504     typedef V value_type;
00505     typedef T time_type;
00506     typedef float distance_type;
00507     typedef bezier_iterator<V,T> iterator;
00508     typedef bezier_iterator<V,T> const_iterator;
00509     
00510     distance_func<value_type> dist;
00511     
00512     using bezier_base<V,T>::get_r;
00513     using bezier_base<V,T>::get_s;
00514     using bezier_base<V,T>::get_dt;
00515 
00516 public: 
00517     bezier() { }
00518     bezier(const value_type &a, const value_type &b, const value_type &c, const value_type &d):
00519         bezier_base<V,T>(a,b,c,d) { }
00520 
00521 
00522     const_iterator begin()const;
00523     const_iterator end()const;
00524     
00525     time_type find_closest(const value_type& x, int i=7, time_type r=(0), time_type s=(1))const
00526     {
00527         float t((r+s)*0.5);
00528         for(;i;i--)
00529         {                       
00530             if(dist(operator()((s-r)*(1.0/3.0)+r),x) < dist(operator()((s-r)*(2.0/3.0)+r),x))
00531                 s=t;
00532             else
00533                 r=t;
00534             t=((r+s)*0.5);
00535         }
00536         return t;
00537     }
00538 
00539     
00540     distance_type find_distance(time_type r, time_type s, int steps=7)const
00541     {
00542         const time_type inc((s-r)/steps);
00543         distance_type ret(0);
00544         value_type last(operator()(r));
00545         
00546         for(r+=inc;r<s;r+=inc)
00547         {
00548             const value_type n(operator()(r));
00549             ret+=dist.uncook(dist(last,n));
00550             last=n;
00551         }
00552         ret+=dist.uncook(dist(last,operator()(r)))*(s-(r-inc))/inc;
00553         
00554         return ret;
00555     }
00556     
00557     distance_type length()const { return find_distance(get_r(),get_s()); }
00558     
00559     /* subdivide at some time t into 2 separate curves left and right
00560     
00561         b0 l1
00562         *       0+1 l2
00563         b1      *       1+2*1+2 l3
00564         *       1+2     *           0+3*1+3*2+3 l4,r1
00565         b2      *       1+2*2+2 r2  *
00566         *       2+3 r3  *
00567         b3 r4   *
00568         *       
00569         
00570         0.1 2.3 ->  0.1 2 3 4 5.6
00571     */
00572     void subdivide(bezier *left, bezier *right, const time_type &time = (time_type)0.5) const
00573     {
00574         time_type t=(t-get_r())/get_dt();
00575         bezier lt,rt;
00576         
00577         value_type temp;
00578         const value_type& a((*this)[0]);
00579         const value_type& b((*this)[1]);
00580         const value_type& c((*this)[2]);
00581         const value_type& d((*this)[3]);
00582 
00583         //1st stage points to keep      
00584         lt[0] = a;
00585         rt[3] = d;
00586 
00587         //2nd stage calc
00588         lt[1] = affine_func(a,b,t);
00589         temp = affine_func(b,c,t);
00590         rt[2] = affine_func(c,d,t);
00591                 
00592         //3rd stage calc
00593         lt[2] = affine_func(lt[1],temp,t);
00594         rt[1] = affine_func(temp,rt[2],t);
00595         
00596         //last stage calc
00597         lt[3] = rt[0] = affine_func(lt[2],rt[1],t);
00598         
00599         //set the time range for l,r (the inside values should be 1, 0 respectively)
00600         lt.set_r(get_r());
00601         rt.set_s(get_s());
00602         
00603         lt.sync();
00604         rt.sync();
00605         
00606         //give back the curves
00607         if(left) *left = lt;
00608         if(right) *right = rt; 
00609     }
00610 
00611     
00612     void evaluate(time_type t, value_type &f, value_type &df) const
00613     {
00614         t=(t-get_r())/get_dt();
00615 
00616         const value_type& a((*this)[0]);
00617         const value_type& b((*this)[1]);
00618         const value_type& c((*this)[2]);
00619         const value_type& d((*this)[3]);
00620         
00621         const value_type p1 = affine_func(
00622                             affine_func(a,b,t),
00623                             affine_func(b,c,t)
00624                             ,t);
00625         const value_type p2 = affine_func(
00626                             affine_func(b,c,t),
00627                             affine_func(c,d,t)
00628                         ,t);
00629         
00630         f = affine_func(p1,p2,t);
00631         df = (p2-p1)*3;
00632     }
00633 };
00634 
00635 _ETL_END_NAMESPACE
00636 
00637 /* === E X T E R N S ======================================================= */
00638 
00639 /* === E N D =============================================================== */
00640 
00641 #endif

Generated on Wed Jun 21 04:14:56 2006 for ETL by  doxygen 1.4.6