00001
00006 #ifndef _EMPTYSPLINE_C
00007 #define _EMPTYSPLINE_C
00008
00009 #ifdef HAVE_CONFIG_H
00010 #include <config.h>
00011 #endif
00012
00013
00014 #include <iostream>
00015 #include <cstddef>
00016
00017 #include "boundarymesh.h"
00018 #include "emptyspline.h"
00019
00020 EmptySpline::EmptySpline(const vector<ControlPoint> &deboor, BoundaryMesh *_bdry_mesh, bool _closed){
00021 vector<double> newk;
00022 initialize(deboor, newk, _bdry_mesh, 1.0, _closed);
00023 }
00024
00025 EmptySpline::EmptySpline(const vector<ControlPoint> &deboor, const vector<double> &newk, BoundaryMesh *_bdry_mesh, bool _closed){
00026 initialize(deboor, newk, _bdry_mesh, 1.0, _closed);
00027 }
00028
00029 EmptySpline::EmptySpline(const vector<ControlPoint> &deboor, const vector<double> &newk, BoundaryMesh *_bdry_mesh, double _restlength, bool _closed)
00030 {
00031 initialize(deboor, newk,_bdry_mesh, _restlength, _closed);
00032 }
00033
00034 void EmptySpline::initialize(const vector<ControlPoint> &deboor, const vector<double> &newk, BoundaryMesh *_bdry_mesh, double _restlength, bool _closed)
00035 {
00036 unsigned i;
00037 bdry_mesh = _bdry_mesh;
00038 restlength = _restlength;
00039
00040
00041
00042
00043
00044 closed=_closed;
00045
00046 if( closed && deboor.size() < 5 ) {
00047 cout<<"Error: closed spline has "<<deboor.size()<<" points but must have at least 5."<<endl;
00048 exit(1);
00049 }
00050
00051 if( deboor.size() < 3 ) {
00052 cout<<"Error: spline has "<<deboor.size()<<" points but must have at least 3."<<endl;
00053 exit(1);
00054 }
00055
00056
00057 vertexs.insert(vertexs.begin(), deboor.size()-3, NULL);
00058
00059
00060 if(newk.size() == 0){
00061 k.push_back(0.0);
00062 for(i=1; i < deboor.size()-1; i++){
00063 k.push_back( k.back() + (*deboor[i+1] - *deboor[i-1]).mag());
00064 }
00065 }
00066
00067
00068 else{
00069 if(newk.size() != deboor.size()-1){
00070 printf("Error: spline has %i deboor points but %i knots (sould be deboor-1)\n",deboor.size(),newk.size());
00071 exit(1);
00072 }
00073 k=newk;
00074 }
00075
00076
00077
00078
00079
00080 Point2D new_point;
00081 ControlPoint temp;
00082 b.push_back(deboor[0]);
00083 b.push_back(deboor[1]);
00084 for(i=1; i < deboor.size()-2; i++){
00085 temp=deboor[i+1];
00086 new_point = *(b.back()) * (dt(i)/(dt(i-1)+dt(i)))
00087 + (*temp) * (dt(i-1)/(dt(i-1)+dt(i)));
00088 b.push_back( bdry_mesh->add_cp(new_point) );
00089 b.push_back(temp);
00090 }
00091 b.push_back(deboor.back());
00092 }
00093
00094 EmptySpline::EmptySpline(const EmptySpline &other)
00095 {
00096 copy(other.b.begin(), other.b.end(), back_inserter(b));
00097 copy(other.k.begin(), other.k.end(), back_inserter(k));
00098 copy(other.vertexs.begin(), other.vertexs.end(), back_inserter(vertexs));
00099 closed = other.closed;
00100 bdry_mesh = other.bdry_mesh;
00101 }
00102
00103 EmptySpline& EmptySpline::operator=(const EmptySpline &other)
00104 {
00105 copy(other.b.begin(), other.b.end(), back_inserter(b));
00106 copy(other.k.begin(), other.k.end(), back_inserter(k));
00107 copy(other.vertexs.begin(), other.vertexs.end(), back_inserter(vertexs));
00108 closed = other.closed;
00109 bdry_mesh = other.bdry_mesh;
00110 return *this;
00111 }
00112
00113 void EmptySpline::compute_knot_vector()
00114 {
00115 k[0]=0.0;
00116 if(b.size() == 3){
00117 k[1] = (*(b[2]) - *(b[0])).mag();
00118 } else {
00119 k[1]=( *(b[3]) - *(b[0])).mag();
00120 for(unsigned i=2; i < k.size()-1; i++){
00121 k[i]=k[i-1] + (*(b[2*i+1]) - *(b[2*i-3])).mag();
00122 }
00123 k[k.size()-1] = k[k.size()-2]+ ( *(b[ b.size()-4 ])- *(b[ b.size()-1 ]) ).mag();
00124 }
00125 }
00126
00127 void EmptySpline::compute_bezier_points()
00128 {
00129 for(unsigned i=1; 2*i < b.size()-1; i++){
00130 *(b[2*i]) = *(b[2*i-1]) * (dt(i) / (dt(i-1)+dt(i)))
00131 + *(b[2*i+1]) * (dt(i-1) / (dt(i-1)+dt(i)));
00132 }
00133 }
00134
00135 ControlPoint EmptySpline::get_segment_center(double u0, double u1) const
00136 {
00137 int u0_idx, u1_idx;
00138 double temp;
00139 if(u0 > u1){
00140 temp = u0;
00141 u0 = u1;
00142 u1 = temp;
00143 }
00144
00145 u0_idx = get_closest_knot(u0);
00146 u1_idx = get_closest_knot(u1);
00147
00148 assert( u1_idx - u0_idx == 1 );
00149 assert( u0_idx < (int)k.size()-1);
00150 assert( u1_idx < (int)k.size() );
00151 assert( u0_idx >= 0);
00152 assert( u1_idx >= 1);
00153 return b[u0_idx*2+1];
00154 }
00155
00163 ControlPoint EmptySpline::get_deboor(int i) const
00164 {
00165 int d_size=((b.size()+1)/2) + 1;
00166
00167 if( i <= 0 ) {return b.front();}
00168 if( i >= d_size-1 ) {return b.back();}
00169 return b[2*i-1];
00170 }
00171
00172 void EmptySpline::getip(double t, int *i, double *p) const
00173 {
00174 *i=-1;
00175 double den;
00176 for(unsigned j=0; j < k.size()-1 ;j++){
00177 if((k[j]<=t)&&(k[j+1]>t)){
00178 *i=j;
00179 break;
00180 }
00181 }
00182 if(k.back()==t){
00183
00184 if(closed){
00185 *i=0;
00186 *p=0.0;
00187 return;
00188 }
00189 else{
00190 *i=k.size()-2;
00191 }
00192 }
00193
00194 if(*i==-1){
00195 cout<<"EmptySpline::getIP: parameter t is out of range"<<endl;
00196 *i=0;
00197 *p=0.0;
00198 return;
00199 }
00200 else{
00201 den=dt(*i);
00202 if(den!=0.0){
00203 *p=(t-k[*i])/den;
00204 }
00205 else{
00206 *p=0.0;
00207 }
00208 }
00209 }
00210
00211 int EmptySpline::get_closest_knot(double u) const
00212 {
00213 double best_dist= fabs(k[0] - u);
00214 double dist;
00215 int best=0;
00216 int i;
00217 for(i=1; i < (int)k.size(); i++){
00218 dist= fabs(k[i] - u);
00219 if(dist < best_dist){
00220 best_dist=dist;
00221 best=i;
00222 }
00223 }
00224 return best;
00225 }
00226
00227 int EmptySpline::get_closest_bezier(double u) const
00228 {
00229 return get_closest_knot(u) * 2 ;
00230 }
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248 int EmptySpline::get_edge_cps(double u0, double u1, ControlPoint &cp0, ControlPoint &cp1, ControlPoint &cp2, int &cp0_is_vert, int &cp2_is_vert) const{
00249 int cp0_idx, cp1_idx, cp2_idx;
00250 cp0_is_vert=-1;
00251 cp2_is_vert=-1;
00252
00253 cp0_idx = get_closest_bezier(u0);
00254 cp2_idx = get_closest_bezier(u1);
00255
00256 if(cp0_idx < 0 || cp2_idx < 0 ){
00257 printf("EmptySpline::get_edge_cps error bad u value\n");
00258 return FALSE;
00259 }
00260 if(((cp0_idx==0) && (cp2_idx == (int)b.size()-2))||
00261 ((cp2_idx==0) && (cp2_idx == (int)b.size()-2))){
00262 cp1_idx = b.size()-1;
00263 }
00264 else if(cp2_idx-cp0_idx==2){
00265 cp1_idx = cp0_idx+1;
00266 }
00267 else if(cp0_idx-cp2_idx==2){
00268 cp1_idx = cp0_idx-1;
00269 }
00270 else{
00271 printf("EmptySpline::get_edge_cps error, incorrect bezier points returned\n");
00272 return FALSE;
00273 }
00274 cp0 = b[cp0_idx];
00275 cp1 = b[cp1_idx];
00276 cp2 = b[cp2_idx];
00277
00278
00279 if(cp0_idx == 0) cp0_is_vert=0;
00280 if(cp0_idx == (int)b.size()-1) cp0_is_vert=1;
00281 if(cp2_idx == 0) cp2_is_vert=0;
00282 if(cp2_idx == (int)b.size()-1) cp2_is_vert=1;
00283 return TRUE;
00284 }
00285
00286 Point2D EmptySpline::evaluate_segment(int s, double u) const
00287 {
00288 s*=2;
00289 double v=1-u;
00290 return ( (*b[s])*v*v )+( (*b[s+1])*2*u*v )+( (*b[s+2])*u*u );
00291 }
00292
00293
00294 Point2D EmptySpline::evaluate(double u) const
00295 {
00296 int i;
00297 double p;
00298 getip(u,&i,&p);
00299 return evaluate_segment(i,p);
00300 }
00301
00302 void EmptySpline::print() const
00303 {
00304 cout<<*this;
00305 }
00306
00307
00308 void EmptySpline::evaluate_full(double u,Point2D *t,Point2D *b1,Point2D *b2,int *i) const
00309 {
00310 double p;
00311 getip(u,i,&p);
00312 int j = *i;
00313 *b1 = (*(b[2*j]))*(1-p) + (*(b[2*j+1]))*p;
00314 *b2 = (*(b[2*j+1]))*(1-p) + (*(b[2*j+2]))*p;
00315 *t = *b1 * (1-p) + *b2 * p;
00316 }
00317
00318
00319 void EmptySpline::add_knot(double u,ControlPoint &cp0, ControlPoint &cp1, ControlPoint &cp2)
00320 {
00321 Point2D p0,p1,p2;
00322 int i;
00323 if(u<=0 || u > k.back()){return;}
00324 evaluate_full(u,&p1,&p0,&p2,&i);
00325
00326 cp0 = b[2*i+1];
00327 *cp0 = p0;
00328 cp1 = bdry_mesh->add_cp(p1);
00329 cp2 = bdry_mesh->add_cp(p2);
00330
00331 b.insert(b.begin()+(2*i+2),2,cp2);
00332 b[2*i+2] = cp1;
00333
00334 k.insert(k.begin()+(i+1),u);
00335 vertexs.insert(vertexs.begin() + (i),NULL);
00336 compute_bezier_points();
00337 }
00338
00339
00340
00341
00342
00343
00344 bool EmptySpline::remove_knot(double u, ControlPoint &cp, double &u0, double &u1){
00345 int i;
00346 double p;
00347 if(u<=0 || u >= k.back()) return false;
00348 getip(u,&i,&p);
00349 if(i<=0 || (unsigned)i == k.size()-1){
00350 cout<<"EmptySpline::remove_knot: cannot remove end point"<<endl;
00351 return false;
00352 }
00353
00354
00355
00356
00357
00358 bdry_mesh->rem_cp( b[2*i] );
00359 bdry_mesh->rem_cp( b[2*i+1] );
00360 b.erase(b.begin() + (2*i), b.begin() + (2*i+2) );
00361 k.erase(k.begin()+i);
00362 vertexs.erase(vertexs.begin() + (i-1));
00363
00364
00365
00366
00367 if(i > 1){
00368 *b[2*i-2] = *b[2*i-3] * (dt(i-1) / ( dt(i-2) + dt(i-1) ))
00369 + *b[2*i-1] * (dt(i-2) / ( dt(i-2) + dt(i-1) ));
00370 }
00371
00372
00373
00374
00375 if((unsigned)2*i < b.size() - 1 ){
00376 *b[2*i] = *b[2*i-1] * (dt(i) / ( dt(i-1) + dt(i) ))
00377 + *b[2*i+1] * (dt(i-1) / ( dt(i-1) + dt(i) ));
00378 }
00379
00380
00381 cp = b[2*i-1];
00382
00383
00384
00385
00386
00387
00388 u0 = k[i-1];
00389 u1 = k[i];
00390 return true;
00391 }
00392
00393
00394
00395 void EmptySpline::move(Point2D *dp, Point2D *dq){
00396 Point2D *new_d;
00397 int i;
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411 if(k.size() >= 3){
00412
00413 new_d = compute_new_points(dp,dq);
00414 }
00415 else{
00416
00417 new_d = new Point2D[1];
00418 new_d[0] = dq[0];
00419 }
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435 for(i=1; i < get_num_deboor()-1; i++){
00436 *get_deboor(i) = *b[2*i-1] + new_d[i-1];
00437 }
00438 compute_bezier_points();
00439 delete[] new_d;
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451 }
00452
00453 Point2D* EmptySpline::compute_new_points(Point2D *dp,Point2D *dq) const{
00454 int i,rows,cols;
00455 int size_dp = k.size();
00456 int size_dq = k.size() - 1;
00457
00458 cols = size_dp - 1;
00459 rows = size_dp - 2 + size_dq;
00460
00461
00462 if (closed) rows++;
00463
00464 Matrix A(rows,cols);
00465 Point2D *B = new Point2D[rows];
00466
00467
00468
00469
00470 int p_affecting;
00471 int col_affected1;
00472 int col_affected2;
00473
00474 for(i=0; i < size_dp-2; i++)
00475 {
00476 p_affecting = i+1;
00477 col_affected1 = i;
00478 col_affected2 = i+1;
00479 A.m[i][col_affected1] = ratio(p_affecting,p_affecting);
00480 A.m[i][col_affected2] = ratio(p_affecting+1,p_affecting);
00481 B[i] = dp[p_affecting] ;
00482 }
00483
00484
00485 int q_affecting;
00486 int col_affected3;
00487 double conr = CONSTRAINT_RATIO;
00488
00489 for(i= size_dp-2; i < size_dp- 2 + size_dq;i++)
00490 {
00491 q_affecting = i-size_dp+2;
00492 col_affected1 = q_affecting - 1;
00493 col_affected2 = q_affecting;
00494 col_affected3 = q_affecting + 1;
00495 if (q_affecting > 0)
00496 if (q_affecting < size_dq - 1)
00497 {
00498
00499 A.m[i][col_affected1] = (ratio(q_affecting, q_affecting) * 0.25) * conr;
00500 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;
00501 A.m[i][col_affected3] = (ratio(q_affecting+2,q_affecting+1) * 0.25) * conr;
00502 B[i] = dq[q_affecting] * conr;
00503 }
00504 else
00505 {
00506 A.m[i][col_affected1] = (ratio(q_affecting, q_affecting) * 0.25) * conr;
00507 A.m[i][col_affected2] = ((ratio(q_affecting+1,q_affecting) * 0.25) + 0.5)*conr ;
00508
00509 B[i] = (dq[q_affecting] - dp[size_dp-1]*0.25) * conr ;
00510 }
00511 else
00512 {
00513
00514 A.m[i][col_affected2] = (0.5 + (ratio(q_affecting+1, q_affecting+1) * 0.25)) * conr;
00515 A.m[i][col_affected3] = (ratio(q_affecting+2,q_affecting+1) * 0.25) * conr;
00516
00517 B[i] = (dq[q_affecting] - dp[0]*0.25) * conr ;
00518 }
00519 }
00520
00521
00522 if (closed)
00523 {
00524
00525
00526 A.m[rows-1][0] = 0.5;
00527 A.m[rows-1][cols-1] = 0.5;
00528
00529 B[rows-1] = dp[0] + *b[0] - *b[1]*0.5 - *b[b.size()-2]*0.5;
00530
00531
00532 A.m[rows-1][0] = A.m[rows-1][0] * BIG;
00533 A.m[rows-1][cols-1] = A.m[rows-1][cols-1] * BIG;
00534 B[rows-1] = B[rows-1] * BIG;
00535 }
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558 int num_vectors = 2;
00559 double **bs = new double*[num_vectors];
00560 for(i=0; i < num_vectors; i++){
00561 bs[i] = new double[rows];
00562 }
00563
00564 for(i=0; i < rows; i++){
00565 bs[0][i] = B[i][0];
00566 bs[1][i] = B[i][1];
00567 }
00568
00569 double **xs;
00570 if(k.size() > 4){
00571
00572 xs = solve_system(A,bs,cols,num_vectors);
00573 } else {
00574
00575
00576 xs = solve_small_system(A,bs,num_vectors);
00577 }
00578
00579
00580 for(i=0; i < num_vectors; i++) delete[] bs[i];
00581 delete[] bs;
00582
00583
00584 Point2D *nd=new Point2D[cols];
00585 for(i=0;i<cols;i++){
00586 nd[i].assign(xs[0][i], xs[1][i]);
00587 }
00588
00589
00590 for(i=0; i < num_vectors; i++) delete[] xs[i];
00591 delete[] xs;
00592 delete[] B;
00593
00594 return nd;
00595 }
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606 double** EmptySpline::solve_system(Matrix& A,double **bs, int cols,int num_vectors) const{
00607 int i,j;
00608
00609 double *B;
00610 double fact;
00611 double **xs=new double*[num_vectors];
00612 double *u=new double[cols];
00613
00614 Matrix *ATA = A.transpose_times_self();
00615 Matrix5 M(*ATA);
00616
00617 double alpha = ATA->m[cols-1][0];
00618 double beta = ATA->m[0][cols-1];
00619 double gamma = -M.d3[0];
00620
00621
00622 if(closed){
00623 M.d3[0] -= gamma;
00624 M.d3[cols-1] -= alpha * beta/gamma;
00625 for(i=0; i < num_vectors; i++){
00626 xs[i] = new double[cols];
00627
00628 u[0] = gamma;
00629 u[cols-1] = alpha;
00630 for(j=1; j < cols-1; j++) { u[j] = 0.0; }
00631
00632 B = A.transpose_times_vector(bs[i]);
00633 B = M.LUsolve(B);
00634 u = M.LUsolve(u);
00635
00636 fact = (B[0] + beta * (B[cols-1]/gamma) ) / (1.0 + u[0] + beta * (u[cols-1]/gamma));
00637 for(j=0; j < cols; j++){
00638 xs[i][j] = B[j] - (fact * u[j]);
00639 }
00640 delete[] B;
00641 }
00642 }
00643
00644
00645 else {
00646 for(i=0; i < num_vectors; i++){
00647 B = A.transpose_times_vector(bs[i]);
00648 B = M.LUsolve(B);
00649 xs[i] = B;
00650 }
00651 }
00652
00653 delete ATA;
00654 delete[] u;
00655 return xs;
00656 }
00657
00658 double** EmptySpline::solve_small_system(Matrix &A,double **bs, int num_vectors) const{
00659 int i;
00660 double **xs=new double*[num_vectors];
00661 double *B;
00662
00663 Matrix *ATA = A.transpose_times_self();
00664 Matrix *ATAinv = ATA->inverse();
00665
00666 for(i=0;i<num_vectors;i++){
00667 B = A.transpose_times_vector(bs[i]);
00668 xs[i] = ATAinv->times_vector(B);
00669 delete[] B;
00670 }
00671 delete ATA;
00672 delete ATAinv;
00673
00674 return xs;
00675 }
00676
00677
00678 void EmptySpline::douglas_peucker(double tol, hash_set<BezierVertex*, CellHasher<BezierVertex>, CellEqual<BezierVertex> > &keep) const{
00679 int nk = k.size();
00680 if(nk <= 2) return;
00681
00682
00683
00684
00685 bool *mark = new bool[nk];
00686 unsigned i;
00687
00688
00689 for(i=0; i < k.size(); i++) mark[i]=true;
00690 simplify(tol, mark, 0, nk-1);
00691
00692
00693 mark[0] = false;
00694 mark[nk-1] = false;
00695
00696
00697
00698
00699 for(i=1; i< (unsigned)nk-1; i++){
00700 if(!mark[i]){
00701 keep.insert(vertexs[i-1]);
00702 }
00703 }
00704 delete[] mark;
00705 }
00706
00707 void EmptySpline::simplify(double tol,bool *mark, int j,int z) const{
00708 if(z == j + 1) return;
00709 int maxi=0;
00710 double maxd=0.0;
00711 double dv;
00712 Point2D pj = *b[2*j];
00713 Point2D pz = *b[2*z];
00714
00715 for(int i=j+1; i < z; i++){
00716 dv = b[2*i]->dist_from_line(pj,pz);
00717 if(dv >= maxd){
00718 maxd = dv;
00719 maxi = i;
00720 }
00721 }
00722 if( maxd > tol ){
00723 mark[maxi] = false;
00724 simplify(tol, mark, j, maxi);
00725 simplify(tol, mark, maxi, z);
00726 }
00727 }
00728
00729 void EmptySpline::bbox(double *top,double *left,double *bot,double *right) const
00730 {
00731 *left=*right=(*(b[0]))[0];
00732 *top=*bot=(*(b[0]))[1];
00733 for(unsigned i=1; i<b.size(); i++){
00734 if( (*(b[i]))[0] < *left ) { *left = (*(b[i]))[0]; }
00735 if( (*(b[i]))[0] > *right ) { *right = (*(b[i]))[0]; }
00736 if( (*(b[i]))[1] > *top ) { *top = ( *(b[i]) )[1]; }
00737 if( (*(b[i]))[1] < *bot ) { *bot = ( *(b[i]) )[1]; }
00738 }
00739 }
00740
00741
00742
00743
00744 ostream& operator<<(ostream& stream,const EmptySpline& qbs)
00745 {
00746 unsigned i;
00747 unsigned size_b=qbs.b.size();
00748 unsigned size_k=qbs.k.size();
00749
00750 stream.precision(3);
00751 stream<<"EmptySpline:= { closed="<<qbs.closed<<", restlength="<<qbs.restlength<<","<<endl;
00752 stream<<"size_b="<< size_b <<", b=[";
00753 for(i=0; i<size_b-1; i++) { stream<<*(qbs.b[i])<<", "; }
00754 stream<<*(qbs.b[size_b-1])<<"],"<<endl;
00755
00756 stream<<"size_k="<<size_k <<", k=[";
00757 for(i=0; i<size_k-1; i++){ stream<<qbs.k[i]<<", "; }
00758 stream<<qbs.k[size_k-1]<<"]"<<endl;
00759 stream<<"Interior Vertexs(num="<<size_k-2<<"): [End-Point, ";
00760 for(i=0; i<size_k-2; i++){ stream<<qbs.vertexs[i]<<", "; }
00761 stream<<"End-Point]}"<<endl;
00762 return stream;
00763 }
00764
00765 #endif