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
00038
00039
00040
00041 _ETL_BEGIN_NAMESPACE
00042
00043 template<typename V,typename T> class bezier;
00044
00046
00047
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
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
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
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
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 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
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;
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
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
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
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
00262
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
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;
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
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
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
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
00342
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
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
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
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
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
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
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
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
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
00584 lt[0] = a;
00585 rt[3] = d;
00586
00587
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
00593 lt[2] = affine_func(lt[1],temp,t);
00594 rt[1] = affine_func(temp,rt[2],t);
00595
00596
00597 lt[3] = rt[0] = affine_func(lt[2],rt[1],t);
00598
00599
00600 lt.set_r(get_r());
00601 rt.set_s(get_s());
00602
00603 lt.sync();
00604 rt.sync();
00605
00606
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
00638
00639
00640
00641 #endif