ETL/trunk/ETL/_bezier.h

   1 /*! ========================================================================
   2 ** Extended Template Library
   3 ** Bezier Template Class Implementation
   4 ** $Id$
   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 #define MAXDEPTH 64     /*  Maximum depth for recursion */
  38
  39 /* take binary sign of a, either -1, or 1 if >= 0 */
  40 #define SGN(a)          (((a)<0) ? -1 : 1)
  41
  42 /* find minimum of a and b */
  43 #ifndef MIN
  44 #define MIN(a,b)        (((a)<(b))?(a):(b))
  45 #endif
  46
  47 /* find maximum of a and b */
  48 #ifndef MAX
  49 #define MAX(a,b)        (((a)>(b))?(a):(b))
  50 #endif
  51
  52 #define BEZIER_EPSILON  (ldexp(1.0,-MAXDEPTH-1)) /*Flatness control value */
  53 //#define       BEZIER_EPSILON  0.00005 /*Flatness control value */
  54 #define DEGREE  3                       /*  Cubic Bezier curve          */
  55 #define W_DEGREE 5                      /*  Degree of eqn to find roots of */
  56
  57 /* === T Y P E D E F S ===================================================== */
  58
  59 /* === C L A S S E S & S T R U C T S ======================================= */
  60
  61 _ETL_BEGIN_NAMESPACE
  62
  63 template<typename V,typename T> class bezier;
  64
  65 //! Cubic Bezier Curve Base Class
  66 // This generic implementation uses the DeCasteljau algorithm.
  67 // Works for just about anything that has an affine combination function
  68 template <typename V,typename T=float>
  69 class bezier_base : public std::unary_function<T,V>
  70 {
  71 public:
  72         typedef V value_type;
  73         typedef T time_type;
  74
  75 private:
  76         value_type a,b,c,d;
  77         time_type r,s;
  78
  79 protected:
  80         affine_combo<value_type,time_type> affine_func;
  81
  82 public:
  83         bezier_base():r(0.0),s(1.0) { }
  84         bezier_base(
  85                 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
  86                 const time_type &r=0.0, const time_type &s=1.0):
  87                 a(a),b(b),c(c),d(d),r(r),s(s) { sync(); }
  88
  89         void sync()
  90         {
  91         }
  92
  93         value_type
  94         operator()(time_type t)const
  95         {
  96                 t=(t-r)/(s-r);
  97                 return
  98                 affine_func(
  99                         affine_func(
 100                                 affine_func(a,b,t),
 101                                 affine_func(b,c,t)
 102                         ,t),
 103                         affine_func(
 104                                 affine_func(b,c,t),
 105                                 affine_func(c,d,t)
 106                         ,t)
 107                 ,t);
 108         }
 109
 110         /*
 111         void evaluate(time_type t, value_type &f, value_type &df) const
 112         {
 113                 t=(t-r)/(s-r);
 114
 115                 value_type p1 = affine_func(
 116                                                         affine_func(a,b,t),
 117                                                         affine_func(b,c,t)
 118                                                         ,t);
 119                 value_type p2 = affine_func(
 120                                                         affine_func(b,c,t),
 121                                                         affine_func(c,d,t)
 122                                                 ,t);
 123
 124                 f = affine_func(p1,p2,t);
 125                 df = (p2-p1)*3;
 126         }
 127         */
 128
 129         void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; }
 130         void set_r(time_type new_r) { r=new_r; }
 131         void set_s(time_type new_s) { s=new_s; }
 132         const time_type &get_r()const { return r; }
 133         const time_type &get_s()const { return s; }
 134         time_type get_dt()const { return s-r; }
 135
 136         bool intersect_hull(const bezier_base<value_type,time_type> &x)const
 137         {
 138                 return 0;
 139         }
 140
 141         //! Bezier curve intersection function
 142         /*! Calculates the time of intersection
 143         **      for the calling curve.
 144         **
 145         **      I still have not figured out a good generic
 146         **      method of doing this for a bi-infinite
 147         **      cubic bezier curve calculated with the DeCasteljau
 148         **      algorithm.
 149         **
 150         **      One method, although it does not work for the
 151         **      entire bi-infinite curve, is to iteratively
 152         **      intersect the hulls. However, we would only detect
 153         **      intersections that occur between R and S.
 154         **
 155         **      It is entirely possible that a new construct similar
 156         **      to the affine combination function will be necessary
 157         **      for this to work properly.
 158         **
 159         **      For now, this function is BROKEN. (although it works
 160         **      for the floating-point specializations, using newton's method)
 161         */
 162         time_type intersect(const bezier_base<value_type,time_type> &x, time_type near=0.0)const
 163         {
 164                 return 0;
 165         }
 166
 167         /* subdivide at some time t into 2 separate curves left and right
 168
 169                 b0 l1
 170                 *               0+1 l2
 171                 b1              *               1+2*1+2 l3
 172                 *               1+2             *                       0+3*1+3*2+3 l4,r1
 173                 b2              *               1+2*2+2 r2      *
 174                 *               2+3     r3      *
 175                 b3 r4   *
 176                 *
 177
 178                 0.1 2.3 ->      0.1 2 3 4 5.6
 179         */
 180 /*      void subdivide(bezier_base *left, bezier_base *right, const time_type &time = (time_type)0.5) const
 181         {
 182                 time_type t = (time-r)/(s-r);
 183                 bezier_base lt,rt;
 184
 185                 value_type temp;
 186
 187                 //1st stage points to keep
 188                 lt.a = a;
 189                 rt.d = d;
 190
 191                 //2nd stage calc
 192                 lt.b = affine_func(a,b,t);
 193                 temp = affine_func(b,c,t);
 194                 rt.c = affine_func(c,d,t);
 195
 196                 //3rd stage calc
 197                 lt.c = affine_func(lt.b,temp,t);
 198                 rt.b = affine_func(temp,rt.c,t);
 199
 200                 //last stage calc
 201                 lt.d = rt.a = affine_func(lt.c,rt.b,t);
 202
 203                 //set the time range for l,r (the inside values should be 1, 0 respectively)
 204                 lt.r = r;
 205                 rt.s = s;
 206
 207                 //give back the curves
 208                 if(left) *left = lt;
 209                 if(right) *right = rt;
 210         }
 211         */
 212         value_type &
 213         operator[](int i)
 214         { return (&a)[i]; }
 215
 216         const value_type &
 217         operator[](int i) const
 218         { return (&a)[i]; }
 219 };
 220
 221
 222 #if 1
 223 // Fast float implementation of a cubic bezier curve
 224 template <>
 225 class bezier_base<float,float> : public std::unary_function<float,float>
 226 {
 227 public:
 228         typedef float value_type;
 229         typedef float time_type;
 230 private:
 231         affine_combo<value_type,time_type> affine_func;
 232         value_type a,b,c,d;
 233         time_type r,s;
 234
 235         value_type _coeff[4];
 236         time_type drs; // reciprocal of (s-r)
 237 public:
 238         bezier_base():r(0.0),s(1.0),drs(1.0) { }
 239         bezier_base(
 240                 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
 241                 const time_type &r=0.0, const time_type &s=1.0):
 242                 a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
 243
 244         void sync()
 245         {
 246 //              drs=1.0/(s-r);
 247                 _coeff[0]=                 a;
 248                 _coeff[1]=           b*3 - a*3;
 249                 _coeff[2]=     c*3 - b*6 + a*3;
 250                 _coeff[3]= d - c*3 + b*3 - a;
 251         }
 252
 253         // Cost Summary: 4 products, 3 sums, and 1 difference.
 254         inline value_type
 255         operator()(time_type t)const
 256         { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
 257
 258         void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
 259         void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
 260         void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
 261         const time_type &get_r()const { return r; }
 262         const time_type &get_s()const { return s; }
 263         time_type get_dt()const { return s-r; }
 264
 265         //! Bezier curve intersection function
 266         /*! Calculates the time of intersection
 267         **      for the calling curve.
 268         */
 269         time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
 270         {
 271                 //BROKEN - the time values of the 2 curves should be independent
 272                 value_type system[4];
 273                 system[0]=_coeff[0]-x._coeff[0];
 274                 system[1]=_coeff[1]-x._coeff[1];
 275                 system[2]=_coeff[2]-x._coeff[2];
 276                 system[3]=_coeff[3]-x._coeff[3];
 277
 278                 t-=r;
 279                 t*=drs;
 280
 281                 // Newton's method
 282                 // Inner loop cost summary: 7 products, 5 sums, 1 difference
 283                 for(;i;i--)
 284                         t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
 285                                 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
 286
 287                 t*=(s-r);
 288                 t+=r;
 289
 290                 return t;
 291         }
 292
 293         value_type &
 294         operator[](int i)
 295         { return (&a)[i]; }
 296
 297         const value_type &
 298         operator[](int i) const
 299         { return (&a)[i]; }
 300 };
 301
 302
 303 // Fast double implementation of a cubic bezier curve
 304 template <>
 305 class bezier_base<double,float> : public std::unary_function<float,double>
 306 {
 307 public:
 308         typedef double value_type;
 309         typedef float time_type;
 310 private:
 311         affine_combo<value_type,time_type> affine_func;
 312         value_type a,b,c,d;
 313         time_type r,s;
 314
 315         value_type _coeff[4];
 316         time_type drs; // reciprocal of (s-r)
 317 public:
 318         bezier_base():r(0.0),s(1.0),drs(1.0) { }
 319         bezier_base(
 320                 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
 321                 const time_type &r=0.0, const time_type &s=1.0):
 322                 a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
 323
 324         void sync()
 325         {
 326 //              drs=1.0/(s-r);
 327                 _coeff[0]=                 a;
 328                 _coeff[1]=           b*3 - a*3;
 329                 _coeff[2]=     c*3 - b*6 + a*3;
 330                 _coeff[3]= d - c*3 + b*3 - a;
 331         }
 332
 333         // 4 products, 3 sums, and 1 difference.
 334         inline value_type
 335         operator()(time_type t)const
 336         { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
 337
 338         void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=1.0/(s-r); }
 339         void set_r(time_type new_r) { r=new_r; drs=1.0/(s-r); }
 340         void set_s(time_type new_s) { s=new_s; drs=1.0/(s-r); }
 341         const time_type &get_r()const { return r; }
 342         const time_type &get_s()const { return s; }
 343         time_type get_dt()const { return s-r; }
 344
 345         //! Bezier curve intersection function
 346         /*! Calculates the time of intersection
 347         **      for the calling curve.
 348         */
 349         time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0.0,int i=15)const
 350         {
 351                 //BROKEN - the time values of the 2 curves should be independent
 352                 value_type system[4];
 353                 system[0]=_coeff[0]-x._coeff[0];
 354                 system[1]=_coeff[1]-x._coeff[1];
 355                 system[2]=_coeff[2]-x._coeff[2];
 356                 system[3]=_coeff[3]-x._coeff[3];
 357
 358                 t-=r;
 359                 t*=drs;
 360
 361                 // Newton's method
 362                 // Inner loop: 7 products, 5 sums, 1 difference
 363                 for(;i;i--)
 364                         t-= (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
 365                                 (system[1]+(system[2]*2+(system[3]*3)*t)*t);
 366
 367                 t*=(s-r);
 368                 t+=r;
 369
 370                 return t;
 371         }
 372
 373         value_type &
 374         operator[](int i)
 375         { return (&a)[i]; }
 376
 377         const value_type &
 378         operator[](int i) const
 379         { return (&a)[i]; }
 380 };
 381
 382 //#ifdef __FIXED__
 383
 384 // Fast double implementation of a cubic bezier curve
 385 /*
 386 template <>
 387 template <class T,unsigned int FIXED_BITS>
 388 class bezier_base<fixed_base<T,FIXED_BITS> > : std::unary_function<fixed_base<T,FIXED_BITS>,fixed_base<T,FIXED_BITS> >
 389 {
 390 public:
 391         typedef fixed_base<T,FIXED_BITS> value_type;
 392         typedef fixed_base<T,FIXED_BITS> time_type;
 393
 394 private:
 395         affine_combo<value_type,time_type> affine_func;
 396         value_type a,b,c,d;
 397         time_type r,s;
 398
 399         value_type _coeff[4];
 400         time_type drs; // reciprocal of (s-r)
 401 public:
 402         bezier_base():r(0.0),s(1.0),drs(1.0) { }
 403         bezier_base(
 404                 const value_type &a, const value_type &b, const value_type &c, const value_type &d,
 405                 const time_type &r=0, const time_type &s=1):
 406                 a(a),b(b),c(c),d(d),r(r),s(s),drs(1.0/(s-r)) { sync(); }
 407
 408         void sync()
 409         {
 410                 drs=time_type(1)/(s-r);
 411                 _coeff[0]=                 a;
 412                 _coeff[1]=           b*3 - a*3;
 413                 _coeff[2]=     c*3 - b*6 + a*3;
 414                 _coeff[3]= d - c*3 + b*3 - a;
 415         }
 416
 417         // 4 products, 3 sums, and 1 difference.
 418         inline value_type
 419         operator()(time_type t)const
 420         { t-=r; t*=drs; return _coeff[0]+(_coeff[1]+(_coeff[2]+(_coeff[3])*t)*t)*t; }
 421
 422         void set_rs(time_type new_r, time_type new_s) { r=new_r; s=new_s; drs=time_type(1)/(s-r); }
 423         void set_r(time_type new_r) { r=new_r; drs=time_type(1)/(s-r); }
 424         void set_s(time_type new_s) { s=new_s; drs=time_type(1)/(s-r); }
 425         const time_type &get_r()const { return r; }
 426         const time_type &get_s()const { return s; }
 427         time_type get_dt()const { return s-r; }
 428
 429         //! Bezier curve intersection function
 430         //! Calculates the time of intersection
 431         //      for the calling curve.
 432         //
 433         time_type intersect(const bezier_base<value_type,time_type> &x, time_type t=0,int i=15)const
 434         {
 435                 value_type system[4];
 436                 system[0]=_coeff[0]-x._coeff[0];
 437                 system[1]=_coeff[1]-x._coeff[1];
 438                 system[2]=_coeff[2]-x._coeff[2];
 439                 system[3]=_coeff[3]-x._coeff[3];
 440
 441                 t-=r;
 442                 t*=drs;
 443
 444                 // Newton's method
 445                 // Inner loop: 7 products, 5 sums, 1 difference
 446                 for(;i;i--)
 447                         t-=(time_type) ( (system[0]+(system[1]+(system[2]+(system[3])*t)*t)*t)/
 448                                 (system[1]+(system[2]*2+(system[3]*3)*t)*t) );
 449
 450                 t*=(s-r);
 451                 t+=r;
 452
 453                 return t;
 454         }
 455
 456         value_type &
 457         operator[](int i)
 458         { return (&a)[i]; }
 459
 460         const value_type &
 461         operator[](int i) const
 462         { return (&a)[i]; }
 463 };
 464 */
 465 //#endif
 466
 467 #endif
 468
 469
 470
 471 template <typename V, typename T>
 472 class bezier_iterator
 473 {
 474 public:
 475
 476         struct iterator_category {};
 477         typedef V value_type;
 478         typedef T difference_type;
 479         typedef V reference;
 480
 481 private:
 482         difference_type t;
 483         difference_type dt;
 484         bezier_base<V,T>        curve;
 485
 486 public:
 487
 488 /*
 489         reference
 490         operator*(void)const { return curve(t); }
 491         const surface_iterator&
 492
 493         operator++(void)
 494         { t+=dt; return &this; }
 495
 496         const surface_iterator&
 497         operator++(int)
 498         { hermite_iterator _tmp=*this; t+=dt; return _tmp; }
 499
 500         const surface_iterator&
 501         operator--(void)
 502         { t-=dt; return &this; }
 503
 504         const surface_iterator&
 505         operator--(int)
 506         { hermite_iterator _tmp=*this; t-=dt; return _tmp; }
 507
 508
 509         surface_iterator
 510         operator+(difference_type __n) const
 511         { return surface_iterator(data+__n[0]+__n[1]*pitch,pitch); }
 512
 513         surface_iterator
 514         operator-(difference_type __n) const
 515         { return surface_iterator(data-__n[0]-__n[1]*pitch,pitch); }
 516 */
 517
 518 };
 519
 520 template <typename V,typename T=float>
 521 class bezier : public bezier_base<V,T>
 522 {
 523 public:
 524         typedef V value_type;
 525         typedef T time_type;
 526         typedef float distance_type;
 527         typedef bezier_iterator<V,T> iterator;
 528         typedef bezier_iterator<V,T> const_iterator;
 529
 530         distance_func<value_type> dist;
 531
 532         using bezier_base<V,T>::get_r;
 533         using bezier_base<V,T>::get_s;
 534         using bezier_base<V,T>::get_dt;
 535
 536 public:
 537         bezier() { }
 538         bezier(const value_type &a, const value_type &b, const value_type &c, const value_type &d):
 539                 bezier_base<V,T>(a,b,c,d) { }
 540
 541
 542         const_iterator begin()const;
 543         const_iterator end()const;
 544
 545         time_type find_closest(bool fast, const value_type& x, int i=7)const
 546         {
 547             if (!fast)
 548             {
 549                         value_type array[4] = {
 550                                 bezier<V,T>::operator[](0),
 551                                 bezier<V,T>::operator[](1),
 552                                 bezier<V,T>::operator[](2),
 553                                 bezier<V,T>::operator[](3)};
 554                         return NearestPointOnCurve(x, array);
 555             }
 556             else
 557             {
 558                         time_type r(0), s(1);
 559                         float t((r+s)*0.5); /* half way between r and s */
 560
 561                         for(;i;i--)
 562                         {
 563                                 // compare 33% of the way between r and s with 67% of the way between r and s
 564                                 if(dist(operator()((s-r)*(1.0/3.0)+r), x) <
 565                                    dist(operator()((s-r)*(2.0/3.0)+r), x))
 566                                         s=t;
 567                                 else
 568                                         r=t;
 569                                 t=((r+s)*0.5);
 570                         }
 571                         return t;
 572                 }
 573         }
 574
 575         distance_type find_distance(time_type r, time_type s, int steps=7)const
 576         {
 577                 const time_type inc((s-r)/steps);
 578                 distance_type ret(0);
 579                 value_type last(operator()(r));
 580
 581                 for(r+=inc;r<s;r+=inc)
 582                 {
 583                         const value_type n(operator()(r));
 584                         ret+=dist.uncook(dist(last,n));
 585                         last=n;
 586                 }
 587                 ret+=dist.uncook(dist(last,operator()(r)))*(s-(r-inc))/inc;
 588
 589                 return ret;
 590         }
 591
 592         distance_type length()const { return find_distance(get_r(),get_s()); }
 593
 594         /* subdivide at some time t into 2 separate curves left and right
 595
 596                 b0 l1
 597                 *               0+1 l2
 598                 b1              *               1+2*1+2 l3
 599                 *               1+2             *                       0+3*1+3*2+3 l4,r1
 600                 b2              *               1+2*2+2 r2      *
 601                 *               2+3     r3      *
 602                 b3 r4   *
 603                 *
 604
 605                 0.1 2.3 ->      0.1 2 3 4 5.6
 606         */
 607         void subdivide(bezier *left, bezier *right, const time_type &time = (time_type)0.5) const
 608         {
 609                 time_type t=(t-get_r())/get_dt();
 610                 bezier lt,rt;
 611
 612                 value_type temp;
 613                 const value_type& a((*this)[0]);
 614                 const value_type& b((*this)[1]);
 615                 const value_type& c((*this)[2]);
 616                 const value_type& d((*this)[3]);
 617
 618                 //1st stage points to keep
 619                 lt[0] = a;
 620                 rt[3] = d;
 621
 622                 //2nd stage calc
 623                 lt[1] = affine_func(a,b,t);
 624                 temp = affine_func(b,c,t);
 625                 rt[2] = affine_func(c,d,t);
 626
 627                 //3rd stage calc
 628                 lt[2] = affine_func(lt[1],temp,t);
 629                 rt[1] = affine_func(temp,rt[2],t);
 630
 631                 //last stage calc
 632                 lt[3] = rt[0] = affine_func(lt[2],rt[1],t);
 633
 634                 //set the time range for l,r (the inside values should be 1, 0 respectively)
 635                 lt.set_r(get_r());
 636                 rt.set_s(get_s());
 637
 638                 lt.sync();
 639                 rt.sync();
 640
 641                 //give back the curves
 642                 if(left) *left = lt;
 643                 if(right) *right = rt;
 644         }
 645
 646
 647         void evaluate(time_type t, value_type &f, value_type &df) const
 648         {
 649                 t=(t-get_r())/get_dt();
 650
 651                 const value_type& a((*this)[0]);
 652                 const value_type& b((*this)[1]);
 653                 const value_type& c((*this)[2]);
 654                 const value_type& d((*this)[3]);
 655
 656                 const value_type p1 = affine_func(
 657                                                         affine_func(a,b,t),
 658                                                         affine_func(b,c,t)
 659                                                         ,t);
 660                 const value_type p2 = affine_func(
 661                                                         affine_func(b,c,t),
 662                                                         affine_func(c,d,t)
 663                                                 ,t);
 664
 665                 f = affine_func(p1,p2,t);
 666                 df = (p2-p1)*3;
 667         }
 668
 669 private:
 670         /*
 671          *  Bezier :
 672          *      Evaluate a Bezier curve at a particular parameter value
 673          *      Fill in control points for resulting sub-curves if "Left" and
 674          *      "Right" are non-null.
 675          *
 676          *    int                       degree;         Degree of bezier curve
 677          *    value_type        *VT;            Control pts
 678          *    time_type         t;                      Parameter value
 679          *    value_type        *Left;          RETURN left half ctl pts
 680          *    value_type        *Right;         RETURN right half ctl pts
 681          */
 682         static value_type Bezier(value_type *VT, int degree, time_type t, value_type *Left, value_type *Right)
 683         {
 684                 int             i, j;           /* Index variables      */
 685                 value_type      Vtemp[W_DEGREE+1][W_DEGREE+1];
 686
 687                 /* Copy control points  */
 688                 for (j = 0; j <= degree; j++)
 689                         Vtemp[0][j] = VT[j];
 690
 691                 /* Triangle computation */
 692                 for (i = 1; i <= degree; i++)
 693                         for (j =0 ; j <= degree - i; j++)
 694                         {
 695                                 Vtemp[i][j][0] = (1.0 - t) * Vtemp[i-1][j][0] + t * Vtemp[i-1][j+1][0];
 696                                 Vtemp[i][j][1] = (1.0 - t) * Vtemp[i-1][j][1] + t * Vtemp[i-1][j+1][1];
 697                         }
 698
 699                 if (Left != NULL)
 700                         for (j = 0; j <= degree; j++)
 701                                 Left[j]  = Vtemp[j][0];
 702
 703                 if (Right != NULL)
 704                         for (j = 0; j <= degree; j++)
 705                                 Right[j] = Vtemp[degree-j][j];
 706
 707                 return (Vtemp[degree][0]);
 708         }
 709
 710         /*
 711          * CrossingCount :
 712          *      Count the number of times a Bezier control polygon
 713          *      crosses the 0-axis. This number is >= the number of roots.
 714          *
 715          *    value_type        *VT;                    Control pts of Bezier curve
 716          */
 717         static int CrossingCount(value_type *VT)
 718         {
 719                 int     i;
 720                 int     n_crossings = 0;        /*  Number of zero-crossings    */
 721                 int             sign, old_sign;         /*  Sign of coefficients                */
 722
 723                 sign = old_sign = SGN(VT[0][1]);
 724                 for (i = 1; i <= W_DEGREE; i++)
 725                 {
 726                         sign = SGN(VT[i][1]);
 727                         if (sign != old_sign) n_crossings++;
 728                         old_sign = sign;
 729                 }
 730
 731                 return n_crossings;
 732         }
 733
 734         /*
 735          *  ControlPolygonFlatEnough :
 736          *      Check if the control polygon of a Bezier curve is flat enough
 737          *      for recursive subdivision to bottom out.
 738          *
 739          *    value_type        *VT;            Control points
 740          */
 741         static int ControlPolygonFlatEnough(value_type *VT)
 742         {
 743                 int                     i;                                      /* Index variable                                       */
 744                 distance_type   distance[W_DEGREE];     /* Distances from pts to line           */
 745                 distance_type   max_distance_above;     /* maximum of these                                     */
 746                 distance_type   max_distance_below;
 747                 time_type               intercept_1, intercept_2, left_intercept, right_intercept;
 748                 distance_type   a, b, c;                        /* Coefficients of implicit                     */
 749                 /* eqn for line from VT[0]-VT[deg]                      */
 750                 /* Find the  perpendicular distance                     */
 751                 /* from each interior control point to          */
 752                 /* line connecting VT[0] and VT[W_DEGREE]       */
 753                 {
 754                         distance_type   abSquared;
 755
 756                         /* Derive the implicit equation for line connecting first *
 757                          *  and last control points */
 758                         a = VT[0][1] - VT[W_DEGREE][1];
 759                         b = VT[W_DEGREE][0] - VT[0][0];
 760                         c = VT[0][0] * VT[W_DEGREE][1] - VT[W_DEGREE][0] * VT[0][1];
 761
 762                         abSquared = (a * a) + (b * b);
 763
 764                         for (i = 1; i < W_DEGREE; i++)
 765                         {
 766                                 /* Compute distance from each of the points to that line        */
 767                                 distance[i] = a * VT[i][0] + b * VT[i][1] + c;
 768                                 if (distance[i] > 0.0) distance[i] =  (distance[i] * distance[i]) / abSquared;
 769                                 if (distance[i] < 0.0) distance[i] = -(distance[i] * distance[i]) / abSquared;
 770                         }
 771                 }
 772
 773                 /* Find the largest distance */
 774                 max_distance_above = max_distance_below = 0.0;
 775
 776                 for (i = 1; i < W_DEGREE; i++)
 777                 {
 778                         if (distance[i] < 0.0) max_distance_below = MIN(max_distance_below, distance[i]);
 779                         if (distance[i] > 0.0) max_distance_above = MAX(max_distance_above, distance[i]);
 780                 }
 781
 782                 /* Implicit equation for "above" line */
 783                 intercept_1 = -(c + max_distance_above)/a;
 784
 785                 /*  Implicit equation for "below" line */
 786                 intercept_2 = -(c + max_distance_below)/a;
 787
 788                 /* Compute intercepts of bounding box   */
 789                 left_intercept = MIN(intercept_1, intercept_2);
 790                 right_intercept = MAX(intercept_1, intercept_2);
 791
 792                 return 0.5 * (right_intercept-left_intercept) < BEZIER_EPSILON ? 1 : 0;
 793         }
 794
 795         /*
 796          *  ComputeXIntercept :
 797          *      Compute intersection of chord from first control point to last
 798          *  with 0-axis.
 799          *
 800          *    value_type        *VT;                    Control points
 801          */
 802         static time_type ComputeXIntercept(value_type *VT)
 803         {
 804                 distance_type YNM = VT[W_DEGREE][1] - VT[0][1];
 805                 return (YNM*VT[0][0] - (VT[W_DEGREE][0] - VT[0][0])*VT[0][1]) / YNM;
 806         }
 807
 808         /*
 809          *  FindRoots :
 810          *      Given a 5th-degree equation in Bernstein-Bezier form, find
 811          *      all of the roots in the interval [0, 1].  Return the number
 812          *      of roots found.
 813          *
 814          *    value_type        *w;                             The control points
 815          *    time_type         *t;                             RETURN candidate t-values
 816          *    int                       depth;                  The depth of the recursion
 817          */
 818         static int FindRoots(value_type *w, time_type *t, int depth)
 819         {
 820                 int             i;
 821                 value_type      Left[W_DEGREE+1];       /* New left and right   */
 822                 value_type      Right[W_DEGREE+1];      /* control polygons             */
 823                 int             left_count;                     /* Solution count from  */
 824                 int                     right_count;            /* children                             */
 825                 time_type       left_t[W_DEGREE+1];     /* Solutions from kids  */
 826                 time_type       right_t[W_DEGREE+1];
 827
 828                 switch (CrossingCount(w))
 829                 {
 830                         case 0 :
 831                         {       /* No solutions here    */
 832                                 return 0;
 833                         }
 834                         case 1 :
 835                         {       /* Unique solution      */
 836                                 /* Stop recursion when the tree is deep enough          */
 837                                 /* if deep enough, return 1 solution at midpoint        */
 838                                 if (depth >= MAXDEPTH)
 839                                 {
 840                                         t[0] = (w[0][0] + w[W_DEGREE][0]) / 2.0;
 841                                         return 1;
 842                                 }
 843                                 if (ControlPolygonFlatEnough(w))
 844                                 {
 845                                         t[0] = ComputeXIntercept(w);
 846                                         return 1;
 847                                 }
 848                                 break;
 849                         }
 850                 }
 851
 852                 /* Otherwise, solve recursively after   */
 853                 /* subdividing control polygon                  */
 854                 Bezier(w, W_DEGREE, 0.5, Left, Right);
 855                 left_count  = FindRoots(Left,  left_t,  depth+1);
 856                 right_count = FindRoots(Right, right_t, depth+1);
 857
 858                 /* Gather solutions together    */
 859                 for (i = 0; i < left_count;  i++) t[i] = left_t[i];
 860                 for (i = 0; i < right_count; i++) t[i+left_count] = right_t[i];
 861
 862                 /* Send back total number of solutions  */
 863                 return (left_count+right_count);
 864         }
 865
 866         /*
 867          *  ConvertToBezierForm :
 868          *              Given a point and a Bezier curve, generate a 5th-degree
 869          *              Bezier-format equation whose solution finds the point on the
 870          *      curve nearest the user-defined point.
 871          *
 872          *    value_type&       P;                              The point to find t for
 873          *    value_type        *VT;                    The control points
 874          */
 875         static void ConvertToBezierForm(const value_type& P, value_type *VT, value_type w[W_DEGREE+1])
 876         {
 877                 int     i, j, k, m, n, ub, lb;
 878                 int     row, column;                            /* Table indices                                */
 879                 value_type      c[DEGREE+1];                    /* VT(i)'s - P                                  */
 880                 value_type      d[DEGREE];                              /* VT(i+1) - VT(i)                              */
 881                 distance_type   cdTable[3][4];          /* Dot product of c, d                  */
 882                 static distance_type z[3][4] = {        /* Precomputed "z" for cubics   */
 883                         {1.0, 0.6, 0.3, 0.1},
 884                         {0.4, 0.6, 0.6, 0.4},
 885                         {0.1, 0.3, 0.6, 1.0}};
 886
 887                 /* Determine the c's -- these are vectors created by subtracting */
 888                 /* point P from each of the control points                                               */
 889                 for (i = 0; i <= DEGREE; i++)
 890                         c[i] = VT[i] - P;
 891
 892                 /* Determine the d's -- these are vectors created by subtracting */
 893                 /* each control point from the next                                                              */
 894                 for (i = 0; i <= DEGREE - 1; i++)
 895                         d[i] = (VT[i+1] - VT[i]) * 3.0;
 896
 897                 /* Create the c,d table -- this is a table of dot products of the */
 898                 /* c's and d's                                                                                                    */
 899                 for (row = 0; row <= DEGREE - 1; row++)
 900                         for (column = 0; column <= DEGREE; column++)
 901                                 cdTable[row][column] = d[row] * c[column];
 902
 903                 /* Now, apply the z's to the dot products, on the skew diagonal */
 904                 /* Also, set up the x-values, making these "points"                             */
 905                 for (i = 0; i <= W_DEGREE; i++)
 906                 {
 907                         w[i][0] = (distance_type)(i) / W_DEGREE;
 908                         w[i][1] = 0.0;
 909                 }
 910
 911                 n = DEGREE;
 912                 m = DEGREE-1;
 913                 for (k = 0; k <= n + m; k++)
 914                 {
 915                         lb = MAX(0, k - m);
 916                         ub = MIN(k, n);
 917                         for (i = lb; i <= ub; i++)
 918                         {
 919                                 j = k - i;
 920                                 w[i+j][1] += cdTable[j][i] * z[j][i];
 921                         }
 922                 }
 923         }
 924
 925         /*
 926          *  NearestPointOnCurve :
 927          *      Compute the parameter value of the point on a Bezier
 928          *              curve segment closest to some arbtitrary, user-input point.
 929          *              Return the point on the curve at that parameter value.
 930          *
 931          *    value_type&       P;                      The user-supplied point
 932          *    value_type        *VT;            Control points of cubic Bezier
 933          */
 934         static time_type NearestPointOnCurve(const value_type& P, value_type VT[4])
 935         {
 936                 value_type      w[W_DEGREE+1];                  /* Ctl pts of 5th-degree curve  */
 937                 time_type       t_candidate[W_DEGREE];  /* Possible roots                                */
 938                 int             n_solutions;                    /* Number of roots found                 */
 939                 time_type       t;                                              /* Parameter value of closest pt */
 940
 941                 /*  Convert problem to 5th-degree Bezier form */
 942                 ConvertToBezierForm(P, VT, w);
 943
 944                 /* Find all possible roots of 5th-degree equation */
 945                 n_solutions = FindRoots(w, t_candidate, 0);
 946
 947                 /* Compare distances of P to all candidates, and to t=0, and t=1 */
 948                 {
 949                         distance_type   dist, new_dist;
 950                         value_type              p, v;
 951                         int                             i;
 952
 953                         /* Check distance to beginning of curve, where t = 0    */
 954                         dist = (P - VT[0]).mag_squared();
 955                         t = 0.0;
 956
 957                         /* Find distances for candidate points  */
 958                         for (i = 0; i < n_solutions; i++)
 959                         {
 960                                 p = Bezier(VT, DEGREE, t_candidate[i], (value_type *)NULL, (value_type *)NULL);
 961                                 new_dist = (P - p).mag_squared();
 962                                 if (new_dist < dist)
 963                                 {
 964                                         dist = new_dist;
 965                                         t = t_candidate[i];
 966                                 }
 967                         }
 968
 969                         /* Finally, look at distance to end point, where t = 1.0 */
 970                         new_dist = (P - VT[DEGREE]).mag_squared();
 971                         if (new_dist < dist)
 972                         {
 973                                 dist = new_dist;
 974                                 t = 1.0;
 975                         }
 976                 }
 977
 978                 /*  Return the point on the curve at parameter value t */
 979                 return t;
 980         }
 981 };
 982
 983 _ETL_END_NAMESPACE
 984
 985 /* === E X T E R N S ======================================================= */
 986
 987 /* === E N D =============================================================== */
 988
 989 #endif