Source/System/NodeCores/Drawables/Nurbs/Internal/OSGBSplineBasisFunction.cpp

   1 /*---------------------------------------------------------------------------*\
   2  *                           OpenSG NURBS Library                            *
   3  *                                                                           *
   4  *                                                                           *
   5  * Copyright (C) 2001-2006 by the University of Bonn, Computer Graphics Group*
   6  *                                                                           *
   7  *                         http://cg.cs.uni-bonn.de/                         *
   8  *                                                                           *
   9  * contact: edhellon@cs.uni-bonn.de, guthe@cs.uni-bonn.de, rk@cs.uni-bonn.de *
  10  *                                                                           *
  11 \*---------------------------------------------------------------------------*/
  12 /*---------------------------------------------------------------------------*\
  13  *                                License                                    *
  14  *                                                                           *
  15  * This library is free software; you can redistribute it and/or modify it   *
  16  * under the terms of the GNU Library General Public License as published    *
  17  * by the Free Software Foundation, version 2.                               *
  18  *                                                                           *
  19  * This library is distributed in the hope that it will be useful, but       *
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of                *
  21  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU         *
  22  * Library General Public License for more details.                          *
  23  *                                                                           *
  24  * You should have received a copy of the GNU Library General Public         *
  25  * License along with this library; if not, write to the Free Software       *
  26  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
  27  *                                                                           *
  28 \*---------------------------------------------------------------------------*/
  29 /*---------------------------------------------------------------------------*\
  30  *                                Changes                                    *
  31  *                                                                           *
  32  *                                                                           *
  33  *                                                                           *
  34  *                                                                           *
  35  *                                                                           *
  36  *                                                                           *
  37 \*---------------------------------------------------------------------------*/
  38 #ifdef WIN32
  39 #   pragma warning (disable : 985)
  40 #endif
  41 #include "OSGBSplineBasisFunction.h"
  42
  43
  44 OSG_USING_NAMESPACE
  45
  46 #ifdef _DEBUG
  47  #ifdef WIN32
  48   #undef THIS_FILE
  49 static char THIS_FILE[] = __FILE__;
  50  #endif
  51 #endif
  52
  53 const char BSplineBasisFunction::ff_const_1[] = "BEGINBSPLINEBASISFUNCTION";
  54 const char BSplineBasisFunction::ff_const_2[] = "NUMBEROFKNOTS";
  55 const char BSplineBasisFunction::ff_const_3[] = "KNOTS";
  56
  57
  58 //setup methods
  59 int BSplineBasisFunction::setKnotVector(const DCTPdvector &k)
  60 {
  61     int err;
  62     if(k.size() < 2)
  63         return -1;               //one knot doesn't make any knotspan
  64     // Commented out because of iv2tso,
  65     // bailed out when encountering degenerate curves (empty knotspan)
  66     // found in some files
  67
  68     err = CheckKnotPoints(k);
  69     if(err)
  70     {
  71 //       std::cerr<<"setKnotVector: err: " << err << std::endl;
  72 //       std::cerr<<"beginknotvec: " << k[0] << std::endl;
  73 //       std::cerr<<"endknotvec: " << k[k.size()-1] << std::endl;
  74 //       std::cerr<<"lengthofknotvec: " << k.size() << std::endl;
  75         return -2; //bad knot series
  76     }
  77     knots = k;
  78     return 0;
  79 }
  80
  81 //query methods
  82 // inserts a knot. Returns the (original) span index of the new knot inserted.
  83 // or -1 on error.
  84 int BSplineBasisFunction::insertKnot(double k)
  85 {
  86     if(k < knots[0] || k > knots[knots.size() - 1])
  87         return -1;
  88     int                    span = findSpan(k);
  89     DCTPdvector::size_type i;
  90     i = 0;
  91     while(knots[i] < k && i < knots.size())
  92         i++;
  93     knots.insert(knots.begin() + i, k);    // FIXME: what if it doesn't fit??? exception?
  94     return span; // FIXME: maybe it would be more efficient to calculate it from i?
  95 }
  96
  97 void BSplineBasisFunction::getParameterInterval(double &minpar, double &maxpar)
  98 {
  99     if(knots.size() < 1)
 100     {
 101         minpar = 1.0; maxpar = 0.0; return;
 102     }
 103     else
 104     {
 105         minpar = knots[0]; maxpar = knots[knots.size() - 1]; return;
 106     }
 107 }
 108
 109 //I/O support - FIXME: read( char *fname ) outta be supported , etc
 110 int BSplineBasisFunction::read(std::istream &infile)
 111 {
 112     //FIXME: maybe we need more checks!!!
 113     char txtbuffer[256];
 114
 115     infile.getline(txtbuffer, 255); //read line
 116
 117     if(strcmp(txtbuffer, ff_const_1) )
 118         return -1;                                  //bad file format
 119
 120     infile >> txtbuffer; //FIXME: error prone: too long string causes problem!!!
 121
 122     if(strcmp(txtbuffer, ff_const_2) )
 123         return -1;                                   //yeah, bad file format again
 124
 125     DCTPdvector::size_type num_of_knots;
 126     infile >> num_of_knots;
 127     if(num_of_knots < 2)
 128         return -2;                   //ah, bad number of knots
 129     knots.resize(num_of_knots); //FIXME: whatif noe enoght memory?
 130
 131     infile >> std::ws >> txtbuffer; //skip whitespaces and read string
 132     if(strcmp(txtbuffer, ff_const_3) )
 133         return -1;
 134 //  std::cerr<<"numofknots: " << num_of_knots << std::endl;
 135
 136     for(DCTPdvector::size_type i = 0; i < num_of_knots; ++i)
 137     {
 138         infile >> knots[i];   //FIXME: ya see, we need ERROR CHECKS!!!
 139 //        std::cerr << knots[ i ] << " ";
 140     }
 141
 142     int ckp = CheckKnotPoints(knots);
 143     if(ckp == -1)
 144         return -3;           // empty knot series
 145     else if(ckp)
 146         return -4;          // wrong knot series
 147
 148     return 0;
 149
 150 }
 151
 152 int BSplineBasisFunction::write(std::ostream &outfile)
 153 {
 154
 155     //FIXME: ya outta do some f***n' ERRORTESTIN'!!!!
 156     outfile.precision(DCTP_PRECISION);
 157     outfile << ff_const_1 << std::endl;
 158
 159     DCTPdvector::size_type num_of_knots = knots.size();
 160     outfile << ff_const_2 << " " << num_of_knots << std::endl << ff_const_3 << " ";
 161
 162     for(DCTPdvector::size_type i = 0; i < num_of_knots; ++i)
 163         outfile << knots[i] << " ";
 164
 165     outfile << std::endl;
 166     return 0;
 167 }
 168
 169 //some REAL functionality
 170 double BSplineBasisFunction::compute(double u, int i, int p)
 171 {
 172 //u - parameter value, knots[0] <= u <= knots[ m ]
 173 //i,p - the ith p-dimensional basis function should be computed
 174     int j;
 175
 176     if(knots.size() < 1)
 177         return -1.0;                   //the knots are not set up
 178     DCTPdvector::size_type m = knots.size() - 1; //m: number of knots
 179     if(u < knots[0] || u > knots[m])
 180         return -2.0;                                   //invalid u
 181     if(p >= int(m))
 182         return -3.0;         //this high dimension is not defined
 183     if(i < 0 || i >= int(m) - p)
 184         return -4.0;                      //i is invalid
 185
 186     if( (i == 0 && u == knots[0]) ||
 187         (i == int(m) - p - 1 && u == knots[m]) )
 188         return 1.0;                                        //special cases
 189     if(u < knots[i] || u >= knots[i + p + 1])
 190         return 0.0;                                            //local support property
 191
 192     DCTPdvector n(p + 1);
 193
 194     for(j = 0; j < p + 1; ++j) //setup 0th degree basis functionsi
 195         if(knots[i + j] <= u && u < knots[i + j + 1])
 196             n[j] = 1.0;
 197         else
 198             n[j] = 0.0;
 199
 200     for(j = 1; j < p + 1; ++j)
 201     {
 202         for(DCTPdvector::size_type k = 0; int(k) < p + 1 - j; ++k)
 203         {
 204             double den = knots[k + i + j] - knots[k + i];
 205             double left;
 206             if(den < DCTP_EPS) //don't divide by 0!
 207                 left = 0.0;
 208             else
 209                 left = (u - knots[k + i]) / den * n[k];
 210             den = knots[k + i + j + 1] - knots[k + i + 1];
 211             double right;
 212             if(den < DCTP_EPS) //don't divide by 0!
 213                 right = 0.0;
 214             else
 215                 right = (knots[k + i + j + 1] - u) / den * n[k + 1];
 216             n[k] = left + right;
 217         }
 218     }
 219
 220     return n[0];
 221 }
 222
 223 int BSplineBasisFunction::computeDersBasisFuns(double dU, int iP, double **&rppdDers, int iDepth)
 224 {
 225     int      i_span;
 226     double **ppd_ndu;
 227     int      i_j;
 228     double  *pd_left;
 229     double  *pd_right;
 230     double   d_saved;
 231     int      i_r;
 232     double   d_temp;
 233     int      i_s1;
 234     int      i_s2;
 235     double **ppd_a;
 236     int      i_k;
 237     int      i_rk;
 238     int      i_pk;
 239     int      i_j1;
 240     int      i_j2;
 241
 242     if(knots.size() < 1)
 243     {
 244         return -1; //the knots are not set up
 245     }
 246     i_span = findSpan(dU);
 247     if(i_span < 0)
 248     {
 249         return -2; //invalid u
 250     }
 251     if(i_span - iP < 0)
 252     {
 253         return -3; //invalid p
 254     }
 255     if(iP < 0)
 256     {
 257         return -4; // negative p
 258     }
 259
 260     ppd_ndu  = new double*[iP + 1];
 261     pd_left  = new double[iP + 1];
 262     pd_right = new double[iP + 1];
 263     ppd_a    = new double*[2];
 264
 265 //      vvd_ndu.resize( iP + 1 );
 266     for(i_j = 0; i_j <= iP; ++i_j)
 267     {
 268         ppd_ndu[i_j] = new double[iP + 1];
 269 //              vvd_ndu[ i_j ].resize( iP + 1 );
 270     }
 271
 272 //      vd_left.resize( iP + 1 );
 273 //      vd_right.resize( iP + 1 );
 274     rppdDers = new double*[iDepth + 1];
 275
 276 //      rvvdDers.resize( iDepth + 1 );
 277     for(i_j = 0; i_j < iDepth + 1; ++i_j)
 278     {
 279         rppdDers[i_j] = new double[iP + 1];
 280 //              rvvdDers[ i_j ].resize( iP + 1 );
 281     }
 282
 283 //      vvd_a.resize( 2 );
 284     ppd_a[0] = new double[iP + 1];
 285 //      vvd_a[ 0 ].resize( iP + 1 );
 286     ppd_a[1] = new double[iP + 1];
 287 //      vvd_a[ 1 ].resize( iP + 1 );
 288
 289     ppd_ndu[0][0] = 1.0;
 290
 291     for(i_j = 1; i_j <= iP; ++i_j)
 292     {
 293         const int ci_j1    = i_j - 1;
 294         double   *pd_ndujr = ppd_ndu[i_j];
 295
 296         pd_left[i_j]  = dU - knots[i_span - ci_j1];
 297         pd_right[i_j] = knots[i_span + i_j] - dU;
 298         d_saved       = 0.0;
 299
 300         for(i_r = 0; i_r < i_j; ++i_r)
 301         {
 302             const int ci_r1 = i_r + 1;
 303             const int ci_jr = i_j - i_r;
 304
 305             (*pd_ndujr)       = pd_right[ci_r1] + pd_left[ci_jr];
 306             d_temp            = ppd_ndu[i_r][ci_j1] / (*pd_ndujr);
 307             ppd_ndu[i_r][i_j] = d_saved + pd_right[ci_r1] * d_temp;
 308             d_saved           = pd_left[ci_jr] * d_temp;
 309             ++pd_ndujr;
 310         }
 311
 312         ppd_ndu[i_j][i_j] = d_saved;
 313     }
 314
 315     for(i_j = 0; i_j <= iP; ++i_j)
 316     {
 317         rppdDers[0][i_j] = ppd_ndu[i_j][iP];
 318     }
 319
 320     for(i_r = 0; i_r <= iP; ++i_r)
 321     {
 322         i_s1        = 0;
 323         i_s2        = 1;
 324         ppd_a[0][0] = 1.0;
 325
 326         for(i_k = 1; i_k <= iDepth; ++i_k)
 327         {
 328             d_temp = 0.0;
 329             i_rk   = i_r - i_k;
 330             i_pk   = iP - i_k;
 331             if(i_r >= i_k)
 332             {
 333                 d_temp         = ppd_a[i_s1][0] / ppd_ndu[i_pk + 1][i_rk];
 334                 ppd_a[i_s2][0] = d_temp;
 335                 d_temp        *= ppd_ndu[i_rk][i_pk];
 336             }
 337             if(i_rk >= -1)
 338             {
 339                 i_j1 = 1;
 340             }
 341             else
 342             {
 343                 i_j1 = -i_rk;
 344             }
 345             if(i_r - 1 <= i_pk)
 346             {
 347                 i_j2 = i_k - 1;
 348             }
 349             else
 350             {
 351                 i_j2 = iP - i_r;
 352             }
 353
 354             for(i_j = i_j1; i_j <= i_j2; ++i_j)
 355             {
 356                 ppd_a[i_s2][i_j] = (ppd_a[i_s1][i_j] - ppd_a[i_s1][i_j - 1])
 357                                    / ppd_ndu[i_pk + 1][i_rk + i_j];
 358                 d_temp += ppd_a[i_s2][i_j] * ppd_ndu[i_rk + i_j][i_pk];
 359             }
 360
 361             if(i_r <= i_pk)
 362             {
 363                 ppd_a[i_s2][i_k] = -ppd_a[i_s1][i_k - 1] / ppd_ndu[i_pk + 1][i_r];
 364                 d_temp          += ppd_a[i_s2][i_k] * ppd_ndu[i_r][i_pk];
 365             }
 366             rppdDers[i_k][i_r] = d_temp;
 367             i_j                = i_s1;
 368             i_s1               = i_s2;
 369             i_s2               = i_j;
 370         }
 371     }
 372
 373     i_r = iP;
 374
 375     for(i_k = 1; i_k <= iDepth; ++i_k)
 376     {
 377         for(i_j = 0; i_j <= iP; ++i_j)
 378         {
 379             rppdDers[i_k][i_j] *= i_r;
 380         }
 381
 382         i_r *= (iP - i_k);
 383     }
 384
 385     //clean up allocated memory
 386     for(i_j = 0; i_j <= iP; ++i_j)
 387     {
 388         delete[]  ppd_ndu[i_j];
 389     }
 390
 391     delete[]  ppd_a[0];
 392     delete[]  ppd_a[1];
 393     delete[]  ppd_ndu;
 394     delete[]  pd_left;
 395     delete[]  pd_right;
 396     delete[]  ppd_a;
 397
 398     return i_span;
 399 }
 400
 401 int BSplineBasisFunction::computeAllNonzero(double u, int p, double *&rpd_n)
 402 {
 403     if(knots.size() < 1)
 404         return -1;                   //the knots are not set up
 405     int i = findSpan(u);
 406     if(i < 0)
 407         return -2;        //invalid u
 408
 409 //FIXME
 410 //  if ( u < 0 ) return -2; //invalid u
 411     if(i - p < 0)
 412         return -3;            //invalid p
 413     rpd_n = new double[p + 1];
 414 //  n.resize( p + 1 );
 415     rpd_n[0] = 1.0;
 416     double *pd_left  = new double[p + 1];
 417     double *pd_right = new double[p + 1];
 418
 419 //  DCTPdvector left( p + 1), right( p + 1 );
 420 //  for( DCTPdvector::size_type j = 1; j <= p; ++j ) {
 421     for(int j = 1; j <= p; ++j)
 422     {
 423         pd_left[j]  = u - knots[i + 1 - j];
 424         pd_right[j] = knots[i + j] - u;
 425         double saved = 0.0;
 426
 427         for(DCTPdvector::size_type r = 0; int(r) < j; ++r)
 428         {
 429             double temp = rpd_n[r] / (pd_left[j - r] + pd_right[r + 1]);
 430             rpd_n[r] = saved + temp * pd_right[r + 1];
 431             saved    = temp * pd_left[j - r];
 432         }
 433
 434         rpd_n[j] = saved;
 435     }
 436
 437     delete[]  pd_left;
 438     delete[]  pd_right;
 439
 440     return i;
 441 }
 442
 443 int BSplineBasisFunction::CheckKnotPoints(const DCTPdvector &k)
 444 {
 445     if(osgAbs(k[0] - k[k.size() - 1]) < DCTP_EPS)
 446     {
 447         std::cerr << "BSplineBasisFunction::CheckKnotPoints: first and last knot are equal: " << k[0] << std::endl;
 448         return -1; //at least two different knots define a knotspan! FIXME: double == double ???
 449     }
 450     DCTPdvector::const_iterator i    = k.begin();
 451     double                      temp = *i; i++;
 452     while(i != k.end() )
 453     {
 454         if(temp > *i)
 455         {
 456             std::cerr << "BSplineBasisFunction::CheckKnotPoints: illegal consecutive knots: " << temp << " " << *i << std::endl;
 457             return -2;
 458         }
 459         temp = *i;
 460         i++;
 461     }
 462     return 0;
 463 }
 464
 465 int BSplineBasisFunction::findSpan(double &u)
 466 {
 467     DCTPdvector::size_type n = knots.size() - 1;
 468
 469     if(u < knots[0] || u > knots[n])
 470     {
 471         //FIXME:
 472         if(u < knots[0])
 473         {
 474             u = knots[0];
 475 //        return 0;
 476         }
 477         else
 478         {
 479             u = knots[n];
 480 //        return  n;
 481         }
 482 //    return -1; //invalid u
 483     }
 484
 485 /*    for( unsigned int ui_test = 0; ui_test <= n; ++ui_test )
 486     {
 487         std::cerr << knots[ ui_test ] << " ";
 488     }
 489     std::cerr << std::endl;*/
 490
 491     while(osgAbs(knots[n] - knots[n - 1]) < DCTP_EPS)
 492         --n;                                                        //FIXME: double ==double ???
 493     --n; //n holds the index of the last different knot before the equal ones at the of the knot vector
 494
 495     while(u < knots[n])
 496         --n;
 497     return int(n);
 498 #if 0
 499     if(osgAbs(u - knots[n + 1]) < DCTP_EPS)
 500         return n;                                            //special case: u is at the end FIXME: double ==double ???
 501     DCTPdvector::size_type low = 0;
 502     while(osgAbs(knots[low] - knots[low + 1]) < DCTP_EPS)
 503         ++low;                                                            //FIXME: double ==double ???
 504     DCTPdvector::size_type high = n + 1;
 505     //now get span with a binary search
 506     DCTPdvector::size_type mid = (low + high) / 2;
 507     while(u < knots[mid] || u >= knots[mid + 1])
 508     {
 509 //        std::cerr << knots[ low ] << " " << knots[ mid ] << " " << knots[ high ] << std::endl;
 510         if(u < knots[mid])
 511             high = mid;
 512         else
 513             low = mid;
 514         mid = (low + high) / 2;
 515     }
 516     return mid;
 517 #endif
 518 }