_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 #define MAXDEPTH 64 /*  Maximum depth for recursion */
00038 
00039 /* take binary sign of a, either -1, or 1 if >= 0 */
00040 #define SGN(a)      (((a)<0) ? -1 : 1)
00041 
00042 /* find minimum of a and b */
00043 #ifndef MIN
00044 #define MIN(a,b)    (((a)<(b))?(a):(b))
00045 #endif
00046 
00047 /* find maximum of a and b */
00048 #ifndef MAX
00049 #define MAX(a,b)    (((a)>(b))?(a):(b))
00050 #endif
00051 
00052 #define BEZIER_EPSILON  (ldexp(1.0,-MAXDEPTH-1)) /*Flatness control value */
00053 //#define   BEZIER_EPSILON  0.00005 /*Flatness control value */
00054 #define DEGREE  3           /*  Cubic Bezier curve      */
00055 #define W_DEGREE 5          /*  Degree of eqn to find roots of */
00056 
00057 /* === T Y P E D E F S ===================================================== */
00058 
00059 /* === C L A S S E S & S T R U C T S ======================================= */
00060 
00061 _ETL_BEGIN_NAMESPACE
00062 
00063 template<typename V,typename T> class bezier;
00064 
00066 // This generic implementation uses the DeCasteljau algorithm.
00067 // Works for just about anything that has an affine combination function
00068 template <typename V,typename T=float>
00069 class bezier_base : public std::unary_function<T,V>
00070 {
00071 public:
00072     typedef V value_type;
00073     typedef T time_type;
00074 
00075 private:
00076     value_type a,b,c,d;
00077     time_type r,s;
00078 
00079 protected:
00080     affine_combo<value_type,time_type> affine_func;
00081 
00082 public:
00083     bezier_base():r(0.0),s(1.0) { }
00084     bezier_base(
00085         const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00086         const time_type &r=0.0, const time_type &s=1.0):
00087         a(a),b(b),c(c),d(d),r(r),s(s) { sync(); }
00088 
00089     void sync()
00090     {
00091     }
00092 
00093     value_type
00094     operator()(time_type t)const
00095     {
00096         t=(t-r)/(s-r);
00097         return
00098         affine_func(
00099             affine_func(
00100                 affine_func(a,b,t),
00101                 affine_func(b,c,t)
00102             ,t),
00103             affine_func(
00104                 affine_func(b,c,t),
00105                 affine_func(c,d,t)
00106             ,t)
00107         ,t);
00108     }
00109 
00110     /*
00111     void evaluate(time_type t, value_type &f, value_type &df) const
00112     {
00113         t=(t-r)/(s-r);
00114 
00115         value_type p1 = affine_func(
00116                             affine_func(a,b,t),
00117                             affine_func(b,c,t)
00118                             ,t);
00119         value_type p2 = affine_func(
00120                             affine_func(b,c,t),
00121                             affine_func(c,d,t)
00122                         ,t);
00123 
00124         f = affine_func(p1,p2,t);
00125         df = (p2-p1)*3;
00126     }
00127     */
00128 
00129     void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; }
00130     void set_r(time_type new_r) { r=new_r; }
00131     void set_s(time_type new_s) { s=new_s; }
00132     const time_type &get_r()const { return r; }
00133     const time_type &get_s()const { return s; }
00134     time_type get_dt()const { return s-r; }
00135 
00136     bool intersect_hull(const bezier_base<value_type,time_type> &x)const
00137     {
00138         return 0;
00139     }
00140 
00142 
00162     time_type intersect(const bezier_base<value_type,time_type> &x, time_type near=0.0)const
00163     {
00164         return 0;
00165     }
00166 
00167     /* subdivide at some time t into 2 separate curves left and right
00168 
00169         b0 l1
00170         *       0+1 l2
00171         b1      *       1+2*1+2 l3
00172         *       1+2     *           0+3*1+3*2+3 l4,r1
00173         b2      *       1+2*2+2 r2  *
00174         *       2+3 r3  *
00175         b3 r4   *
00176         *
00177 
00178         0.1 2.3 ->  0.1 2 3 4 5.6
00179     */
00180 /*  void subdivide(bezier_base *left, bezier_base *right, const time_type &time = (time_type)0.5) const
00181     {
00182         time_type t = (time-r)/(s-r);
00183         bezier_base lt,rt;
00184 
00185         value_type temp;
00186 
00187         //1st stage points to keep
00188         lt.a = a;
00189         rt.d = d;
00190 
00191         //2nd stage calc
00192         lt.b = affine_func(a,b,t);
00193         temp = affine_func(b,c,t);
00194         rt.c = affine_func(c,d,t);
00195 
00196         //3rd stage calc
00197         lt.c = affine_func(lt.b,temp,t);
00198         rt.b = affine_func(temp,rt.c,t);
00199 
00200         //last stage calc
00201         lt.d = rt.a = affine_func(lt.c,rt.b,t);
00202 
00203         //set the time range for l,r (the inside values should be 1, 0 respectively)
00204         lt.r = r;
00205         rt.s = s;
00206 
00207         //give back the curves
00208         if(left) *left = lt;
00209         if(right) *right = rt;
00210     }
00211     */
00212     value_type &
00213     operator[](int i)
00214     { return (&a)[i]; }
00215 
00216     const value_type &
00217     operator[](int i) const
00218     { return (&a)[i]; }
00219 };
00220 
00221 
00222 #if 1
00223 // Fast float implementation of a cubic bezier curve
00224 template <>
00225 class bezier_base<float,float> : public std::unary_function<float,float>
00226 {
00227 public:
00228     typedef float value_type;
00229     typedef float time_type;
00230 private:
00231     affine_combo<value_type,time_type> affine_func;
00232     value_type a,b,c,d;
00233     time_type r,s;
00234 
00235     value_type _coeff[4];
00236     time_type drs; // reciprocal of (s-r)
00237 public:
00238     bezier_base():r(0.0),s(1.0),drs(1.0) { }
00239     bezier_base(
00240         const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00241         const time_type &r=0.0, const time_type &s=1.0):
00242         a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
00243 
00244     void sync()
00245     {
00246 //      drs=1.0/(s-r);
00247         _coeff[0]=                 a;
00248         _coeff[1]=           b*3 - a*3;
00249         _coeff[2]=     c*3 - b*6 + a*3;
00250         _coeff[3]= d - c*3 + b*3 - a;
00251     }
00252 
00253     // Cost Summary: 4 products, 3 sums, and 1 difference.
00254     inline value_type
00255     operator()(time_type t)const
00256     { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
00257 
00258     void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
00259     void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
00260     void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
00261     const time_type &get_r()const { return r; }
00262     const time_type &get_s()const { return s; }
00263     time_type get_dt()const { return s-r; }
00264 
00266 
00269     time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
00270     {
00271         //BROKEN - the time values of the 2 curves should be independent
00272         value_type system[4];
00273         system[0]=_coeff[0]-x._coeff[0];
00274         system[1]=_coeff[1]-x._coeff[1];
00275         system[2]=_coeff[2]-x._coeff[2];
00276         system[3]=_coeff[3]-x._coeff[3];
00277 
00278         t-=r;
00279         t*=drs;
00280 
00281         // Newton's method
00282         // Inner loop cost summary: 7 products, 5 sums, 1 difference
00283         for(;i;i--)
00284             t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
00285                 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
00286 
00287         t*=(s-r);
00288         t+=r;
00289 
00290         return t;
00291     }
00292 
00293     value_type &
00294     operator[](int i)
00295     { return (&a)[i]; }
00296 
00297     const value_type &
00298     operator[](int i) const
00299     { return (&a)[i]; }
00300 };
00301 
00302 
00303 // Fast double implementation of a cubic bezier curve
00304 template <>
00305 class bezier_base<double,float> : public std::unary_function<float,double>
00306 {
00307 public:
00308     typedef double value_type;
00309     typedef float time_type;
00310 private:
00311     affine_combo<value_type,time_type> affine_func;
00312     value_type a,b,c,d;
00313     time_type r,s;
00314 
00315     value_type _coeff[4];
00316     time_type drs; // reciprocal of (s-r)
00317 public:
00318     bezier_base():r(0.0),s(1.0),drs(1.0) { }
00319     bezier_base(
00320         const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00321         const time_type &r=0.0, const time_type &s=1.0):
00322         a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
00323 
00324     void sync()
00325     {
00326 //      drs=1.0/(s-r);
00327         _coeff[0]=                 a;
00328         _coeff[1]=           b*3 - a*3;
00329         _coeff[2]=     c*3 - b*6 + a*3;
00330         _coeff[3]= d - c*3 + b*3 - a;
00331     }
00332 
00333     // 4 products, 3 sums, and 1 difference.
00334     inline value_type
00335     operator()(time_type t)const
00336     { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
00337 
00338     void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
00339     void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
00340     void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
00341     const time_type &get_r()const { return r; }
00342     const time_type &get_s()const { return s; }
00343     time_type get_dt()const { return s-r; }
00344 
00346 
00349     time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
00350     {
00351         //BROKEN - the time values of the 2 curves should be independent
00352         value_type system[4];
00353         system[0]=_coeff[0]-x._coeff[0];
00354         system[1]=_coeff[1]-x._coeff[1];
00355         system[2]=_coeff[2]-x._coeff[2];
00356         system[3]=_coeff[3]-x._coeff[3];
00357 
00358         t-=r;
00359         t*=drs;
00360 
00361         // Newton's method
00362         // Inner loop: 7 products, 5 sums, 1 difference
00363         for(;i;i--)
00364             t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
00365                 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
00366 
00367         t*=(s-r);
00368         t+=r;
00369 
00370         return t;
00371     }
00372 
00373     value_type &
00374     operator[](int i)
00375     { return (&a)[i]; }
00376 
00377     const value_type &
00378     operator[](int i) const
00379     { return (&a)[i]; }
00380 };
00381 
00382 //#ifdef __FIXED__
00383 
00384 // Fast double implementation of a cubic bezier curve
00385 /*
00386 template <>
00387 template <class T,unsigned int FIXED_BITS>
00388 class bezier_base<fixed_base<T,FIXED_BITS> > : std::unary_function<fixed_base<T,FIXED_BITS>,fixed_base<T,FIXED_BITS> >
00389 {
00390 public:
00391     typedef fixed_base<T,FIXED_BITS> value_type;
00392     typedef fixed_base<T,FIXED_BITS> time_type;
00393 
00394 private:
00395     affine_combo<value_type,time_type> affine_func;
00396     value_type a,b,c,d;
00397     time_type r,s;
00398 
00399     value_type _coeff[4];
00400     time_type drs; // reciprocal of (s-r)
00401 public:
00402     bezier_base():r(0.0),s(1.0),drs(1.0) { }
00403     bezier_base(
00404         const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00405         const time_type &r=0, const time_type &s=1):
00406         a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
00407 
00408     void sync()
00409     {
00410         drs=time_type(1)/(s-r);
00411         _coeff[0]=                 a;
00412         _coeff[1]=           b*3 - a*3;
00413         _coeff[2]=     c*3 - b*6 + a*3;
00414         _coeff[3]= d - c*3 + b*3 - a;
00415     }
00416 
00417     // 4 products, 3 sums, and 1 difference.
00418     inline value_type
00419     operator()(time_type t)const
00420     { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
00421 
00422     void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=time_type(1)/(s-r); }
00423     void set_r(time_type new_r) { r=new_r; drs=time_type(1)/(s-r); }
00424     void set_s(time_type new_s) { s=new_s; drs=time_type(1)/(s-r); }
00425     const time_type &get_r()const { return r; }
00426     const time_type &get_s()const { return s; }
00427     time_type get_dt()const { return s-r; }
00428 
00431     //  for the calling curve.
00432     //
00433     time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0,int i=15)const
00434     {
00435         value_type system[4];
00436         system[0]=_coeff[0]-x._coeff[0];
00437         system[1]=_coeff[1]-x._coeff[1];
00438         system[2]=_coeff[2]-x._coeff[2];
00439         system[3]=_coeff[3]-x._coeff[3];
00440 
00441         t-=r;
00442         t*=drs;
00443 
00444         // Newton's method
00445         // Inner loop: 7 products, 5 sums, 1 difference
00446         for(;i;i--)
00447             t-=(time_type) ( (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
00448                 (system[1]+(system[2]*2+(system[3]*3)*t)*t) );
00449 
00450         t*=(s-r);
00451         t+=r;
00452 
00453         return t;
00454     }
00455 
00456     value_type &
00457     operator[](int i)
00458     { return (&a)[i]; }
00459 
00460     const value_type &
00461     operator[](int i) const
00462     { return (&a)[i]; }
00463 };
00464 */
00465 //#endif
00466 
00467 #endif
00468 
00469 
00470 
00471 template <typename V, typename T>
00472 class bezier_iterator
00473 {
00474 public:
00475 
00476     struct iterator_category {};
00477     typedef V value_type;
00478     typedef T difference_type;
00479     typedef V reference;
00480 
00481 private:
00482     difference_type t;
00483     difference_type dt;
00484     bezier_base<V,T>    curve;
00485 
00486 public:
00487 
00488 /*
00489     reference
00490     operator*(void)const { return curve(t); }
00491     const surface_iterator&
00492 
00493     operator++(void)
00494     { t+=dt; return &this; }
00495 
00496     const surface_iterator&
00497     operator++(int)
00498     { hermite_iterator _tmp=*this; t+=dt; return _tmp; }
00499 
00500     const surface_iterator&
00501     operator--(void)
00502     { t-=dt; return &this; }
00503 
00504     const surface_iterator&
00505     operator--(int)
00506     { hermite_iterator _tmp=*this; t-=dt; return _tmp; }
00507 
00508 
00509     surface_iterator
00510     operator+(difference_type __n) const
00511     { return surface_iterator(data+__n[0]+__n[1]*pitch,pitch); }
00512 
00513     surface_iterator
00514     operator-(difference_type __n) const
00515     { return surface_iterator(data-__n[0]-__n[1]*pitch,pitch); }
00516 */
00517 
00518 };
00519 
00520 template <typename V,typename T=float>
00521 class bezier : public bezier_base<V,T>
00522 {
00523 public:
00524     typedef V value_type;
00525     typedef T time_type;
00526     typedef float distance_type;
00527     typedef bezier_iterator<V,T> iterator;
00528     typedef bezier_iterator<V,T> const_iterator;
00529 
00530     distance_func<value_type> dist;
00531 
00532     using bezier_base<V,T>::get_r;
00533     using bezier_base<V,T>::get_s;
00534     using bezier_base<V,T>::get_dt;
00535 
00536 public:
00537     bezier() { }
00538     bezier(const value_type &a, const value_type &b, const value_type &c, const value_type &d):
00539         bezier_base<V,T>(a,b,c,d) { }
00540 
00541 
00542     const_iterator begin()const;
00543     const_iterator end()const;
00544 
00545     time_type find_closest(bool fast, const value_type& x, int i=7)const
00546     {
00547         if (!fast)
00548         {
00549             value_type array[4] = {
00550                 bezier<V,T>::operator[](0),
00551                 bezier<V,T>::operator[](1),
00552                 bezier<V,T>::operator[](2),
00553                 bezier<V,T>::operator[](3)};
00554             float t = NearestPointOnCurve(x, array);
00555             return t > 0.999999 ? 0.999999 : t < 0.000001 ? 0.000001 : t;
00556         }
00557         else
00558         {
00559             time_type r(0), s(1);
00560             float t((r+s)*0.5); /* half way between r and s */
00561 
00562             for(;i;i--)
00563             {
00564                 // compare 33% of the way between r and s with 67% of the way between r and s
00565                 if(dist(operator()((s-r)*(1.0/3.0)+r), x) <
00566                    dist(operator()((s-r)*(2.0/3.0)+r), x))
00567                     s=t;
00568                 else
00569                     r=t;
00570                 t=((r+s)*0.5);
00571             }
00572             return t;
00573         }
00574     }
00575 
00576     distance_type find_distance(time_type r, time_type s, int steps=7)const
00577     {
00578         const time_type inc((s-r)/steps);
00579         distance_type ret(0);
00580         value_type last(operator()(r));
00581 
00582         for(r+=inc;r<s;r+=inc)
00583         {
00584             const value_type n(operator()(r));
00585             ret+=dist.uncook(dist(last,n));
00586             last=n;
00587         }
00588         ret+=dist.uncook(dist(last,operator()(r)))*(s-(r-inc))/inc;
00589 
00590         return ret;
00591     }
00592 
00593     distance_type length()const { return find_distance(get_r(),get_s()); }
00594 
00595     /* subdivide at some time t into 2 separate curves left and right
00596 
00597         b0 l1
00598         *       0+1 l2
00599         b1      *       1+2*1+2 l3
00600         *       1+2     *           0+3*1+3*2+3 l4,r1
00601         b2      *       1+2*2+2 r2  *
00602         *       2+3 r3  *
00603         b3 r4   *
00604         *
00605 
00606         0.1 2.3 ->  0.1 2 3 4 5.6
00607     */
00608     void subdivide(bezier *left, bezier *right, const time_type &time = (time_type)0.5) const
00609     {
00610         time_type t=(t-get_r())/get_dt();
00611         bezier lt,rt;
00612 
00613         value_type temp;
00614         const value_type& a((*this)[0]);
00615         const value_type& b((*this)[1]);
00616         const value_type& c((*this)[2]);
00617         const value_type& d((*this)[3]);
00618 
00619         //1st stage points to keep
00620         lt[0] = a;
00621         rt[3] = d;
00622 
00623         //2nd stage calc
00624         lt[1] = affine_func(a,b,t);
00625         temp = affine_func(b,c,t);
00626         rt[2] = affine_func(c,d,t);
00627 
00628         //3rd stage calc
00629         lt[2] = affine_func(lt[1],temp,t);
00630         rt[1] = affine_func(temp,rt[2],t);
00631 
00632         //last stage calc
00633         lt[3] = rt[0] = affine_func(lt[2],rt[1],t);
00634 
00635         //set the time range for l,r (the inside values should be 1, 0 respectively)
00636         lt.set_r(get_r());
00637         rt.set_s(get_s());
00638 
00639         lt.sync();
00640         rt.sync();
00641 
00642         //give back the curves
00643         if(left) *left = lt;
00644         if(right) *right = rt;
00645     }
00646 
00647 
00648     void evaluate(time_type t, value_type &f, value_type &df) const
00649     {
00650         t=(t-get_r())/get_dt();
00651 
00652         const value_type& a((*this)[0]);
00653         const value_type& b((*this)[1]);
00654         const value_type& c((*this)[2]);
00655         const value_type& d((*this)[3]);
00656 
00657         const value_type p1 = affine_func(
00658                             affine_func(a,b,t),
00659                             affine_func(b,c,t)
00660                             ,t);
00661         const value_type p2 = affine_func(
00662                             affine_func(b,c,t),
00663                             affine_func(c,d,t)
00664                         ,t);
00665 
00666         f = affine_func(p1,p2,t);
00667         df = (p2-p1)*3;
00668     }
00669 
00670 private:
00671     /*
00672      *  Bezier :
00673      *  Evaluate a Bezier curve at a particular parameter value
00674      *      Fill in control points for resulting sub-curves if "Left" and
00675      *  "Right" are non-null.
00676      *
00677      *    int           degree;     Degree of bezier curve
00678      *    value_type    *VT;        Control pts
00679      *    time_type     t;          Parameter value
00680      *    value_type    *Left;      RETURN left half ctl pts
00681      *    value_type    *Right;     RETURN right half ctl pts
00682      */
00683     static value_type Bezier(value_type *VT, int degree, time_type t, value_type *Left, value_type *Right)
00684     {
00685         int         i, j;       /* Index variables  */
00686         value_type  Vtemp[W_DEGREE+1][W_DEGREE+1];
00687 
00688         /* Copy control points  */
00689         for (j = 0; j <= degree; j++)
00690             Vtemp[0][j] = VT[j];
00691 
00692         /* Triangle computation */
00693         for (i = 1; i <= degree; i++)
00694             for (j =0 ; j <= degree - i; j++)
00695             {
00696                 Vtemp[i][j][0] = (1.0 - t) * Vtemp[i-1][j][0] + t * Vtemp[i-1][j+1][0];
00697                 Vtemp[i][j][1] = (1.0 - t) * Vtemp[i-1][j][1] + t * Vtemp[i-1][j+1][1];
00698             }
00699 
00700         if (Left != NULL)
00701             for (j = 0; j <= degree; j++)
00702                 Left[j]  = Vtemp[j][0];
00703 
00704         if (Right != NULL)
00705             for (j = 0; j <= degree; j++)
00706                 Right[j] = Vtemp[degree-j][j];
00707 
00708         return (Vtemp[degree][0]);
00709     }
00710 
00711     /*
00712      * CrossingCount :
00713      *  Count the number of times a Bezier control polygon
00714      *  crosses the 0-axis. This number is >= the number of roots.
00715      *
00716      *    value_type    *VT;            Control pts of Bezier curve
00717      */
00718     static int CrossingCount(value_type *VT)
00719     {
00720         int     i;
00721         int     n_crossings = 0;    /*  Number of zero-crossings    */
00722         int     sign, old_sign;     /*  Sign of coefficients        */
00723 
00724         sign = old_sign = SGN(VT[0][1]);
00725         for (i = 1; i <= W_DEGREE; i++)
00726         {
00727             sign = SGN(VT[i][1]);
00728             if (sign != old_sign) n_crossings++;
00729             old_sign = sign;
00730         }
00731 
00732         return n_crossings;
00733     }
00734 
00735     /*
00736      *  ControlPolygonFlatEnough :
00737      *  Check if the control polygon of a Bezier curve is flat enough
00738      *  for recursive subdivision to bottom out.
00739      *
00740      *    value_type    *VT;        Control points
00741      */
00742     static int ControlPolygonFlatEnough(value_type *VT)
00743     {
00744         int             i;                  /* Index variable                   */
00745         distance_type   distance[W_DEGREE]; /* Distances from pts to line       */
00746         distance_type   max_distance_above; /* maximum of these                 */
00747         distance_type   max_distance_below;
00748         time_type       intercept_1, intercept_2, left_intercept, right_intercept;
00749         distance_type   a, b, c;            /* Coefficients of implicit         */
00750         /* eqn for line from VT[0]-VT[deg]          */
00751         /* Find the  perpendicular distance         */
00752         /* from each interior control point to      */
00753         /* line connecting VT[0] and VT[W_DEGREE]   */
00754         {
00755             distance_type   abSquared;
00756 
00757             /* Derive the implicit equation for line connecting first *
00758              *  and last control points */
00759             a = VT[0][1] - VT[W_DEGREE][1];
00760             b = VT[W_DEGREE][0] - VT[0][0];
00761             c = VT[0][0] * VT[W_DEGREE][1] - VT[W_DEGREE][0] * VT[0][1];
00762 
00763             abSquared = (a * a) + (b * b);
00764 
00765             for (i = 1; i < W_DEGREE; i++)
00766             {
00767                 /* Compute distance from each of the points to that line    */
00768                 distance[i] = a * VT[i][0] + b * VT[i][1] + c;
00769                 if (distance[i] > 0.0) distance[i] =  (distance[i] * distance[i]) / abSquared;
00770                 if (distance[i] < 0.0) distance[i] = -(distance[i] * distance[i]) / abSquared;
00771             }
00772         }
00773 
00774         /* Find the largest distance */
00775         max_distance_above = max_distance_below = 0.0;
00776 
00777         for (i = 1; i < W_DEGREE; i++)
00778         {
00779             if (distance[i] < 0.0) max_distance_below = MIN(max_distance_below, distance[i]);
00780             if (distance[i] > 0.0) max_distance_above = MAX(max_distance_above, distance[i]);
00781         }
00782 
00783         /* Implicit equation for "above" line */
00784         intercept_1 = -(c + max_distance_above)/a;
00785 
00786         /*  Implicit equation for "below" line */
00787         intercept_2 = -(c + max_distance_below)/a;
00788 
00789         /* Compute intercepts of bounding box   */
00790         left_intercept = MIN(intercept_1, intercept_2);
00791         right_intercept = MAX(intercept_1, intercept_2);
00792 
00793         return 0.5 * (right_intercept-left_intercept) < BEZIER_EPSILON ? 1 : 0;
00794     }
00795 
00796     /*
00797      *  ComputeXIntercept :
00798      *  Compute intersection of chord from first control point to last
00799      *  with 0-axis.
00800      *
00801      *    value_type    *VT;            Control points
00802      */
00803     static time_type ComputeXIntercept(value_type *VT)
00804     {
00805         distance_type YNM = VT[W_DEGREE][1] - VT[0][1];
00806         return (YNM*VT[0][0] - (VT[W_DEGREE][0] - VT[0][0])*VT[0][1]) / YNM;
00807     }
00808 
00809     /*
00810      *  FindRoots :
00811      *  Given a 5th-degree equation in Bernstein-Bezier form, find
00812      *  all of the roots in the interval [0, 1].  Return the number
00813      *  of roots found.
00814      *
00815      *    value_type    *w;             The control points
00816      *    time_type     *t;             RETURN candidate t-values
00817      *    int           depth;          The depth of the recursion
00818      */
00819     static int FindRoots(value_type *w, time_type *t, int depth)
00820     {
00821         int         i;
00822         value_type  Left[W_DEGREE+1];   /* New left and right   */
00823         value_type  Right[W_DEGREE+1];  /* control polygons     */
00824         int         left_count;         /* Solution count from  */
00825         int         right_count;        /* children             */
00826         time_type   left_t[W_DEGREE+1]; /* Solutions from kids  */
00827         time_type   right_t[W_DEGREE+1];
00828 
00829         switch (CrossingCount(w))
00830         {
00831             case 0 :
00832             {   /* No solutions here    */
00833                 return 0;
00834             }
00835             case 1 :
00836             {   /* Unique solution  */
00837                 /* Stop recursion when the tree is deep enough      */
00838                 /* if deep enough, return 1 solution at midpoint    */
00839                 if (depth >= MAXDEPTH)
00840                 {
00841                     t[0] = (w[0][0] + w[W_DEGREE][0]) / 2.0;
00842                     return 1;
00843                 }
00844                 if (ControlPolygonFlatEnough(w))
00845                 {
00846                     t[0] = ComputeXIntercept(w);
00847                     return 1;
00848                 }
00849                 break;
00850             }
00851         }
00852 
00853         /* Otherwise, solve recursively after   */
00854         /* subdividing control polygon          */
00855         Bezier(w, W_DEGREE, 0.5, Left, Right);
00856         left_count  = FindRoots(Left,  left_t,  depth+1);
00857         right_count = FindRoots(Right, right_t, depth+1);
00858 
00859         /* Gather solutions together    */
00860         for (i = 0; i < left_count;  i++) t[i] = left_t[i];
00861         for (i = 0; i < right_count; i++) t[i+left_count] = right_t[i];
00862 
00863         /* Send back total number of solutions  */
00864         return (left_count+right_count);
00865     }
00866 
00867     /*
00868      *  ConvertToBezierForm :
00869      *      Given a point and a Bezier curve, generate a 5th-degree
00870      *      Bezier-format equation whose solution finds the point on the
00871      *      curve nearest the user-defined point.
00872      *
00873      *    value_type&   P;              The point to find t for
00874      *    value_type    *VT;            The control points
00875      */
00876     static void ConvertToBezierForm(const value_type& P, value_type *VT, value_type w[W_DEGREE+1])
00877     {
00878         int     i, j, k, m, n, ub, lb;
00879         int     row, column;                /* Table indices                */
00880         value_type  c[DEGREE+1];            /* VT(i)'s - P                  */
00881         value_type  d[DEGREE];              /* VT(i+1) - VT(i)              */
00882         distance_type   cdTable[3][4];      /* Dot product of c, d          */
00883         static distance_type z[3][4] = {    /* Precomputed "z" for cubics   */
00884             {1.0, 0.6, 0.3, 0.1},
00885             {0.4, 0.6, 0.6, 0.4},
00886             {0.1, 0.3, 0.6, 1.0}};
00887 
00888         /* Determine the c's -- these are vectors created by subtracting */
00889         /* point P from each of the control points                       */
00890         for (i = 0; i <= DEGREE; i++)
00891             c[i] = VT[i] - P;
00892 
00893         /* Determine the d's -- these are vectors created by subtracting */
00894         /* each control point from the next                              */
00895         for (i = 0; i <= DEGREE - 1; i++)
00896             d[i] = (VT[i+1] - VT[i]) * 3.0;
00897 
00898         /* Create the c,d table -- this is a table of dot products of the */
00899         /* c's and d's                                                    */
00900         for (row = 0; row <= DEGREE - 1; row++)
00901             for (column = 0; column <= DEGREE; column++)
00902                 cdTable[row][column] = d[row] * c[column];
00903 
00904         /* Now, apply the z's to the dot products, on the skew diagonal */
00905         /* Also, set up the x-values, making these "points"             */
00906         for (i = 0; i <= W_DEGREE; i++)
00907         {
00908             w[i][0] = (distance_type)(i) / W_DEGREE;
00909             w[i][1] = 0.0;
00910         }
00911 
00912         n = DEGREE;
00913         m = DEGREE-1;
00914         for (k = 0; k <= n + m; k++)
00915         {
00916             lb = MAX(0, k - m);
00917             ub = MIN(k, n);
00918             for (i = lb; i <= ub; i++)
00919             {
00920                 j = k - i;
00921                 w[i+j][1] += cdTable[j][i] * z[j][i];
00922             }
00923         }
00924     }
00925 
00926     /*
00927      *  NearestPointOnCurve :
00928      *      Compute the parameter value of the point on a Bezier
00929      *      curve segment closest to some arbtitrary, user-input point.
00930      *      Return the point on the curve at that parameter value.
00931      *
00932      *    value_type&   P;          The user-supplied point
00933      *    value_type    *VT;        Control points of cubic Bezier
00934      */
00935     static time_type NearestPointOnCurve(const value_type& P, value_type VT[4])
00936     {
00937         value_type  w[W_DEGREE+1];          /* Ctl pts of 5th-degree curve  */
00938         time_type   t_candidate[W_DEGREE];  /* Possible roots                */
00939         int         n_solutions;            /* Number of roots found         */
00940         time_type   t;                      /* Parameter value of closest pt */
00941 
00942         /*  Convert problem to 5th-degree Bezier form */
00943         ConvertToBezierForm(P, VT, w);
00944 
00945         /* Find all possible roots of 5th-degree equation */
00946         n_solutions = FindRoots(w, t_candidate, 0);
00947 
00948         /* Compare distances of P to all candidates, and to t=0, and t=1 */
00949         {
00950             distance_type   dist, new_dist;
00951             value_type      p, v;
00952             int             i;
00953 
00954             /* Check distance to beginning of curve, where t = 0    */
00955             dist = (P - VT[0]).mag_squared();
00956             t = 0.0;
00957 
00958             /* Find distances for candidate points  */
00959             for (i = 0; i < n_solutions; i++)
00960             {
00961                 p = Bezier(VT, DEGREE, t_candidate[i], (value_type *)NULL, (value_type *)NULL);
00962                 new_dist = (P - p).mag_squared();
00963                 if (new_dist < dist)
00964                 {
00965                     dist = new_dist;
00966                     t = t_candidate[i];
00967                 }
00968             }
00969 
00970             /* Finally, look at distance to end point, where t = 1.0 */
00971             new_dist = (P - VT[DEGREE]).mag_squared();
00972             if (new_dist < dist)
00973             {
00974                 dist = new_dist;
00975                 t = 1.0;
00976             }
00977         }
00978 
00979         /*  Return the point on the curve at parameter value t */
00980         return t;
00981     }
00982 };
00983 
00984 _ETL_END_NAMESPACE
00985 
00986 /* === E X T E R N S ======================================================= */
00987 
00988 /* === E N D =============================================================== */
00989 
00990 #endif

Generated on Fri Jun 22 14:07:58 2007 for ETL by  doxygen 1.5.2