00001
00005 #ifdef HAVE_CONFIG_H
00006 #include <tumble-conf.h>
00007 #endif
00008
00010
00011
00012 #include <iostream>
00013 #include <cstddef>
00014
00015 #include "globals.h"
00016 #include "datastore.h"
00017 #include "spline.h"
00018
00019
00020 using namespace std;
00021
00022
00023
00024
00025
00026
00027
00028 static const double big=0.0;
00029 static const double constraint_ratio=10.0;
00030
00031
00032
00038 QBSpline::QBSpline(const vector<ControlPoint> &deboor, DataStore *_data_store, bool _closed){
00039 vector<double> newk;
00040 initialize(deboor, newk, _data_store, 1.0, _closed);
00041 }
00042
00049 QBSpline::QBSpline(const vector<ControlPoint> &deboor, const vector<double> &newk, DataStore *_data_store, bool _closed){
00050 initialize(deboor, newk, _data_store, 1.0, _closed);
00051 }
00052
00060 QBSpline::QBSpline(const vector<ControlPoint> &deboor, const vector<double> &newk, DataStore *_data_store, double _restlength, bool _closed)
00061 {
00062 initialize(deboor, newk, _data_store, _restlength, _closed);
00063 }
00064
00082 void QBSpline::initialize(const vector<ControlPoint> &deboor, const vector<double> &newk, DataStore *_data_store, double _restlength, bool _closed)
00083 {
00084 unsigned i;
00085 data_store = _data_store;
00086 restlength = _restlength;
00087 closed=_closed;
00088
00089 if( !data_store )
00090 FATAL_ERROR("QBSpline::intialize -- data_store is NULL");
00091
00092
00093 if( closed && (deboor[0] != deboor[deboor.size()-1]) )
00094 FATAL_ERROR("QBSpline::intialize -- Closed spline has two distinct endpoints");
00095
00096
00097 if( closed && deboor.size() < 5 )
00098 FATAL_ERROR("QBSpline::intialize -- Closed spline has "<<deboor.size()<<" points, but must have at least 5.");
00099
00100
00101 if( deboor.size() < 3 )
00102 FATAL_ERROR("QBSpline::intialize -- Spline has "<<deboor.size()<<" points but must have at least 3.");
00103
00104
00105
00106
00107
00108 vertexs.insert(vertexs.begin(), deboor.size()-3, NULL);
00109
00110
00111
00112
00113
00114
00115 if(newk.size() == 0){
00116 k.push_back(0.0);
00117 for(i=1; i < deboor.size()-1; i++){
00118 k.push_back( k.back() + (*deboor[i+1] - *deboor[i-1]).mag());
00119 }
00120 } else {
00121 assert(newk.size() == deboor.size() - 1);
00122 for(i=0; i < newk.size()-1; i++){
00123 if( newk[i] >= newk[i+1] ) FATAL_ERROR("QBSpline::intialize -- knot sequence is non-increasing" );
00124 }
00125 k=newk;
00126 }
00127
00128
00129
00130
00131
00132 Point2D new_point;
00133 ControlPoint temp;
00134 b.push_back(deboor[0]);
00135 b.push_back(deboor[1]);
00136 for(i=1; i < deboor.size()-2; i++){
00137 temp=deboor[i+1];
00138 new_point = *(b.back()) * (dt(i)/(dt(i-1)+dt(i)))
00139 + (*temp) * (dt(i-1)/(dt(i-1)+dt(i)));
00140 b.push_back(data_store->add_cp(new_point));
00141 b.push_back(temp);
00142 }
00143 b.push_back(deboor.back());
00144 }
00145
00147 QBSpline::QBSpline(const QBSpline &other)
00148 {
00149 copy(other.b.begin(), other.b.end(), back_inserter(b));
00150 copy(other.k.begin(), other.k.end(), back_inserter(k));
00151 copy(other.vertexs.begin(), other.vertexs.end(), back_inserter(vertexs));
00152 closed = other.closed;
00153 data_store = other.data_store;
00154 }
00155
00157 QBSpline& QBSpline::operator=(const QBSpline &other)
00158 {
00159 copy(other.b.begin(), other.b.end(), back_inserter(b));
00160 copy(other.k.begin(), other.k.end(), back_inserter(k));
00161 copy(other.vertexs.begin(), other.vertexs.end(), back_inserter(vertexs));
00162 closed = other.closed;
00163 data_store = other.data_store;
00164 return *this;
00165 }
00166
00180 ControlPoint QBSpline::get_segment_center(double u0, double u1) const
00181 {
00182 int u0_idx, u1_idx;
00183 double temp;
00184 if(u0 > u1){
00185 temp = u0;
00186 u0 = u1;
00187 u1 = temp;
00188 }
00189
00190 u0_idx = get_closest_knot(u0);
00191 u1_idx = get_closest_knot(u1);
00192
00193 assert( u1_idx - u0_idx == 1 );
00194 assert( u0_idx < (int)k.size()-1);
00195 assert( u1_idx < (int)k.size() );
00196 assert( u0_idx >= 0);
00197 assert( u1_idx >= 1);
00198 return b[u0_idx*2+1];
00199 }
00200
00211 ControlPoint QBSpline::get_deboor(int i) const
00212 {
00213 int d_size=((b.size()+1)/2) + 1;
00214
00215 if( i <= 0 ) {return b.front();}
00216 if( i >= d_size-1 ) {return b.back();}
00217 return b[2*i-1];
00218 }
00219
00228 void QBSpline::getip(double t, int *i, double *p) const
00229 {
00230 *i=-1;
00231 double den;
00232 for(unsigned j=0; j < k.size()-1 ;j++){
00233 if((k[j]<=t)&&(k[j+1]>t)){
00234 *i=j;
00235 break;
00236 }
00237 }
00238 if(k.back()==t){
00239
00240 if(closed){
00241 *i=0;
00242 *p=0.0;
00243 return;
00244 }
00245 else{
00246 *i=k.size()-2;
00247 }
00248 }
00249
00250 if(*i==-1){
00251 cout<<"QBSpline::getIP: parameter t is out of range"<<endl;
00252 *i=0;
00253 *p=0.0;
00254 return;
00255 }
00256 else{
00257 den=dt(*i);
00258 if(den!=0.0){
00259 *p=(t-k[*i])/den;
00260 }
00261 else{
00262 *p=0.0;
00263 }
00264 }
00265 }
00266
00279 int QBSpline::get_closest_knot(double u) const
00280 {
00281 double best_dist= fabs(k[0] - u);
00282 double dist;
00283 int best=0;
00284 int i;
00285 for(i=1; i < (int)k.size(); i++){
00286 dist= fabs(k[i] - u);
00287 if(dist < best_dist){
00288 best_dist=dist;
00289 best=i;
00290 }
00291 }
00292 return best;
00293 }
00294
00301 int QBSpline::get_closest_bezier(double u) const
00302 {
00303 return get_closest_knot(u) * 2 ;
00304 }
00305
00333 int QBSpline::get_edge_cps(double u0, double u1, ControlPoint &cp0, ControlPoint &cp1, ControlPoint &cp2, int &cp0_is_vert, int &cp2_is_vert) const
00334 {
00335 int cp0_idx, cp1_idx, cp2_idx;
00336 cp0_is_vert=-1;
00337 cp2_is_vert=-1;
00338
00339 cp0_idx = get_closest_bezier(u0);
00340 cp2_idx = get_closest_bezier(u1);
00341
00342 if(cp0_idx < 0 || cp2_idx < 0 ){
00343 printf("QBSpline::get_edge_cps error bad u value\n");
00344 return FALSE;
00345 }
00346 if(((cp0_idx==0) && (cp2_idx == (int)b.size()-2))||
00347 ((cp2_idx==0) && (cp2_idx == (int)b.size()-2))){
00348 cp1_idx = b.size()-1;
00349 }
00350 else if(cp2_idx-cp0_idx==2){
00351 cp1_idx = cp0_idx+1;
00352 }
00353 else if(cp0_idx-cp2_idx==2){
00354 cp1_idx = cp0_idx-1;
00355 }
00356 else{
00357 printf("QBSpline::get_edge_cps error, incorrect bezier points returned\n");
00358 return FALSE;
00359 }
00360 cp0 = b[cp0_idx];
00361 cp1 = b[cp1_idx];
00362 cp2 = b[cp2_idx];
00363
00364
00365 if(cp0_idx == 0) cp0_is_vert=0;
00366 if(cp0_idx == (int)b.size()-1) cp0_is_vert=1;
00367 if(cp2_idx == 0) cp2_is_vert=0;
00368 if(cp2_idx == (int)b.size()-1) cp2_is_vert=1;
00369 return TRUE;
00370 }
00371
00380 Point2D QBSpline::evaluate_segment(int s, double u) const
00381 {
00382 s*=2;
00383 double v=1-u;
00384 return ( (*b[s])*v*v )+( (*b[s+1])*2*u*v )+( (*b[s+2])*u*u );
00385 }
00386
00387
00395 Point2D QBSpline::evaluate(double u) const
00396 {
00397 int i;
00398 double p;
00399 getip(u,&i,&p);
00400 return evaluate_segment(i,p);
00401 }
00402
00404 void QBSpline::print() const
00405 {
00406 cout<<*this;
00407 }
00408
00421 void QBSpline::evaluate_full(double u,Point2D *t,Point2D *b1,Point2D *b2,int *i) const
00422 {
00423 double p;
00424 getip(u,i,&p);
00425 int j = *i;
00426 *b1 = (*(b[2*j]))*(1-p) + (*(b[2*j+1]))*p;
00427 *b2 = (*(b[2*j+1]))*(1-p) + (*(b[2*j+2]))*p;
00428 *t = *b1 * (1-p) + *b2 * p;
00429 }
00430
00437 void QBSpline::split(int i, double u, ControlPoint& cp0,
00438 ControlPoint& cp1, ControlPoint& cp2)
00439 {
00440 assert(i >= 0); assert((unsigned)i < k.size());
00441 assert(u >= 0.0); assert(u <= 1.0);
00442
00443 add_knot(k[i] + u*(k[i+1]-k[i]), cp0, cp1, cp2);
00444 }
00445
00467 void QBSpline::add_knot(double u,ControlPoint &cp0, ControlPoint &cp1, ControlPoint &cp2)
00468 {
00469 Point2D p0,p1,p2;
00470 int i;
00471 if(u<=0 || u > k.back())
00472 FATAL_ERROR("QBSpline::add_knot -- knot value: "<<u<<" is out of range");
00473
00474
00475 evaluate_full(u,&p1,&p0,&p2,&i);
00476
00477 cp0 = b[2*i+1];
00478 cp0.access() = p0;
00479
00480
00481 cp1 = data_store->add_cp(p1);
00482 cp2 = data_store->add_cp(p2);
00483 b.insert(b.begin()+(2*i+2),2,cp2);
00484 b[2*i+2] = cp1;
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494 k.insert(k.begin()+(i+1),u);
00495
00496
00497 vertexs.insert(vertexs.begin() + (i),NULL);
00498 }
00499
00521 bool QBSpline::remove_knot(double u, ControlPoint &cp, double &u0, double &u1){
00522 int i;
00523 double p;
00524 if(u<=0 || u >= k.back()) return false;
00525 getip(u,&i,&p);
00526 if(i<=0 || (unsigned)i == k.size()-1){
00527 cout<<"QBSpline::remove_knot: cannot remove end point"<<endl;
00528 return false;
00529 }
00530
00531
00532
00533
00534
00535 data_store->rem_cp( b[2*i] );
00536 data_store->rem_cp( b[2*i+1] );
00537 b.erase(b.begin() + (2*i), b.begin() + (2*i+2) );
00538 k.erase(k.begin()+i);
00539 vertexs.erase(vertexs.begin() + (i-1));
00540
00541
00542
00543
00544 if(i > 1){
00545 b[2*i-2].access()
00546 = *b[2*i-3] * (dt(i-1) / ( dt(i-2) + dt(i-1) ))
00547 + *b[2*i-1] * (dt(i-2) / ( dt(i-2) + dt(i-1) ));
00548 }
00549
00550
00551
00552
00553 if((unsigned)2*i < b.size() - 1 ){
00554 b[2*i].access()
00555 = *b[2*i-1] * (dt(i) / ( dt(i-1) + dt(i) ))
00556 + *b[2*i+1] * (dt(i-1) / ( dt(i-1) + dt(i) ));
00557 }
00558
00559
00560 cp = b[2*i-1];
00561
00562
00563
00564
00565
00566
00567 u0 = k[i-1];
00568 u1 = k[i];
00569 return true;
00570 }
00571
00572
00587 void QBSpline::move(Point2D *dp, Point2D *dq, unsigned xCoord, unsigned yCoord){
00588
00589 #ifdef RAW_LS
00590 move_raw(dp,dq);
00591 return;
00592
00593 #endif
00594 #ifndef RAW_LS
00595 Point2D *new_d;
00596 int i;
00597
00598
00599 if(k.size() >= 3){
00600
00601 new_d = compute_new_points(dp,dq);
00602 }
00603 else{
00604
00605 new_d = new Point2D[1];
00606 new_d[0] = dq[0];
00607 }
00608
00609
00610
00611
00612
00613 Point2D newPos;
00614
00615
00616
00617 for(i=1; i < get_num_deboor()-1; i++){
00618 const ControlPoint cp = get_deboor(i);
00619 DataPoint dp = data_store -> get_data(cp);
00620
00621
00622
00623 newPos = *b[2*i-1] + new_d[i-1];
00624 dp->set(newPos.x(), xCoord);
00625 dp->set(newPos.y(), yCoord);
00626
00627
00628 }
00629
00630
00631
00632 for(i=1; 2*i < ((int)b.size())-1; i++){
00633 const ControlPoint cp = b[2*i];
00634 DataPoint dp = data_store -> get_data(cp);
00635
00636
00637
00638 newPos = *b[2*i-1] * ( dt(i) / (dt(i-1) + dt(i)) )
00639 + *b[2*i+1] * ( dt(i-1) / (dt(i-1) + dt(i)) );
00640 dp->set(newPos.x(), xCoord);
00641 dp->set(newPos.y(), yCoord);
00642
00643
00644 }
00645
00646 delete[] new_d;
00647
00648 #endif
00649 }
00650
00651
00652
00653
00654
00655
00656
00657 #ifdef RAW_LS
00658 void QBSpline::move_raw(Point2D *dp, Point2D *dq){
00659
00660
00661
00662
00663
00664 int i;
00665
00666 for (i=1; i< get_num_deboor()-1; i++){
00667 *get_deboor(i) = *b[2*i-1] + dq[i-1]*2.0 - (dp[i-1] + dp[i])*0.5;
00668 }
00669
00670
00671 for(i=1; 2*i < ((int)b.size())-1; i++){
00672 *b[2*i] = *b[2*i-1] * ( dt(i) / (dt(i-1) + dt(i)) )
00673 + *b[2*i+1] * ( dt(i-1) / (dt(i-1) + dt(i)) );
00674 }
00675
00676 }
00677 #endif
00678
00689 Point2D* QBSpline::compute_new_points(Point2D *dp,Point2D *dq) const{
00690 int i,rows,cols;
00691 int size_dp = k.size();
00692 int size_dq = k.size() - 1;
00693
00694 cols = size_dp - 1;
00695 rows = size_dp - 2 + size_dq;
00696
00697
00698 if (closed) rows++;
00699
00700 Matrix A(rows,cols);
00701 Point2D *B = new Point2D[rows];
00702
00703
00704
00705
00706 int p_affecting;
00707 int col_affected1;
00708 int col_affected2;
00709
00710 for(i=0; i < size_dp-2; i++)
00711 {
00712 p_affecting = i+1;
00713 col_affected1 = i;
00714 col_affected2 = i+1;
00715 A.m[i][col_affected1] = ratio(p_affecting,p_affecting);
00716 A.m[i][col_affected2] = ratio(p_affecting+1,p_affecting);
00717 B[i] = dp[p_affecting] ;
00718 }
00719
00720
00721 int q_affecting;
00722 int col_affected3;
00723 double conr = constraint_ratio;
00724
00725 for(i= size_dp-2; i < size_dp- 2 + size_dq;i++)
00726 {
00727 q_affecting = i-size_dp+2;
00728 col_affected1 = q_affecting - 1;
00729 col_affected2 = q_affecting;
00730 col_affected3 = q_affecting + 1;
00731 if (q_affecting > 0)
00732 if (q_affecting < size_dq - 1)
00733 {
00734
00735 A.m[i][col_affected1] = (ratio(q_affecting, q_affecting) * 0.25) * conr;
00736 A.m[i][col_affected2] = ((ratio(q_affecting+1,q_affecting) * 0.25) + 0.5 + (ratio(q_affecting+1, q_affecting+1) * 0.25)) * conr;
00737 A.m[i][col_affected3] = (ratio(q_affecting+2,q_affecting+1) * 0.25) * conr;
00738 B[i] = dq[q_affecting] * conr;
00739 }
00740 else
00741 {
00742 A.m[i][col_affected1] = (ratio(q_affecting, q_affecting) * 0.25) * conr;
00743 A.m[i][col_affected2] = ((ratio(q_affecting+1,q_affecting) * 0.25) + 0.5)*conr ;
00744
00745 B[i] = (dq[q_affecting] - dp[size_dp-1]*0.25) * conr ;
00746 }
00747 else
00748 {
00749
00750 A.m[i][col_affected2] = (0.5 + (ratio(q_affecting+1, q_affecting+1) * 0.25)) * conr;
00751 A.m[i][col_affected3] = (ratio(q_affecting+2,q_affecting+1) * 0.25) * conr;
00752
00753 B[i] = (dq[q_affecting] - dp[0]*0.25) * conr ;
00754 }
00755 }
00756
00757
00758 if (closed)
00759 {
00760
00761
00762 A.m[rows-1][0] = 0.5;
00763 A.m[rows-1][cols-1] = 0.5;
00764
00765 B[rows-1] = dp[0] + *b[0] - *b[1]*0.5 - *b[b.size()-2]*0.5;
00766
00767
00768 A.m[rows-1][0] = A.m[rows-1][0] * big;
00769 A.m[rows-1][cols-1] = A.m[rows-1][cols-1] * big;
00770 B[rows-1] = B[rows-1] * big;
00771 }
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786
00787
00788
00789
00790
00791
00792
00793
00794 int num_vectors = 2;
00795 double **bs = new double*[num_vectors];
00796 for(i=0; i < num_vectors; i++){
00797 bs[i] = new double[rows];
00798 }
00799
00800 for(i=0; i < rows; i++){
00801 bs[0][i] = B[i][0];
00802 bs[1][i] = B[i][1];
00803 }
00804
00805 double **xs;
00806 if(k.size() > 4){
00807
00808 xs = solve_system(A,bs,cols,num_vectors);
00809 } else {
00810
00811
00812 xs = solve_small_system(A,bs,num_vectors);
00813 }
00814
00815
00816 for(i=0; i < num_vectors; i++) delete[] bs[i];
00817 delete[] bs;
00818
00819
00820 Point2D *nd=new Point2D[cols];
00821 for(i=0;i<cols;i++){
00822 nd[i].assign(xs[0][i], xs[1][i]);
00823 }
00824
00825
00826 for(i=0; i < num_vectors; i++) delete[] xs[i];
00827 delete[] xs;
00828 delete[] B;
00829
00830 return nd;
00831 }
00832
00855 double** QBSpline::solve_system(Matrix& A,double **bs, int cols,int num_vectors) const{
00856 int i,j;
00857
00858 double *B;
00859 double fact;
00860 double **xs=new double*[num_vectors];
00861 double *u=new double[cols];
00862
00863 Matrix *ATA = A.transpose_times_self();
00864 Matrix5 M(*ATA);
00865
00866 double alpha = ATA->m[cols-1][0];
00867 double beta = ATA->m[0][cols-1];
00868 double gamma = -M.d3[0];
00869
00870
00871 if(closed){
00872 M.d3[0] -= gamma;
00873 M.d3[cols-1] -= alpha * beta/gamma;
00874 for(i=0; i < num_vectors; i++){
00875 xs[i] = new double[cols];
00876
00877 u[0] = gamma;
00878 u[cols-1] = alpha;
00879 for(j=1; j < cols-1; j++) { u[j] = 0.0; }
00880
00881 B = A.transpose_times_vector(bs[i]);
00882 B = M.LUsolve(B);
00883 u = M.LUsolve(u);
00884
00885 fact = (B[0] + beta * (B[cols-1]/gamma) ) / (1.0 + u[0] + beta * (u[cols-1]/gamma));
00886 for(j=0; j < cols; j++){
00887 xs[i][j] = B[j] - (fact * u[j]);
00888 }
00889 delete[] B;
00890 }
00891 }
00892
00893
00894 else {
00895 for(i=0; i < num_vectors; i++){
00896 B = A.transpose_times_vector(bs[i]);
00897 B = M.LUsolve(B);
00898 xs[i] = B;
00899 }
00900 }
00901
00902 delete ATA;
00903 delete[] u;
00904 return xs;
00905 }
00906
00917 double** QBSpline::solve_small_system(Matrix &A,double **bs, int num_vectors) const{
00918 int i;
00919 double **xs=new double*[num_vectors];
00920 double *B;
00921
00922 Matrix *ATA = A.transpose_times_self();
00923 Matrix *ATAinv = ATA->inverse();
00924
00925 for(i=0;i<num_vectors;i++){
00926 B = A.transpose_times_vector(bs[i]);
00927 xs[i] = ATAinv->times_vector(B);
00928 delete[] B;
00929 }
00930 delete ATA;
00931 delete ATAinv;
00932
00933 return xs;
00934 }
00935
00947 void QBSpline::douglas_peucker(double tol, hashers::hash_set<BezierVertex*> &keep) const
00948 {
00949 unsigned i;
00950 int nk = k.size();
00951 if(nk <= 2) return;
00952
00953
00954 if(tol== 0.0){
00955 for(i=0; i< (unsigned)nk-2; i++) keep.insert( vertexs[i] );
00956 return;
00957 }
00958
00959 double top, bot, left, right;
00960 bbox(&top, &bot, &left, &right);
00961 double size = MAX(fabs(top-bot), fabs(left-right));
00962 tol = size*tol;
00963
00964
00965
00966
00967 bool mark[nk];
00968
00969
00970 for(i=0; i < k.size(); i++) mark[i]=true;
00971 simplify(tol, mark, 0, nk-1);
00972
00973
00974 mark[0] = false;
00975 mark[nk-1] = false;
00976
00977
00978
00979
00980 for(i=1; i< (unsigned)nk-1; i++){
00981 if(!mark[i]) keep.insert(vertexs[i-1]);
00982 }
00983 }
00984
00999 void QBSpline::simplify(double tol,bool *mark, int j,int z) const
01000 {
01001 if(z == j + 1) return;
01002 int maxi=0;
01003 double maxd=0.0;
01004 double dv;
01005 Point2D pj = *b[2*j];
01006 Point2D pz = *b[2*z];
01007
01008 for(int i=j+1; i < z; i++){
01009 dv = b[2*i]->dist_from_line(pj,pz);
01010 if(dv >= maxd){
01011 maxd = dv;
01012 maxi = i;
01013 }
01014 }
01015 if( maxd > tol ){
01016 mark[maxi] = false;
01017 simplify(tol, mark, j, maxi);
01018 simplify(tol, mark, maxi, z);
01019 }
01020 }
01021
01031 void QBSpline::bbox(double *top,double *left,double *bot,double *right) const
01032 {
01033 *left=*right=(*(b[0]))[0];
01034 *top=*bot=(*(b[0]))[1];
01035 for(unsigned i=1; i<b.size(); i++){
01036 if( (*(b[i]))[0] < *left ) { *left = (*(b[i]))[0]; }
01037 if( (*(b[i]))[0] > *right ) { *right = (*(b[i]))[0]; }
01038 if( (*(b[i]))[1] > *top ) { *top = ( *(b[i]) )[1]; }
01039 if( (*(b[i]))[1] < *bot ) { *bot = ( *(b[i]) )[1]; }
01040 }
01041 }
01042
01047 int QBSpline::get_num_deboor() const
01048 {
01049 return ((b.size()+1)/2)+1;
01050 }
01051
01056 int QBSpline::get_num_segments() const
01057 {
01058 return k.size() - 1;
01059 }
01060
01069 void QBSpline::set_bezier_vertex(double u, BezierVertex *v)
01070 {
01071 vertexs[get_closest_knot(u)-1]=v;
01072 }
01073
01083 void QBSpline::set_bezier_vertex(int idx, BezierVertex *v)
01084 {
01085
01086 assert( (idx >= 1) && (idx <= (int)k.size()-2));
01087 vertexs[idx-1]=v;
01088 }
01089
01099 unsigned QBSpline::get_bezier_vertex_idx(const BezierVertex *v) const
01100 {
01101 unsigned j;
01102 std::vector<BezierVertex*>::const_iterator i;
01103 for(j=0, i=vertexs.begin(); i!= vertexs.end(); ++i, j++){
01104 if( v == *i) return j+1;
01105 }
01106 return 0;
01107 }
01108
01116 BezierVertex *QBSpline::get_bezier_vertex(unsigned idx) const
01117 {
01118 return vertexs[idx - 1];
01119 }
01120
01127 ControlPoint QBSpline::get_control_point_at_idx(unsigned idx) const
01128 {
01129 assert( idx < k.size() );
01130 return b[idx*2];
01131 }
01132
01143 ControlPoint QBSpline::get_edge_center_cp(unsigned u0_idx, unsigned u1_idx) const
01144 {
01145 unsigned idx;
01146 assert( abs( (int) u0_idx - (int) u1_idx) == 1 );
01147 assert( u0_idx < k.size());
01148 assert( u1_idx < k.size());
01149 idx = u0_idx + u1_idx;
01150 return b[idx];
01151 }
01152
01158 double QBSpline::get_u_at_idx(unsigned idx) const
01159 {
01160 assert(idx < k.size());
01161 return k[idx];
01162 }
01163
01172 double QBSpline::dt(int i) const
01173 {
01174 return k[i+1]-k[i];
01175 }
01176
01178 double QBSpline::ratio(int a,int b) const
01179 {
01180 return (k[a]-k[a-1])/(k[b+1]-k[b-1]);
01181 }
01182
01184 ostream& operator<<(ostream& stream,const QBSpline& qbs)
01185 {
01186 unsigned i;
01187 unsigned size_b=qbs.b.size();
01188 unsigned size_k=qbs.k.size();
01189
01190 stream.precision(3);
01191 stream<<"QBSpline:= { closed="<<qbs.closed<<", restlength="<<qbs.restlength<<","<<endl;
01192 stream<<"size_b="<< size_b <<", b=[";
01193 for(i=0; i<size_b-1; i++) { stream<<*(qbs.b[i])<<", "; }
01194 stream<<*(qbs.b[size_b-1])<<"],"<<endl;
01195
01196 stream<<"size_k="<<size_k <<", k=[";
01197 for(i=0; i<size_k-1; i++){ stream<<qbs.k[i]<<", "; }
01198 stream<<qbs.k[size_k-1]<<"]"<<endl;
01199 stream<<"Interior Vertexs(num="<<size_k-2<<"): [End-Point, ";
01200 for(i=0; i<size_k-2; i++){ stream<<qbs.vertexs[i]<<", "; }
01201 stream<<"End-Point]}"<<endl;
01202 return stream;
01203 }