00001
00026
00027
00028 #ifndef __ETL_BEZIER_H
00029 #define __ETL_BEZIER_H
00030
00031
00032
00033 #include "_curve_func.h"
00034 #include <cmath>
00035
00036
00037
00038
00039 #define MAXDEPTH 64
00040
00041
00042 #define SGN(a) (((a)<0) ? -1 : 1)
00043
00044
00045 #ifndef MIN
00046 #define MIN(a,b) (((a)<(b))?(a):(b))
00047 #endif
00048
00049
00050 #ifndef MAX
00051 #define MAX(a,b) (((a)>(b))?(a):(b))
00052 #endif
00053
00054 #define BEZIER_EPSILON (ldexp(1.0,-MAXDEPTH-1))
00055
00056 #define DEGREE 3
00057 #define W_DEGREE 5
00058
00059
00060
00061
00062
00063 _ETL_BEGIN_NAMESPACE
00064
00065 template<typename V,typename T> class bezier;
00066
00068
00069
00070 template <typename V,typename T=float>
00071 class bezier_base : public std::unary_function<T,V>
00072 {
00073 public:
00074 typedef V value_type;
00075 typedef T time_type;
00076
00077 private:
00078 value_type a,b,c,d;
00079 time_type r,s;
00080
00081 protected:
00082 affine_combo<value_type,time_type> affine_func;
00083
00084 public:
00085 bezier_base():r(0.0),s(1.0) { }
00086 bezier_base(
00087 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00088 const time_type &r=0.0, const time_type &s=1.0):
00089 a(a),b(b),c(c),d(d),r(r),s(s) { sync(); }
00090
00091 void sync()
00092 {
00093 }
00094
00095 value_type
00096 operator()(time_type t)const
00097 {
00098 t=(t-r)/(s-r);
00099 return
00100 affine_func(
00101 affine_func(
00102 affine_func(a,b,t),
00103 affine_func(b,c,t)
00104 ,t),
00105 affine_func(
00106 affine_func(b,c,t),
00107 affine_func(c,d,t)
00108 ,t)
00109 ,t);
00110 }
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131 void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; }
00132 void set_r(time_type new_r) { r=new_r; }
00133 void set_s(time_type new_s) { s=new_s; }
00134 const time_type &get_r()const { return r; }
00135 const time_type &get_s()const { return s; }
00136 time_type get_dt()const { return s-r; }
00137
00138 bool intersect_hull(const bezier_base<value_type,time_type> &x)const
00139 {
00140 return 0;
00141 }
00142
00144
00164 time_type intersect(const bezier_base<value_type,time_type> &x, time_type near=0.0)const
00165 {
00166 return 0;
00167 }
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214 value_type &
00215 operator[](int i)
00216 { return (&a)[i]; }
00217
00218 const value_type &
00219 operator[](int i) const
00220 { return (&a)[i]; }
00221 };
00222
00223
00224 #if 1
00225
00226 template <>
00227 class bezier_base<float,float> : public std::unary_function<float,float>
00228 {
00229 public:
00230 typedef float value_type;
00231 typedef float time_type;
00232 private:
00233 affine_combo<value_type,time_type> affine_func;
00234 value_type a,b,c,d;
00235 time_type r,s;
00236
00237 value_type _coeff[4];
00238 time_type drs;
00239 public:
00240 bezier_base():r(0.0),s(1.0),drs(1.0) { }
00241 bezier_base(
00242 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00243 const time_type &r=0.0, const time_type &s=1.0):
00244 a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
00245
00246 void sync()
00247 {
00248
00249 _coeff[0]= a;
00250 _coeff[1]= b*3 - a*3;
00251 _coeff[2]= c*3 - b*6 + a*3;
00252 _coeff[3]= d - c*3 + b*3 - a;
00253 }
00254
00255
00256 inline value_type
00257 operator()(time_type t)const
00258 { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
00259
00260 void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
00261 void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
00262 void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
00263 const time_type &get_r()const { return r; }
00264 const time_type &get_s()const { return s; }
00265 time_type get_dt()const { return s-r; }
00266
00268
00271 time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
00272 {
00273
00274 value_type system[4];
00275 system[0]=_coeff[0]-x._coeff[0];
00276 system[1]=_coeff[1]-x._coeff[1];
00277 system[2]=_coeff[2]-x._coeff[2];
00278 system[3]=_coeff[3]-x._coeff[3];
00279
00280 t-=r;
00281 t*=drs;
00282
00283
00284
00285 for(;i;i--)
00286 t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
00287 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
00288
00289 t*=(s-r);
00290 t+=r;
00291
00292 return t;
00293 }
00294
00295 value_type &
00296 operator[](int i)
00297 { return (&a)[i]; }
00298
00299 const value_type &
00300 operator[](int i) const
00301 { return (&a)[i]; }
00302 };
00303
00304
00305
00306 template <>
00307 class bezier_base<double,float> : public std::unary_function<float,double>
00308 {
00309 public:
00310 typedef double value_type;
00311 typedef float time_type;
00312 private:
00313 affine_combo<value_type,time_type> affine_func;
00314 value_type a,b,c,d;
00315 time_type r,s;
00316
00317 value_type _coeff[4];
00318 time_type drs;
00319 public:
00320 bezier_base():r(0.0),s(1.0),drs(1.0) { }
00321 bezier_base(
00322 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
00323 const time_type &r=0.0, const time_type &s=1.0):
00324 a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
00325
00326 void sync()
00327 {
00328
00329 _coeff[0]= a;
00330 _coeff[1]= b*3 - a*3;
00331 _coeff[2]= c*3 - b*6 + a*3;
00332 _coeff[3]= d - c*3 + b*3 - a;
00333 }
00334
00335
00336 inline value_type
00337 operator()(time_type t)const
00338 { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
00339
00340 void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
00341 void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
00342 void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
00343 const time_type &get_r()const { return r; }
00344 const time_type &get_s()const { return s; }
00345 time_type get_dt()const { return s-r; }
00346
00348
00351 time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
00352 {
00353
00354 value_type system[4];
00355 system[0]=_coeff[0]-x._coeff[0];
00356 system[1]=_coeff[1]-x._coeff[1];
00357 system[2]=_coeff[2]-x._coeff[2];
00358 system[3]=_coeff[3]-x._coeff[3];
00359
00360 t-=r;
00361 t*=drs;
00362
00363
00364
00365 for(;i;i--)
00366 t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
00367 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
00368
00369 t*=(s-r);
00370 t+=r;
00371
00372 return t;
00373 }
00374
00375 value_type &
00376 operator[](int i)
00377 { return (&a)[i]; }
00378
00379 const value_type &
00380 operator[](int i) const
00381 { return (&a)[i]; }
00382 };
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469 #endif
00470
00471
00472
00473 template <typename V, typename T>
00474 class bezier_iterator
00475 {
00476 public:
00477
00478 struct iterator_category {};
00479 typedef V value_type;
00480 typedef T difference_type;
00481 typedef V reference;
00482
00483 private:
00484 difference_type t;
00485 difference_type dt;
00486 bezier_base<V,T> curve;
00487
00488 public:
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520 };
00521
00522 template <typename V,typename T=float>
00523 class bezier : public bezier_base<V,T>
00524 {
00525 public:
00526 typedef V value_type;
00527 typedef T time_type;
00528 typedef float distance_type;
00529 typedef bezier_iterator<V,T> iterator;
00530 typedef bezier_iterator<V,T> const_iterator;
00531
00532 distance_func<value_type> dist;
00533
00534 using bezier_base<V,T>::get_r;
00535 using bezier_base<V,T>::get_s;
00536 using bezier_base<V,T>::get_dt;
00537
00538 public:
00539 bezier() { }
00540 bezier(const value_type &a, const value_type &b, const value_type &c, const value_type &d):
00541 bezier_base<V,T>(a,b,c,d) { }
00542
00543
00544 const_iterator begin()const;
00545 const_iterator end()const;
00546
00547 time_type find_closest(bool fast, const value_type& x, int i=7)const
00548 {
00549 if (!fast)
00550 {
00551 value_type array[4] = {
00552 bezier<V,T>::operator[](0),
00553 bezier<V,T>::operator[](1),
00554 bezier<V,T>::operator[](2),
00555 bezier<V,T>::operator[](3)};
00556 float t = NearestPointOnCurve(x, array);
00557 return t > 0.999999 ? 0.999999 : t < 0.000001 ? 0.000001 : t;
00558 }
00559 else
00560 {
00561 time_type r(0), s(1);
00562 float t((r+s)*0.5);
00563
00564 for(;i;i--)
00565 {
00566
00567 if(dist(operator()((s-r)*(1.0/3.0)+r), x) <
00568 dist(operator()((s-r)*(2.0/3.0)+r), x))
00569 s=t;
00570 else
00571 r=t;
00572 t=((r+s)*0.5);
00573 }
00574 return t;
00575 }
00576 }
00577
00578 distance_type find_distance(time_type r, time_type s, int steps=7)const
00579 {
00580 const time_type inc((s-r)/steps);
00581 distance_type ret(0);
00582 value_type last(operator()(r));
00583
00584 for(r+=inc;r<s;r+=inc)
00585 {
00586 const value_type n(operator()(r));
00587 ret+=dist.uncook(dist(last,n));
00588 last=n;
00589 }
00590 ret+=dist.uncook(dist(last,operator()(r)))*(s-(r-inc))/inc;
00591
00592 return ret;
00593 }
00594
00595 distance_type length()const { return find_distance(get_r(),get_s()); }
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610 void subdivide(bezier *left, bezier *right, const time_type &time = (time_type)0.5) const
00611 {
00612 time_type t=(time-get_r())/get_dt();
00613 bezier lt,rt;
00614
00615 value_type temp;
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
00622 lt[0] = a;
00623 rt[3] = d;
00624
00625
00626 lt[1] = affine_func(a,b,t);
00627 temp = affine_func(b,c,t);
00628 rt[2] = affine_func(c,d,t);
00629
00630
00631 lt[2] = affine_func(lt[1],temp,t);
00632 rt[1] = affine_func(temp,rt[2],t);
00633
00634
00635 lt[3] = rt[0] = affine_func(lt[2],rt[1],t);
00636
00637
00638 lt.set_r(get_r());
00639 rt.set_s(get_s());
00640
00641 lt.sync();
00642 rt.sync();
00643
00644
00645 if(left) *left = lt;
00646 if(right) *right = rt;
00647 }
00648
00649
00650 void evaluate(time_type t, value_type &f, value_type &df) const
00651 {
00652 t=(t-get_r())/get_dt();
00653
00654 const value_type& a((*this)[0]);
00655 const value_type& b((*this)[1]);
00656 const value_type& c((*this)[2]);
00657 const value_type& d((*this)[3]);
00658
00659 const value_type p1 = affine_func(
00660 affine_func(a,b,t),
00661 affine_func(b,c,t)
00662 ,t);
00663 const value_type p2 = affine_func(
00664 affine_func(b,c,t),
00665 affine_func(c,d,t)
00666 ,t);
00667
00668 f = affine_func(p1,p2,t);
00669 df = (p2-p1)*3;
00670 }
00671
00672 private:
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685 static value_type Bezier(value_type *VT, int degree, time_type t, value_type *Left, value_type *Right)
00686 {
00687 int i, j;
00688 value_type Vtemp[W_DEGREE+1][W_DEGREE+1];
00689
00690
00691 for (j = 0; j <= degree; j++)
00692 Vtemp[0][j] = VT[j];
00693
00694
00695 for (i = 1; i <= degree; i++)
00696 for (j =0 ; j <= degree - i; j++)
00697 {
00698 Vtemp[i][j][0] = (1.0 - t) * Vtemp[i-1][j][0] + t * Vtemp[i-1][j+1][0];
00699 Vtemp[i][j][1] = (1.0 - t) * Vtemp[i-1][j][1] + t * Vtemp[i-1][j+1][1];
00700 }
00701
00702 if (Left != NULL)
00703 for (j = 0; j <= degree; j++)
00704 Left[j] = Vtemp[j][0];
00705
00706 if (Right != NULL)
00707 for (j = 0; j <= degree; j++)
00708 Right[j] = Vtemp[degree-j][j];
00709
00710 return (Vtemp[degree][0]);
00711 }
00712
00713
00714
00715
00716
00717
00718
00719
00720 static int CrossingCount(value_type *VT)
00721 {
00722 int i;
00723 int n_crossings = 0;
00724 int sign, old_sign;
00725
00726 sign = old_sign = SGN(VT[0][1]);
00727 for (i = 1; i <= W_DEGREE; i++)
00728 {
00729 sign = SGN(VT[i][1]);
00730 if (sign != old_sign) n_crossings++;
00731 old_sign = sign;
00732 }
00733
00734 return n_crossings;
00735 }
00736
00737
00738
00739
00740
00741
00742
00743
00744 static int ControlPolygonFlatEnough(value_type *VT)
00745 {
00746 int i;
00747 distance_type distance[W_DEGREE];
00748 distance_type max_distance_above;
00749 distance_type max_distance_below;
00750 time_type intercept_1, intercept_2, left_intercept, right_intercept;
00751 distance_type a, b, c;
00752
00753
00754
00755
00756 {
00757 distance_type abSquared;
00758
00759
00760
00761 a = VT[0][1] - VT[W_DEGREE][1];
00762 b = VT[W_DEGREE][0] - VT[0][0];
00763 c = VT[0][0] * VT[W_DEGREE][1] - VT[W_DEGREE][0] * VT[0][1];
00764
00765 abSquared = (a * a) + (b * b);
00766
00767 for (i = 1; i < W_DEGREE; i++)
00768 {
00769
00770 distance[i] = a * VT[i][0] + b * VT[i][1] + c;
00771 if (distance[i] > 0.0) distance[i] = (distance[i] * distance[i]) / abSquared;
00772 if (distance[i] < 0.0) distance[i] = -(distance[i] * distance[i]) / abSquared;
00773 }
00774 }
00775
00776
00777 max_distance_above = max_distance_below = 0.0;
00778
00779 for (i = 1; i < W_DEGREE; i++)
00780 {
00781 if (distance[i] < 0.0) max_distance_below = MIN(max_distance_below, distance[i]);
00782 if (distance[i] > 0.0) max_distance_above = MAX(max_distance_above, distance[i]);
00783 }
00784
00785
00786 intercept_1 = -(c + max_distance_above)/a;
00787
00788
00789 intercept_2 = -(c + max_distance_below)/a;
00790
00791
00792 left_intercept = MIN(intercept_1, intercept_2);
00793 right_intercept = MAX(intercept_1, intercept_2);
00794
00795 return 0.5 * (right_intercept-left_intercept) < BEZIER_EPSILON ? 1 : 0;
00796 }
00797
00798
00799
00800
00801
00802
00803
00804
00805 static time_type ComputeXIntercept(value_type *VT)
00806 {
00807 distance_type YNM = VT[W_DEGREE][1] - VT[0][1];
00808 return (YNM*VT[0][0] - (VT[W_DEGREE][0] - VT[0][0])*VT[0][1]) / YNM;
00809 }
00810
00811
00812
00813
00814
00815
00816
00817
00818
00819
00820
00821 static int FindRoots(value_type *w, time_type *t, int depth)
00822 {
00823 int i;
00824 value_type Left[W_DEGREE+1];
00825 value_type Right[W_DEGREE+1];
00826 int left_count;
00827 int right_count;
00828 time_type left_t[W_DEGREE+1];
00829 time_type right_t[W_DEGREE+1];
00830
00831 switch (CrossingCount(w))
00832 {
00833 case 0 :
00834 {
00835 return 0;
00836 }
00837 case 1 :
00838 {
00839
00840
00841 if (depth >= MAXDEPTH)
00842 {
00843 t[0] = (w[0][0] + w[W_DEGREE][0]) / 2.0;
00844 return 1;
00845 }
00846 if (ControlPolygonFlatEnough(w))
00847 {
00848 t[0] = ComputeXIntercept(w);
00849 return 1;
00850 }
00851 break;
00852 }
00853 }
00854
00855
00856
00857 Bezier(w, W_DEGREE, 0.5, Left, Right);
00858 left_count = FindRoots(Left, left_t, depth+1);
00859 right_count = FindRoots(Right, right_t, depth+1);
00860
00861
00862 for (i = 0; i < left_count; i++) t[i] = left_t[i];
00863 for (i = 0; i < right_count; i++) t[i+left_count] = right_t[i];
00864
00865
00866 return (left_count+right_count);
00867 }
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878 static void ConvertToBezierForm(const value_type& P, value_type *VT, value_type w[W_DEGREE+1])
00879 {
00880 int i, j, k, m, n, ub, lb;
00881 int row, column;
00882 value_type c[DEGREE+1];
00883 value_type d[DEGREE];
00884 distance_type cdTable[3][4];
00885 static distance_type z[3][4] = {
00886 {1.0, 0.6, 0.3, 0.1},
00887 {0.4, 0.6, 0.6, 0.4},
00888 {0.1, 0.3, 0.6, 1.0}};
00889
00890
00891
00892 for (i = 0; i <= DEGREE; i++)
00893 c[i] = VT[i] - P;
00894
00895
00896
00897 for (i = 0; i <= DEGREE - 1; i++)
00898 d[i] = (VT[i+1] - VT[i]) * 3.0;
00899
00900
00901
00902 for (row = 0; row <= DEGREE - 1; row++)
00903 for (column = 0; column <= DEGREE; column++)
00904 cdTable[row][column] = d[row] * c[column];
00905
00906
00907
00908 for (i = 0; i <= W_DEGREE; i++)
00909 {
00910 w[i][0] = (distance_type)(i) / W_DEGREE;
00911 w[i][1] = 0.0;
00912 }
00913
00914 n = DEGREE;
00915 m = DEGREE-1;
00916 for (k = 0; k <= n + m; k++)
00917 {
00918 lb = MAX(0, k - m);
00919 ub = MIN(k, n);
00920 for (i = lb; i <= ub; i++)
00921 {
00922 j = k - i;
00923 w[i+j][1] += cdTable[j][i] * z[j][i];
00924 }
00925 }
00926 }
00927
00928
00929
00930
00931
00932
00933
00934
00935
00936
00937 static time_type NearestPointOnCurve(const value_type& P, value_type VT[4])
00938 {
00939 value_type w[W_DEGREE+1];
00940 time_type t_candidate[W_DEGREE];
00941 int n_solutions;
00942 time_type t;
00943
00944
00945 ConvertToBezierForm(P, VT, w);
00946
00947
00948 n_solutions = FindRoots(w, t_candidate, 0);
00949
00950
00951 {
00952 distance_type dist, new_dist;
00953 value_type p, v;
00954 int i;
00955
00956
00957 dist = (P - VT[0]).mag_squared();
00958 t = 0.0;
00959
00960
00961 for (i = 0; i < n_solutions; i++)
00962 {
00963 p = Bezier(VT, DEGREE, t_candidate[i], (value_type *)NULL, (value_type *)NULL);
00964 new_dist = (P - p).mag_squared();
00965 if (new_dist < dist)
00966 {
00967 dist = new_dist;
00968 t = t_candidate[i];
00969 }
00970 }
00971
00972
00973 new_dist = (P - VT[DEGREE]).mag_squared();
00974 if (new_dist < dist)
00975 {
00976 dist = new_dist;
00977 t = 1.0;
00978 }
00979 }
00980
00981
00982 return t;
00983 }
00984 };
00985
00986 _ETL_END_NAMESPACE
00987
00988
00989
00990
00991
00992 #endif