00001
00025
00026
00027 #ifndef __ETL_BEZIER_H
00028 #define __ETL_BEZIER_H
00029
00030
00031
00032 #include "_curve_func.h"
00033 #include <ETL/fixed>
00034
00035
00036
00037 #define MAXDEPTH 64
00038
00039
00040 #define SGN(a) (((a)<0) ? -1 : 1)
00041
00042
00043 #ifndef MIN
00044 #define MIN(a,b) (((a)<(b))?(a):(b))
00045 #endif
00046
00047
00048 #ifndef MAX
00049 #define MAX(a,b) (((a)>(b))?(a):(b))
00050 #endif
00051
00052 #define BEZIER_EPSILON (ldexp(1.0,-MAXDEPTH-1))
00053
00054 #define DEGREE 3
00055 #define W_DEGREE 5
00056
00057
00058
00059
00060
00061 _ETL_BEGIN_NAMESPACE
00062
00063 template<typename V,typename T> class bezier;
00064
00066
00067
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
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
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
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 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
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;
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
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
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
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
00282
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
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;
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
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
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
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
00362
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
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
00431
00432
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 #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
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 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);
00561
00562 for(;i;i--)
00563 {
00564
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
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
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
00620 lt[0] = a;
00621 rt[3] = d;
00622
00623
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
00629 lt[2] = affine_func(lt[1],temp,t);
00630 rt[1] = affine_func(temp,rt[2],t);
00631
00632
00633 lt[3] = rt[0] = affine_func(lt[2],rt[1],t);
00634
00635
00636 lt.set_r(get_r());
00637 rt.set_s(get_s());
00638
00639 lt.sync();
00640 rt.sync();
00641
00642
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
00673
00674
00675
00676
00677
00678
00679
00680
00681
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;
00686 value_type Vtemp[W_DEGREE+1][W_DEGREE+1];
00687
00688
00689 for (j = 0; j <= degree; j++)
00690 Vtemp[0][j] = VT[j];
00691
00692
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
00713
00714
00715
00716
00717
00718 static int CrossingCount(value_type *VT)
00719 {
00720 int i;
00721 int n_crossings = 0;
00722 int sign, old_sign;
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
00737
00738
00739
00740
00741
00742 static int ControlPolygonFlatEnough(value_type *VT)
00743 {
00744 int i;
00745 distance_type distance[W_DEGREE];
00746 distance_type max_distance_above;
00747 distance_type max_distance_below;
00748 time_type intercept_1, intercept_2, left_intercept, right_intercept;
00749 distance_type a, b, c;
00750
00751
00752
00753
00754 {
00755 distance_type abSquared;
00756
00757
00758
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
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
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
00784 intercept_1 = -(c + max_distance_above)/a;
00785
00786
00787 intercept_2 = -(c + max_distance_below)/a;
00788
00789
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
00798
00799
00800
00801
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
00811
00812
00813
00814
00815
00816
00817
00818
00819 static int FindRoots(value_type *w, time_type *t, int depth)
00820 {
00821 int i;
00822 value_type Left[W_DEGREE+1];
00823 value_type Right[W_DEGREE+1];
00824 int left_count;
00825 int right_count;
00826 time_type left_t[W_DEGREE+1];
00827 time_type right_t[W_DEGREE+1];
00828
00829 switch (CrossingCount(w))
00830 {
00831 case 0 :
00832 {
00833 return 0;
00834 }
00835 case 1 :
00836 {
00837
00838
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
00854
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
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
00864 return (left_count+right_count);
00865 }
00866
00867
00868
00869
00870
00871
00872
00873
00874
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;
00880 value_type c[DEGREE+1];
00881 value_type d[DEGREE];
00882 distance_type cdTable[3][4];
00883 static distance_type z[3][4] = {
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
00889
00890 for (i = 0; i <= DEGREE; i++)
00891 c[i] = VT[i] - P;
00892
00893
00894
00895 for (i = 0; i <= DEGREE - 1; i++)
00896 d[i] = (VT[i+1] - VT[i]) * 3.0;
00897
00898
00899
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
00905
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
00928
00929
00930
00931
00932
00933
00934
00935 static time_type NearestPointOnCurve(const value_type& P, value_type VT[4])
00936 {
00937 value_type w[W_DEGREE+1];
00938 time_type t_candidate[W_DEGREE];
00939 int n_solutions;
00940 time_type t;
00941
00942
00943 ConvertToBezierForm(P, VT, w);
00944
00945
00946 n_solutions = FindRoots(w, t_candidate, 0);
00947
00948
00949 {
00950 distance_type dist, new_dist;
00951 value_type p, v;
00952 int i;
00953
00954
00955 dist = (P - VT[0]).mag_squared();
00956 t = 0.0;
00957
00958
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
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
00980 return t;
00981 }
00982 };
00983
00984 _ETL_END_NAMESPACE
00985
00986
00987
00988
00989
00990 #endif