ETL/tags/ETL_0_04_08/ETL/ETL/_bezier.h

   1 /*! ========================================================================
   2 ** Extended Template Library
   3 ** Bezier Template Class Implementation
   4 ** $Id: _bezier.h,v 1.1.1.1 2005/01/04 01:31:46 darco Exp $
   5 **
   6 ** Copyright (c) 2002 Robert B. Quattlebaum Jr.
   7 **
   8 ** This package is free software; you can redistribute it and/or
   9 ** modify it under the terms of the GNU General Public License as
  10 ** published by the Free Software Foundation; either version 2 of
  11 ** the License, or (at your option) any later version.
  12 **
  13 ** This package is distributed in the hope that it will be useful,
  14 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 ** General Public License for more details.
  17 **
  18 ** === N O T E S ===========================================================
  19 **
  20 ** This is an internal header file, included by other ETL headers.
  21 ** You should not attempt to use it directly.
  22 **
  23 ** ========================================================================= */
  24
  25 /* === S T A R T =========================================================== */
  26
  27 #ifndef __ETL_BEZIER_H
  28 #define __ETL_BEZIER_H
  29
  30 /* === H E A D E R S ======================================================= */
  31
  32 #include "_curve_func.h"
  33 #include <ETL/fixed>
  34
  35 /* === M A C R O S ========================================================= */
  36
  37 /* === T Y P E D E F S ===================================================== */
  38
  39 /* === C L A S S E S & S T R U C T S ======================================= */
  40
  41 _ETL_BEGIN_NAMESPACE
  42
  43 template<typename V,typename T> class bezier;
  44
  45 //! Cubic Bezier Curve Base Class
  46 // This generic implementation uses the DeCasteljau algorithm.
  47 // Works for just about anything that has an affine combination function
  48 template <typename V,typename T=float>
  49 class bezier_base : public std::unary_function<T,V>
  50 {
  51 public:
  52         typedef V value_type;
  53         typedef T time_type;
  54
  55 private:
  56         value_type a,b,c,d;
  57         time_type r,s;
  58
  59 protected:
  60         affine_combo<value_type,time_type> affine_func;
  61
  62 public:
  63         bezier_base():r(0.0),s(1.0) { }
  64         bezier_base(
  65                 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
  66                 const time_type &r=0.0, const time_type &s=1.0):
  67                 a(a),b(b),c(c),d(d),r(r),s(s) { sync(); }
  68
  69         void sync()
  70         {
  71         }
  72
  73         value_type
  74         operator()(time_type t)const
  75         {
  76                 t=(t-r)/(s-r);
  77                 return
  78                 affine_func(
  79                         affine_func(
  80                                 affine_func(a,b,t),
  81                                 affine_func(b,c,t)
  82                         ,t),
  83                         affine_func(
  84                                 affine_func(b,c,t),
  85                                 affine_func(c,d,t)
  86                         ,t)
  87                 ,t);
  88         }
  89
  90         /*
  91         void evaluate(time_type t, value_type &f, value_type &df) const
  92         {
  93                 t=(t-r)/(s-r);
  94
  95                 value_type p1 = affine_func(
  96                                                         affine_func(a,b,t),
  97                                                         affine_func(b,c,t)
  98                                                         ,t);
  99                 value_type p2 = affine_func(
 100                                                         affine_func(b,c,t),
 101                                                         affine_func(c,d,t)
 102                                                 ,t);
 103
 104                 f = affine_func(p1,p2,t);
 105                 df = (p2-p1)*3;
 106         }
 107         */
 108
 109         void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; }
 110         void set_r(time_type new_r) { r=new_r; }
 111         void set_s(time_type new_s) { s=new_s; }
 112         const time_type &get_r()const { return r; }
 113         const time_type &get_s()const { return s; }
 114         time_type get_dt()const { return s-r; }
 115
 116         bool intersect_hull(const bezier_base<value_type,time_type> &x)const
 117         {
 118                 return 0;
 119         }
 120
 121         //! Bezier curve intersection function
 122         /*! Calculates the time of intersection
 123         **      for the calling curve.
 124         **
 125         **      I still have not figured out a good generic
 126         **      method of doing this for a bi-infinite
 127         **      cubic bezier curve calculated with the DeCasteljau
 128         **      algorithm.
 129         **
 130         **      One method, although it does not work for the
 131         **      entire bi-infinite curve, is to iteratively
 132         **      intersect the hulls. However, we would only detect
 133         **      intersections that occur between R and S.
 134         **
 135         **      It is entirely possible that a new construct similar
 136         **      to the affine combination function will be necessary
 137         **      for this to work properly.
 138         **
 139         **      For now, this function is BROKEN. (although it works
 140         **      for the floating-point specializations, using newton's method)
 141         */
 142         time_type intersect(const bezier_base<value_type,time_type> &x, time_type near=0.0)const
 143         {
 144                 return 0;
 145         }
 146
 147         /* subdivide at some time t into 2 separate curves left and right
 148
 149                 b0 l1
 150                 *               0+1 l2
 151                 b1              *               1+2*1+2 l3
 152                 *               1+2             *                       0+3*1+3*2+3 l4,r1
 153                 b2              *               1+2*2+2 r2      *
 154                 *               2+3     r3      *
 155                 b3 r4   *
 156                 *
 157
 158                 0.1 2.3 ->      0.1 2 3 4 5.6
 159         */
 160 /*      void subdivide(bezier_base *left, bezier_base *right, const time_type &time = (time_type)0.5) const
 161         {
 162                 time_type t = (time-r)/(s-r);
 163                 bezier_base lt,rt;
 164
 165                 value_type temp;
 166
 167                 //1st stage points to keep
 168                 lt.a = a;
 169                 rt.d = d;
 170
 171                 //2nd stage calc
 172                 lt.b = affine_func(a,b,t);
 173                 temp = affine_func(b,c,t);
 174                 rt.c = affine_func(c,d,t);
 175
 176                 //3rd stage calc
 177                 lt.c = affine_func(lt.b,temp,t);
 178                 rt.b = affine_func(temp,rt.c,t);
 179
 180                 //last stage calc
 181                 lt.d = rt.a = affine_func(lt.c,rt.b,t);
 182
 183                 //set the time range for l,r (the inside values should be 1, 0 respectively)
 184                 lt.r = r;
 185                 rt.s = s;
 186
 187                 //give back the curves
 188                 if(left) *left = lt;
 189                 if(right) *right = rt;
 190         }
 191         */
 192         value_type &
 193         operator[](int i)
 194         { return (&a)[i]; }
 195
 196         const value_type &
 197         operator[](int i) const
 198         { return (&a)[i]; }
 199 };
 200
 201
 202 #if 1
 203 // Fast float implementation of a cubic bezier curve
 204 template <>
 205 class bezier_base<float,float> : public std::unary_function<float,float>
 206 {
 207 public:
 208         typedef float value_type;
 209         typedef float time_type;
 210 private:
 211         affine_combo<value_type,time_type> affine_func;
 212         value_type a,b,c,d;
 213         time_type r,s;
 214
 215         value_type _coeff[4];
 216         time_type drs; // reciprocal of (s-r)
 217 public:
 218         bezier_base():r(0.0),s(1.0),drs(1.0) { }
 219         bezier_base(
 220                 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
 221                 const time_type &r=0.0, const time_type &s=1.0):
 222                 a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
 223
 224         void sync()
 225         {
 226 //              drs=1.0/(s-r);
 227                 _coeff[0]=                 a;
 228                 _coeff[1]=           b*3 - a*3;
 229                 _coeff[2]=     c*3 - b*6 + a*3;
 230                 _coeff[3]= d - c*3 + b*3 - a;
 231         }
 232
 233         // Cost Summary: 4 products, 3 sums, and 1 difference.
 234         inline value_type
 235         operator()(time_type t)const
 236         { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
 237
 238         void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
 239         void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
 240         void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
 241         const time_type &get_r()const { return r; }
 242         const time_type &get_s()const { return s; }
 243         time_type get_dt()const { return s-r; }
 244
 245         //! Bezier curve intersection function
 246         /*! Calculates the time of intersection
 247         **      for the calling curve.
 248         */
 249         time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
 250         {
 251                 //BROKEN - the time values of the 2 curves should be independent
 252                 value_type system[4];
 253                 system[0]=_coeff[0]-x._coeff[0];
 254                 system[1]=_coeff[1]-x._coeff[1];
 255                 system[2]=_coeff[2]-x._coeff[2];
 256                 system[3]=_coeff[3]-x._coeff[3];
 257
 258                 t-=r;
 259                 t*=drs;
 260
 261                 // Newton's method
 262                 // Inner loop cost summary: 7 products, 5 sums, 1 difference
 263                 for(;i;i--)
 264                         t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
 265                                 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
 266
 267                 t*=(s-r);
 268                 t+=r;
 269
 270                 return t;
 271         }
 272
 273         value_type &
 274         operator[](int i)
 275         { return (&a)[i]; }
 276
 277         const value_type &
 278         operator[](int i) const
 279         { return (&a)[i]; }
 280 };
 281
 282
 283 // Fast double implementation of a cubic bezier curve
 284 template <>
 285 class bezier_base<double,float> : public std::unary_function<float,double>
 286 {
 287 public:
 288         typedef double value_type;
 289         typedef float time_type;
 290 private:
 291         affine_combo<value_type,time_type> affine_func;
 292         value_type a,b,c,d;
 293         time_type r,s;
 294
 295         value_type _coeff[4];
 296         time_type drs; // reciprocal of (s-r)
 297 public:
 298         bezier_base():r(0.0),s(1.0),drs(1.0) { }
 299         bezier_base(
 300                 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
 301                 const time_type &r=0.0, const time_type &s=1.0):
 302                 a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
 303
 304         void sync()
 305         {
 306 //              drs=1.0/(s-r);
 307                 _coeff[0]=                 a;
 308                 _coeff[1]=           b*3 - a*3;
 309                 _coeff[2]=     c*3 - b*6 + a*3;
 310                 _coeff[3]= d - c*3 + b*3 - a;
 311         }
 312
 313         // 4 products, 3 sums, and 1 difference.
 314         inline value_type
 315         operator()(time_type t)const
 316         { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
 317
 318         void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
 319         void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
 320         void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
 321         const time_type &get_r()const { return r; }
 322         const time_type &get_s()const { return s; }
 323         time_type get_dt()const { return s-r; }
 324
 325         //! Bezier curve intersection function
 326         /*! Calculates the time of intersection
 327         **      for the calling curve.
 328         */
 329         time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
 330         {
 331                 //BROKEN - the time values of the 2 curves should be independent
 332                 value_type system[4];
 333                 system[0]=_coeff[0]-x._coeff[0];
 334                 system[1]=_coeff[1]-x._coeff[1];
 335                 system[2]=_coeff[2]-x._coeff[2];
 336                 system[3]=_coeff[3]-x._coeff[3];
 337
 338                 t-=r;
 339                 t*=drs;
 340
 341                 // Newton's method
 342                 // Inner loop: 7 products, 5 sums, 1 difference
 343                 for(;i;i--)
 344                         t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
 345                                 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
 346
 347                 t*=(s-r);
 348                 t+=r;
 349
 350                 return t;
 351         }
 352
 353         value_type &
 354         operator[](int i)
 355         { return (&a)[i]; }
 356
 357         const value_type &
 358         operator[](int i) const
 359         { return (&a)[i]; }
 360 };
 361
 362 //#ifdef __FIXED__
 363
 364 // Fast double implementation of a cubic bezier curve
 365 /*
 366 template <>
 367 template <class T,unsigned int FIXED_BITS>
 368 class bezier_base<fixed_base<T,FIXED_BITS> > : std::unary_function<fixed_base<T,FIXED_BITS>,fixed_base<T,FIXED_BITS> >
 369 {
 370 public:
 371         typedef fixed_base<T,FIXED_BITS> value_type;
 372         typedef fixed_base<T,FIXED_BITS> time_type;
 373
 374 private:
 375         affine_combo<value_type,time_type> affine_func;
 376         value_type a,b,c,d;
 377         time_type r,s;
 378
 379         value_type _coeff[4];
 380         time_type drs; // reciprocal of (s-r)
 381 public:
 382         bezier_base():r(0.0),s(1.0),drs(1.0) { }
 383         bezier_base(
 384                 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
 385                 const time_type &r=0, const time_type &s=1):
 386                 a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
 387
 388         void sync()
 389         {
 390                 drs=time_type(1)/(s-r);
 391                 _coeff[0]=                 a;
 392                 _coeff[1]=           b*3 - a*3;
 393                 _coeff[2]=     c*3 - b*6 + a*3;
 394                 _coeff[3]= d - c*3 + b*3 - a;
 395         }
 396
 397         // 4 products, 3 sums, and 1 difference.
 398         inline value_type
 399         operator()(time_type t)const
 400         { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
 401
 402         void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=time_type(1)/(s-r); }
 403         void set_r(time_type new_r) { r=new_r; drs=time_type(1)/(s-r); }
 404         void set_s(time_type new_s) { s=new_s; drs=time_type(1)/(s-r); }
 405         const time_type &get_r()const { return r; }
 406         const time_type &get_s()const { return s; }
 407         time_type get_dt()const { return s-r; }
 408
 409         //! Bezier curve intersection function
 410         //! Calculates the time of intersection
 411         //      for the calling curve.
 412         //
 413         time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0,int i=15)const
 414         {
 415                 value_type system[4];
 416                 system[0]=_coeff[0]-x._coeff[0];
 417                 system[1]=_coeff[1]-x._coeff[1];
 418                 system[2]=_coeff[2]-x._coeff[2];
 419                 system[3]=_coeff[3]-x._coeff[3];
 420
 421                 t-=r;
 422                 t*=drs;
 423
 424                 // Newton's method
 425                 // Inner loop: 7 products, 5 sums, 1 difference
 426                 for(;i;i--)
 427                         t-=(time_type) ( (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
 428                                 (system[1]+(system[2]*2+(system[3]*3)*t)*t) );
 429
 430                 t*=(s-r);
 431                 t+=r;
 432
 433                 return t;
 434         }
 435
 436         value_type &
 437         operator[](int i)
 438         { return (&a)[i]; }
 439
 440         const value_type &
 441         operator[](int i) const
 442         { return (&a)[i]; }
 443 };
 444 */
 445 //#endif
 446
 447 #endif
 448
 449
 450
 451 template <typename V, typename T>
 452 class bezier_iterator
 453 {
 454 public:
 455
 456         struct iterator_category {};
 457         typedef V value_type;
 458         typedef T difference_type;
 459         typedef V reference;
 460
 461 private:
 462         difference_type t;
 463         difference_type dt;
 464         bezier_base<V,T>        curve;
 465
 466 public:
 467
 468 /*
 469         reference
 470         operator*(void)const { return curve(t); }
 471         const surface_iterator&
 472
 473         operator++(void)
 474         { t+=dt; return &this; }
 475
 476         const surface_iterator&
 477         operator++(int)
 478         { hermite_iterator _tmp=*this; t+=dt; return _tmp; }
 479
 480         const surface_iterator&
 481         operator--(void)
 482         { t-=dt; return &this; }
 483
 484         const surface_iterator&
 485         operator--(int)
 486         { hermite_iterator _tmp=*this; t-=dt; return _tmp; }
 487
 488
 489         surface_iterator
 490         operator+(difference_type __n) const
 491         { return surface_iterator(data+__n[0]+__n[1]*pitch,pitch); }
 492
 493         surface_iterator
 494         operator-(difference_type __n) const
 495         { return surface_iterator(data-__n[0]-__n[1]*pitch,pitch); }
 496 */
 497
 498 };
 499
 500 template <typename V,typename T=float>
 501 class bezier : public bezier_base<V,T>
 502 {
 503 public:
 504         typedef V value_type;
 505         typedef T time_type;
 506         typedef float distance_type;
 507         typedef bezier_iterator<V,T> iterator;
 508         typedef bezier_iterator<V,T> const_iterator;
 509
 510         distance_func<value_type> dist;
 511
 512         using bezier_base<V,T>::get_r;
 513         using bezier_base<V,T>::get_s;
 514         using bezier_base<V,T>::get_dt;
 515
 516 public:
 517         bezier() { }
 518         bezier(const value_type &a, const value_type &b, const value_type &c, const value_type &d):
 519                 bezier_base<V,T>(a,b,c,d) { }
 520
 521
 522         const_iterator begin()const;
 523         const_iterator end()const;
 524
 525         time_type find_closest(const value_type& x, int i=7, time_type r=(0), time_type s=(1))const
 526         {
 527                 float t((r+s)*0.5);
 528                 for(;i;i--)
 529                 {
 530                         if(dist(operator()((s-r)*(1.0/3.0)+r),x) < dist(operator()((s-r)*(2.0/3.0)+r),x))
 531                                 s=t;
 532                         else
 533                                 r=t;
 534                         t=((r+s)*0.5);
 535                 }
 536                 return t;
 537         }
 538
 539
 540         distance_type find_distance(time_type r, time_type s, int steps=7)const
 541         {
 542                 const time_type inc((s-r)/steps);
 543                 distance_type ret(0);
 544                 value_type last(operator()(r));
 545
 546                 for(r+=inc;r<s;r+=inc)
 547                 {
 548                         const value_type n(operator()(r));
 549                         ret+=dist.uncook(dist(last,n));
 550                         last=n;
 551                 }
 552                 ret+=dist.uncook(dist(last,operator()(r)))*(s-(r-inc))/inc;
 553
 554                 return ret;
 555         }
 556
 557         distance_type length()const { return find_distance(get_r(),get_s()); }
 558
 559         /* subdivide at some time t into 2 separate curves left and right
 560
 561                 b0 l1
 562                 *               0+1 l2
 563                 b1              *               1+2*1+2 l3
 564                 *               1+2             *                       0+3*1+3*2+3 l4,r1
 565                 b2              *               1+2*2+2 r2      *
 566                 *               2+3     r3      *
 567                 b3 r4   *
 568                 *
 569
 570                 0.1 2.3 ->      0.1 2 3 4 5.6
 571         */
 572         void subdivide(bezier *left, bezier *right, const time_type &time = (time_type)0.5) const
 573         {
 574                 time_type t=(t-get_r())/get_dt();
 575                 bezier lt,rt;
 576
 577                 value_type temp;
 578                 const value_type& a((*this)[0]);
 579                 const value_type& b((*this)[1]);
 580                 const value_type& c((*this)[2]);
 581                 const value_type& d((*this)[3]);
 582
 583                 //1st stage points to keep
 584                 lt[0] = a;
 585                 rt[3] = d;
 586
 587                 //2nd stage calc
 588                 lt[1] = affine_func(a,b,t);
 589                 temp = affine_func(b,c,t);
 590                 rt[2] = affine_func(c,d,t);
 591
 592                 //3rd stage calc
 593                 lt[2] = affine_func(lt[1],temp,t);
 594                 rt[1] = affine_func(temp,rt[2],t);
 595
 596                 //last stage calc
 597                 lt[3] = rt[0] = affine_func(lt[2],rt[1],t);
 598
 599                 //set the time range for l,r (the inside values should be 1, 0 respectively)
 600                 lt.set_r(get_r());
 601                 rt.set_s(get_s());
 602
 603                 lt.sync();
 604                 rt.sync();
 605
 606                 //give back the curves
 607                 if(left) *left = lt;
 608                 if(right) *right = rt;
 609         }
 610
 611
 612         void evaluate(time_type t, value_type &f, value_type &df) const
 613         {
 614                 t=(t-get_r())/get_dt();
 615
 616                 const value_type& a((*this)[0]);
 617                 const value_type& b((*this)[1]);
 618                 const value_type& c((*this)[2]);
 619                 const value_type& d((*this)[3]);
 620
 621                 const value_type p1 = affine_func(
 622                                                         affine_func(a,b,t),
 623                                                         affine_func(b,c,t)
 624                                                         ,t);
 625                 const value_type p2 = affine_func(
 626                                                         affine_func(b,c,t),
 627                                                         affine_func(c,d,t)
 628                                                 ,t);
 629
 630                 f = affine_func(p1,p2,t);
 631                 df = (p2-p1)*3;
 632         }
 633 };
 634
 635 _ETL_END_NAMESPACE
 636
 637 /* === E X T E R N S ======================================================= */
 638
 639 /* === E N D =============================================================== */
 640
 641 #endif