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