OSGBezierCurve3D.cpp

00001 /*---------------------------------------------------------------------------*\
00002  *                           OpenSG NURBS Library                            *
00003  *                                                                           *
00004  *                                                                           *
00005  * Copyright (C) 2001-2006 by the University of Bonn, Computer Graphics Group*
00006  *                                                                           *
00007  *                         http://cg.cs.uni-bonn.de/                         *
00008  *                                                                           *
00009  * contact: edhellon@cs.uni-bonn.de, guthe@cs.uni-bonn.de, rk@cs.uni-bonn.de *
00010  *                                                                           *
00011 \*---------------------------------------------------------------------------*/
00012 /*---------------------------------------------------------------------------*\
00013  *                                License                                    *
00014  *                                                                           *
00015  * This library is free software; you can redistribute it and/or modify it   *
00016  * under the terms of the GNU Library General Public License as published    *
00017  * by the Free Software Foundation, version 2.                               *
00018  *                                                                           *
00019  * This library is distributed in the hope that it will be useful, but       *
00020  * WITHOUT ANY WARRANTY; without even the implied warranty of                *
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU         *
00022  * Library General Public License for more details.                          *
00023  *                                                                           *
00024  * You should have received a copy of the GNU Library General Public         *
00025  * License along with this library; if not, write to the Free Software       *
00026  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
00027  *                                                                           *
00028 \*---------------------------------------------------------------------------*/
00029 /*---------------------------------------------------------------------------*\
00030  *                                Changes                                    *
00031  *                                                                           *
00032  *                                                                           *
00033  *                                                                           *
00034  *                                                                           *
00035  *                                                                           *
00036  *                                                                           *
00037 \*---------------------------------------------------------------------------*/
00038 #ifdef WIN32
00039 #   pragma warning (disable : 985)
00040 #endif
00041 #include "OSGBezierCurve3D.h"
00042 #include "OSGBaseTypes.h"
00043
00044 OSG_USING_NAMESPACE
00045
00046
00047 #ifdef _DEBUG
00048  #ifdef WIN32
00049   #undef THIS_FILE
00050 static char THIS_FILE[] = __FILE__;
00051  #endif
00052 #endif
00053 
00054
00055 //setup functions
00056 int BezierCurve3D::setControlPointVector(const DCTPVec4dvector& cps)
00057 {
00058     if(cps.size() < 2)
00059         return -1;                 //invalid dimension, at least two points are required
00060     control_points = cps;
00061     return 0;
00062 }
00063
00064
00065 //some REAL functionality
00067 
00077 Vec3d BezierCurve3D::computewdeCasteljau(double t, int &error)
00078 {
00079     Vec4d rat_res = computewdeCasteljau4D(t, error);
00080     Vec3d res;
00081
00082     res[0] = rat_res[0] / rat_res[3];
00083     res[1] = rat_res[1] / rat_res[3];
00084     res[2] = rat_res[2] / rat_res[3];
00085
00086     return res;
00087 }
00088
00089
00090 Vec4d BezierCurve3D::computewdeCasteljau4D(double t, int &error)
00091 {
00092     //FIXME: verification before goin' into computation!!
00093 //  Vec3dvector Q = control_points; //local array not to destroy da other points
00094     const unsigned int n = control_points.size() - 1;
00095     Vec4d              rat_res;
00096
00097     if(n < 1)   //too few points, at least 2 needed
00098     {
00099         error      = -1;
00100         rat_res    = control_points[0];
00101         rat_res[0] = DCTP_EPS * floor(rat_res[0] / DCTP_EPS);
00102         rat_res[1] = DCTP_EPS * floor(rat_res[1] / DCTP_EPS);
00103         rat_res[2] = DCTP_EPS * floor(rat_res[2] / DCTP_EPS);
00104         rat_res[3] = DCTP_EPS * floor(rat_res[3] / DCTP_EPS);
00105         return rat_res;
00106     }
00107
00108     unsigned int       i, k;
00109     Vec4d             *Q  = new Vec4d[n];
00110     const double       t2 = 1.0 - t;
00111     const unsigned int n1 = n - 1;
00112
00113     for(i = 0; i < n; ++i)
00114     {
00115         const unsigned int i1 = i + 1;
00116         Q[i] = control_points[i] * t2 + control_points[i1] * t;
00117     }
00118
00119     for(k = 0; k < n1; ++k)
00120     {
00121         const unsigned int nk = n1 - k;
00122
00123         for(i = 0; i < nk; ++i)
00124         {
00125             const unsigned int i1 = i + 1;
00126             Q[i] *= t2;
00127             Q[i] += Q[i1] * t;
00128         }
00129     }
00130
00131     rat_res = Q[0];
00132
00133     delete[]  Q;
00134     return rat_res;
00135
00136 }
00137
00139
00148 Vec3d BezierCurve3D::computeLinearApproximation(double t, int &error)
00149 {
00150     //FIXME: verification before goin' into computation!!
00151     DCTPVec4dvector::size_type n = control_points.size() - 1;
00152     Vec3d                      result(0.0, 0.0, 0.0);
00153     Vec4d                      rat_res;
00154
00155     if(n < 1)
00156     { //too few points, at least 2 needed
00157         error = -1;
00158         return result;
00159     }
00160     rat_res   = control_points[0] * t + control_points[n] * (1 - t);
00161     result[0] = rat_res[0] / rat_res[3];
00162     result[1] = rat_res[1] / rat_res[3];
00163     result[2] = rat_res[2] / rat_res[3];
00164
00165     return (result);
00166 }
00167
00169
00179 int BezierCurve3D::midPointSubDivision(bezier3dvector &newbeziers)
00180 {
00181     // This function is time critical so optimize at the cost of readabiltity...
00182     DCTPVec4dvector::size_type n = control_points.size();
00183
00184     if(n < 2)    //too few points, at least 2 needed to split curve
00185     {
00186         return -1;
00187     }
00188
00189     newbeziers.resize(2);   // we return exactly two curves
00190     DCTPVec4dvector::size_type i, k;
00191
00192     DCTPVec4dvector &cp1 = newbeziers[0].control_points;
00193     DCTPVec4dvector &cp2 = newbeziers[1].control_points;
00194     cp1.clear();    // very important for performance (no copying around of obsolte stuff!)
00195     cp2.clear();    // very important for performance (no copying around of obsolte stuff!)
00196     cp1.resize(n);
00197     cp2.resize(n);
00198
00199     for(k = 0; k < n; ++k)
00200     {
00201         cp1[k] = control_points[k];
00202     }
00203
00204     --n;
00205
00206     for(k = 0; k < n; ++k)
00207     {
00208         cp2[n - k] = cp1[n];
00209
00210         for(i = n; i > k; --i)
00211         {
00212             cp1[i] += cp1[i - 1];
00213             cp1[i] *= 0.5;
00214         }
00215     }
00216
00217     cp2[0] = cp1[n];
00218 //  newbeziers[ 0 ].setControlPointVector( cp1 );
00219 //  newbeziers[ 1 ].setControlPointVector( cp2 );
00220     return 0;
00221 }
00222
00224
00234 int BezierCurve3D::midPointSubDivision(BezierCurve3D &newcurve)
00235 {
00236     // This function is time critical so optimize at the cost of readabiltity...
00237     DCTPVec4dvector::size_type n = control_points.size();
00238
00239     if(n < 2)
00240     {
00241         return -1;  //too few points, at least 2 needed to split curve
00242     }
00243
00244     DCTPVec4dvector::size_type i, k;
00245
00246     DCTPVec4dvector &cp1 = control_points;
00247     DCTPVec4dvector &cp2 = newcurve.control_points;
00248
00249     cp2.clear();    // very important for performance (no copying around of obsolte stuff!)
00250     cp2.resize(n);
00251
00252     --n;
00253
00254     for(k = 0; k < n; ++k)
00255     {
00256         cp2[n - k] = cp1[n];
00257
00258         for(i = n; i > k; --i)
00259         {
00260             cp1[i] += cp1[i - 1];
00261             cp1[i] *= 0.5;
00262         }
00263     }
00264
00265     cp2[0] = cp1[n];
00266
00267     return 0;
00268 }
00269
00271
00282 int BezierCurve3D::subDivision(double t, bezier3dvector &newbeziers)
00283 {
00284     if(t <= 0.0 || t >= 1.0)
00285         return -1;                         // t must be between (0, 1) exclusive
00286
00287     newbeziers.resize(2);   // we return exactly two curves
00288     DCTPVec4dvector            Q = control_points; //local array not to destroy da other points
00289     DCTPVec4dvector::size_type n = control_points.size() - 1;
00290     DCTPVec4dvector            cp1(n + 1);
00291     DCTPVec4dvector            cp2(n + 1);
00292     if(n < 1)    //too few points, at least 2 needed to split curve
00293     {
00294         return -1;
00295     }
00296
00297     for(DCTPVec4dvector::size_type k = 0; k < n; ++k)
00298     {
00299         cp1[k]     = Q [0];
00300         cp2[n - k] = Q [n - k];
00301
00302         for(DCTPVec4dvector::size_type i = 0; i < n - k; ++i)
00303         {
00304             Q[i] = Q[i] * (1.0 - t) + Q[i + 1] * t;
00305         }
00306     }
00307
00308     cp1[n] = Q[0];
00309     cp2[0] = Q[0];
00310     newbeziers[0].setControlPointVector(cp1);
00311     newbeziers[1].setControlPointVector(cp2);
00312     return 0;
00313 }
00314
00316
00327 int BezierCurve3D::subDivision(double t, BezierCurve3D &newcurve)
00328 {
00329     if(t <= 0.0 || t >= 1.0)
00330     {
00331         return -1; // t must be between (0, 1) exclusive
00332     }
00333
00334     // This function is time critical so optimize at the cost of readabiltity...
00335     DCTPVec4dvector::size_type n = control_points.size();
00336
00337     if(n < 2)
00338     {
00339         return -1;  //too few points, at least 2 needed to split curve
00340     }
00341
00342     double t2 = 1.0 - t;
00343
00344     DCTPVec4dvector::size_type i, k;
00345
00346     DCTPVec4dvector &cp1 = control_points;
00347     DCTPVec4dvector &cp2 = newcurve.control_points;
00348
00349     cp2.clear();    // very imporatant for performance (no copying around of obsolte stuff!)
00350     cp2.resize(n);
00351
00352     --n;
00353
00354     for(k = 0; k < n; ++k)
00355     {
00356         cp2[n - k] = cp1[n];
00357
00358         for(i = n; i > k; --i)
00359         {
00360             cp1[i] *= t;
00361             cp1[i] += cp1[i - 1] * t2;
00362         }
00363     }
00364
00365     cp2[0] = cp1[n];
00366
00367     return 0;
00368 }
00369
00371
00380 int BezierCurve3D::approximate(std::vector<double> &vertices, double delta, unsigned char strategy)
00381 {
00382     vertices.resize(0);
00383
00384     // check for first degree Bezier.
00385     if(control_points.size() == 2)
00386     {
00387         vertices.push_back(0.0);
00388         vertices.push_back(1.0);
00389         return 0;
00390     }
00391
00392     // call recursive subdividing function
00393     return approximate_sub(vertices, delta, 0.0, 1.0, strategy);
00394 }
00395
00396 int BezierCurve3D::approximate_sub(std::vector<double> &vertices, double delta, double min, double max, unsigned char strategy)
00397 {
00398     double              max_error = 0;
00399     Vec3d               ae;
00400     double              aenorm;
00401     double              t1, t2;
00402     int                 worst_i = 0;
00403     Vec3d               n0_norm;
00404     DCTPVec3dvector     eucl;
00405     DCTPVec4dvector     mycps;
00406     int                 i;
00407     std::vector<double> t;
00408
00409     int n = control_points.size() - 1;
00410
00411     for(i = 0; i <= n; i++)
00412     {
00413         t.push_back((double( n - i ) ) / n);
00414     }
00415
00416     // first, make a copy of the cp vector, and do one
00417     // or more de Casteljau steps to get rid of 0 weights
00418     mycps = control_points;
00419
00420     for(i = 0; i <= n; ++i)
00421     {
00422         unsigned int s  = mycps.size();
00423         bool         ok = true;
00424
00425         mycps.push_back(mycps[s - 1]);
00426         t.push_back(t[s - 1]);
00427
00428         for(int j = s; j > i; --j)
00429         {
00430             mycps[j] = (mycps[j] + mycps[j - 1]) * 0.5f;
00431             t[j]     = (t[j] + t[j - 1]) * 0.5f;
00432             if(mycps[j][2] < DCTP_EPS)
00433                 ok = false;
00434         }
00435
00436         if(ok)
00437             break;
00438     }
00439
00440     n = mycps.size() - 1;
00441
00442     eucl.resize(n + 1);
00443
00444     for(i = 0; i <= n; ++i)
00445     {
00446         eucl[i][0] = mycps[i][0] / mycps[i][3];
00447         eucl[i][1] = mycps[i][1] / mycps[i][3];
00448         eucl[i][2] = mycps[i][2] / mycps[i][3];
00449     }
00450
00451     if( (strategy & DISTANCE) == LINE_DISTANCE)
00452     {
00453         n0_norm = eucl[n] - eucl[0];
00454         aenorm  = n0_norm.squareLength();
00455         if(aenorm > 1e-300)
00456         {
00457             n0_norm *= 1.0 / aenorm;
00458         }
00459         else
00460         {
00461             n0_norm = Vec3d(0.0, 0.0, 0.0);
00462         }
00463     }
00464
00465     for(i = 0; i <= n; i++)
00466     {
00467         if( (strategy & DISTANCE) == POINT_DISTANCE)
00468         {
00469             t2 = 1.0 - t[i];
00470         }
00471         else
00472         {
00473             t2 = (eucl[i] - eucl[0]).dot(n0_norm);
00474             if(t2 < 0.0)
00475             {
00476                 t2 = 0.0;
00477             }
00478             else if(t2 > 1.0)
00479             {
00480                 t2 = 1.0;
00481             }
00482         }
00483         t1 = 1.0 - t2;
00484         ae = eucl[i] - eucl[0] * t1 - eucl[n] * t2;
00485
00486 //    std::cerr << "i: " << i << " ae.x: " << ae[0] << " ae.y: " << ae[1] << std::endl;
00487
00488         aenorm = sqrt(ae.squareLength() );
00489         if(aenorm > max_error)
00490         {
00491             max_error = aenorm;
00492             worst_i   = i;
00493         }
00494     }
00495
00496 //  std::cerr.precision( DCTP_PRECISION );
00497 //  std::cerr << " twopower: " << twopower << std::endl;
00498 //  std::cerr << " act_error: " << act_error << " max_error: " << max_error << std::endl;
00499 //  std::cerr << control_points[ 0 ][0] << " " << control_points[ 0 ][1] << std::endl;
00500 //  std::cerr << control_points[ control_points.size() - 1 ][0] << " " << control_points[ control_points.size() - 1 ][1] << std::endl;
00501     if(max_error > delta)
00502     {
00503         // we have to subdivide further
00504         BezierCurve3D newbez;
00505         int           error;
00506         double        mid;
00507         if( (strategy & SUBDIVISION) == MIDPOINT_SUBDIVISION)
00508         {
00509             error = midPointSubDivision(newbez);
00510             if(error)
00511                 return error;
00512             mid = (min + max) * 0.5;
00513         }
00514         else
00515         {
00516             mid   = (double(worst_i) ) / n;
00517             error = subDivision(mid, newbez);
00518             if(error)
00519                 return error;
00520             mid = min + (max - min) * mid;
00521         }
00522         approximate_sub(vertices, delta, min, mid, strategy);
00523         newbez.approximate_sub(vertices, delta, mid, max, strategy);
00524     }
00525     else
00526     {
00527         // this is a good enough approximation
00528         if(!vertices.size() )
00529         {
00530             // this is the first approximation, we have to record both the
00531             // startpoint and the endpoint
00532             vertices.push_back(min);
00533             vertices.push_back(max);
00534         }
00535         else
00536         {
00537             // we had some subdivisions before, we only need to record the endpoint
00538             vertices.push_back(max);
00539         }
00540     }
00541     return 0;
00542 }
00543
00544 // generate a bezier curve through these points
00545 int BezierCurve3D::createCurve(DCTPVec4dvector &points)
00546 {
00547     int n = points.size() - 1;
00548
00549     if(n < 1)
00550     {
00551         return -1;  // too few points
00552     }
00553
00554     control_points.resize(n + 1);
00555
00556     control_points[0] = points[0];
00557     control_points[n] = points[n];
00558
00559     if(n == 1)
00560     {
00561         return 0;   // linear curve, so we are done...
00562     }
00563
00564 /*  if( m_svvvdCreateMatrix.size( ) == 0 )
00565     {
00566         if( loadCreateMatrices( ) != 0 )
00567         {
00568             return -3;  // matrix file not found
00569         }
00570     }
00571 
00572     if( n - 1 > m_svvvdCreateMatrix.size( ) )
00573     {
00574         return -2;  // degree too high (sorry...)
00575     }*/
00576
00577     int i;
00578 //    int j;
00579 //  std::vector< std::vector< double > >    &mat = m_svvvdCreateMatrix[ n - 2 ];
00580
00581     for(i = 1; i < n; ++i)
00582     {
00583 /*      std::vector< double >   &row = mat[ i ];
00584         control_points[ i ][0] = control_points[ i ][1] = control_points[ i ][2] = 0.0;
00585         for( j = 0; j <= n; ++j )
00586         {
00587             control_points[ i ] += ( points[ j ] - points[ n >> 1 ] ) * row[ j ];
00588 //          std::cerr << row[ j ] << " ";
00589         }
00590         control_points[ i ] += points[ n >> 1 ];
00591 //      std::cerr << std::endl;*/
00592         control_points[i] = points[i];
00593     }
00594
00595     // try to approximate it better
00596     double maxerr = 1.0;
00597
00598 //  std::cerr << "n = " << n;
00599
00600     unsigned int ui_exit = 0;
00601     while(maxerr > 1e-8)
00602     {
00603         std::vector<Vec4d> vcl_err(n + 1);
00604
00605         maxerr = 0.0;
00606
00607         for(i = 0; i <= n; ++i)
00608         {
00609             int err = 0;
00610             vcl_err[i] = points[i] - computewdeCasteljau4D( (double(i) ) / n, err);
00611
00612             double acterr = (vcl_err[i]).squareLength();
00613
00614             maxerr = osgMax(maxerr, acterr);
00615         }
00616
00617         for(i = 1; i < n; ++i)
00618         {
00619 //          std::vector< double >   &row = mat[ i ];
00620             control_points[i] += vcl_err[i];
00621         }
00622
00623         if( (++ui_exit) > 10000)
00624         {
00625 //          for( i = 0; i <= n; ++i )
00626 //          {
00627 // FIXME: operator<< depracated
00628 //              std::cerr << points[ i ] << std::endl;
00629 //          }
00630 //          std::cerr << std::endl;
00631 //          std::cerr << "approxerr: " << sqrt( maxerr ) << std::endl;
00632
00633             return -2;
00634         }
00635     }
00636 //  std::cerr << ", approxerr: " << sqrt( maxerr ) << std::endl;
00637     return 0;   // everything ok.
00638 }
00639
00640
00642
00652 bool BezierCurve3D::reduceDegree(double tol)
00653 {
00654     UInt32 n = control_points.size() - 1;      // orig cps: 0, ..., n
00655     if(n < 2)
00656     {
00657         // cannot degree reduce a first degree curve
00658         return false;
00659     }
00660     DCTPVec4dvector b_left(n);
00661     DCTPVec4dvector b_right(n);
00662     UInt32          i;
00663
00664     // calculate b_right:
00665     b_right[0] = control_points[0];
00666
00667     for(i = 1; i < n; ++i)
00668     {
00669         b_right[i] = (control_points[i] * n - b_right[i - 1] * i) *
00670                      (1.0 / Real64(n - i));
00671     }
00672
00673     // calculate b_left:
00674     b_left[n - 1] = control_points[n];
00675
00676     for(i = n - 1; i > 0; --i)
00677     {
00678         b_left[i - 1] = (control_points[i] * n - b_left[i] * (n - i)) *
00679                         (1.0 / Real64(i));
00680     }
00681
00682     // check for introduced error:
00683     Real64 quad_error = tol * tol;
00684     double dist4d;
00685
00686     unsigned int n_half  = n >> 1;
00687     unsigned int n_half1 = (n + 1) >> 1;
00688
00689 //  for (i = 0; i < n_half; ++i)
00690 //    {
00691 //        control_points[ i ] = b_right[ i ];
00692 //    }
00693     for(i = n_half1; i < n; ++i)
00694     {
00695         b_right[i] = b_left[i];
00696     }
00697
00698     if(n_half != n_half1)
00699     {
00700         dist4d          = b_right[n_half].dist2(b_left[n_half]);
00701         b_right[n_half] = b_left[n_half] * 0.5 + b_right[n_half] * 0.5;
00702     }
00703     else
00704     {
00705         dist4d = control_points[n_half].dist2(0.5 * (b_right[n_half - 1] + b_left[n_half]));
00706     }
00707
00708
00709
00710     //Vec3d dist;
00711     double minweight = b_right[0][3];
00712
00713     for(i = 1; i < n; ++i)
00714     {
00715         if(minweight > b_right[i][3])
00716             minweight = b_right[i][3];
00717     }
00718
00719     if(minweight < DCTP_EPS)
00720     {
00721         // can't degree reduce a curve with zero weight(s)...
00722         return false;
00723     }
00724
00725     if(dist4d / (minweight * minweight) > quad_error)
00726     {
00727         return false;
00728     }
00729
00730     control_points = b_right;
00731 //    setControlPointVector( b_new );
00732
00733     return true;
00734 }