emptyspline.C

Go to the documentation of this file.
00001 
00006 #ifndef _EMPTYSPLINE_C
00007 #define _EMPTYSPLINE_C
00008 
00009 #ifdef HAVE_CONFIG_H
00010 #include <config.h>
00011 #endif /* HAVE_CONFIG_H */
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   /* A closed spline is a spline where the first and last deboor point
00041    * is the same.  Open splines must have at least 3 deboor points,
00042    * closed splines must have at least 5 deboor points
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   /* Initialize the vertexs vector */
00057   vertexs.insert(vertexs.begin(), deboor.size()-3, NULL); 
00058 
00059   /* if newk is empty, make our own knots */
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   /* Store knot vector.  There should be d.size()-1 knots. */
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   /* Populate the b vector there are 2*deboor.size()-3 ControlPoints
00077    * in b.  The new points must be added to the point list of the
00078    * BoundryComplex.
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     //cout<<"u0: "<<u0<<" u0-idx: "<<u0_idx<<"  u1: "<<u1<<" u1_idx: "<<u1_idx<<endl;
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     //this is possibly wrong
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 /* This is code for a reverse lookup of the control points on a spline given the u values into it.
00233  * It is necessary in order to rebuild a mesh from the ruby output file format
00234  * Also, we require u0 < u1 for this to work
00235  * Furthermoore, this function will check if cp0 or cp2 are at the ends of the spline.  This data will
00236  * be returned via the cp0_is_vert and cp2_is_vert variables.  
00237  * We let -1=not a vertex
00238  *         0=vertex0
00239  *         1=vertex1
00240  *
00241  * This way we can determine where each BezierVertex is in respect to the boundary and can thus set the
00242  * boundary data correctly with the BezierVertex::set_bdry() functions. 
00243  *
00244  * This whole function is only used when reading in from the ruby file format.  This is the conssession
00245  * we must make for not having cached control point values.  Since it is only called during the read
00246  * operation the inefficientcies here are not important. 
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     //if(u1 <= u0) return FALSE;
00253     cp0_idx = get_closest_bezier(u0);
00254     cp2_idx = get_closest_bezier(u1);
00255     //printf("u0: %f  u1: %f  cp0: %i  cp2: %i spline: %x spline_max: %i\n",(float)u0,(float)u1,cp0_idx,cp2_idx,(unsigned)this,b.size()-1);
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     /* Determine if cp0 or cp2 are BoundaryVertexs.  Return -1 for no, 0 for vertex0, and 1 for vertex1 */
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 /* This function must be called AFTER the Bezier Edges and vertexs 
00340  * associated with the knot to be removed have been destroyed.  This is
00341  * because the data points are owned by the bezier Edges and vertexs and
00342  * so must be deleted before the control points are deleted by this function.  
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     /* Now the two segments on either side of the knot will become one.  
00355      * We will remove two bezier points and the knot value.  Also, we 
00356      * will remove the control points from the mesh.
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     /* update preceeding bezier point unless it is the first point of the spline
00365      * in which case it is a deboor point
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     /* update proceeding bezier point unless it is the last point of the spline
00373      * in which case it is a deboor point
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     /* Now send back the new center control point of the final segment */
00381     cp = b[2*i-1];
00382     
00383     /* Also send back the u values for the new bezier_edge to be created
00384      * We can't rely on the BezierVertex's for this info, as the D0 vertexs
00385      * don't have any u values
00386      */
00387     //compute_bezier_points();
00388     u0 = k[i-1];
00389     u1 = k[i];
00390     return true; 
00391 }
00392 
00393 //This function does NOT move the endpoint(s)
00394 //Although it does respect their apparent motion as given in dp
00395 void EmptySpline::move(Point2D *dp, Point2D *dq){
00396     Point2D *new_d;
00397     int i;
00398 
00399     /* Print out dp and dq for debugging purposes
00400     cout << "-------------------------"<<endl;
00401     cout << "dp: ";
00402     for (i=0;i<k.size();i++)
00403       cout << dp[i] << " - ";
00404     cout << endl << "=====" << endl<<"dq: ";
00405     for (i=0;i<k.size()-1;i++)
00406       cout << dq[i] << " - ";
00407     cout <<endl;
00408     */
00409 
00410     //get new d points
00411     if(k.size() >= 3){
00412       //Solve for the displacements of the deBoor points
00413       new_d = compute_new_points(dp,dq);
00414     }
00415     else{
00416         //too small to build the matrix
00417         new_d = new Point2D[1];
00418         new_d[0] = dq[0];
00419     }
00420     
00421 
00422     /*
00423     cout << "Printing new vectors" << endl;
00424     for (int i=0; i< k.size()-1;i++)
00425       {
00426     cout << new_d[i] << " - ";
00427       }
00428     cout << endl;
00429     */
00430 
00431     // These two lines will set the endpoints appropriately, however we aren't move the endpoints 
00432     //*get_deboor(0) = *b[0] + dp[0];
00433     //*get_deboor(get_num_deboor()-1) = *b[b.size()-1] + dp[k.size()-1];
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     //debugging print errors on differentiability condition
00444     if (closed)
00445       {
00446     Point2D err = *b[1]*0.5 + *b[b.size()-2]*0.5;
00447     err -= *b[0];
00448     cout << "Diff error is: "<<err<<endl;
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     // Closed Splines have one extra constraint for differentiability at the d0 vertex
00462     if (closed) rows++;
00463 
00464     Matrix A(rows,cols);
00465     Point2D *B = new Point2D[rows];
00466 
00467 
00468     //construct matrix
00469     // add constraints for each p affectng the deBoor point to it's right and left
00470     int p_affecting;
00471     int col_affected1;
00472     int col_affected2;
00473     // i is constraint(row number)
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     //add constraint for each dq affecting the deBoor point above it, and to it's right and left
00485     int q_affecting;
00486     int col_affected3;
00487     double conr = CONSTRAINT_RATIO;
00488     // i is constraint(row number)
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           // normal interior case
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         {  //last dq case, only affects two columns
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           //affect of the end point goes on the right hand side
00509           B[i] = (dq[q_affecting] - dp[size_dp-1]*0.25) * conr ;
00510         }
00511     else 
00512       {
00513         // first dq case, only affects two columns
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         //affect of the end point goes on the right hand side
00517         B[i] = (dq[q_affecting] - dp[0]*0.25) * conr ;
00518       }
00519       }
00520 
00521     // Add the extra constraint for differentiability at d0 vertex of closed splines
00522     if (closed) 
00523       {
00524     // Note that these 0.5 are arbitrary, otherwise the problem is nonlinear
00525     // This paradigm is equivalent to that of fixing the knot sequence everywhere else
00526     A.m[rows-1][0] = 0.5;
00527     A.m[rows-1][cols-1] = 0.5;
00528     // Note this constraint is not solely in terms of displacements
00529     B[rows-1] = dp[0] + *b[0] - *b[1]*0.5 - *b[b.size()-2]*0.5;
00530 
00531     // This is a numerical trick to make this constraint BIG times as important as the others
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         /* Print out matrix and vector for debugging purposes
00538     if (closed)
00539     for (int x=0;x<rows;x++)
00540       {
00541     for (int y=0;y<cols;y++)
00542       cout << A.m[x][y] << "\t";
00543     cout << B[x];
00544     cout << endl;
00545       }
00546        */
00547 
00548     /* this solver system is set up so that multiple vectors can
00549      * be solved using the same matrix.  Right now only x and
00550      * y coordinates are being solved for, but if there is more
00551      * data in the future this will be trivial to upgrade and still
00552      * maintain optimal efficiency (the matrix is factored only once
00553      * and used to solve many systems)  I can change this to be
00554      * faster if we don't need this robustness
00555      */
00556 
00557     //construct array of vectors to be solved
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         //send matrix and array of vectors to solver
00572         xs = solve_system(A,bs,cols,num_vectors);
00573     } else {
00574         //send to matrix inverse solver since it is too small for the
00575         //band diagonal solver
00576         xs = solve_small_system(A,bs,num_vectors);
00577     }
00578   
00579     //destroy array of vectors to be solved;
00580     for(i=0; i < num_vectors; i++) delete[] bs[i];
00581     delete[] bs;
00582 
00583     //create new points
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     //destroy array of solution vectors
00590     for(i=0; i < num_vectors; i++) delete[] xs[i];
00591     delete[] xs;
00592     delete[] B;
00593   
00594     return nd;
00595 }
00596 /*
00597  this function takes constraint matrix A and array of vectors to solve,
00598  bs. A and bs are not modified.  The function then computes A^T * A and
00599  stores it as a Matrix5 (5 banded matrix).  Then Crout LU decomposition
00600  and a LUsolver [both are in Matrix5] are used to solve for each vector.
00601  Since A^T*A is actually a cyclic banded matrix the Sherman-Morrison
00602  Formula is used to correct for the errors caused by the non-zero 
00603  off-diagonal elements.  Finally the function will return an array of
00604  solution vectors.
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     /* if closed we need to solve the cyclic banded case */
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     /* otherwise it is a simple banded case to solve */
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; /* use vectors returned by transpose_times_vector for xs (will be deleted later)*/   
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     /* we create a vector of bezier points who coorespond to
00683      * a BezierVertex.  This has size =  k.size().
00684      */
00685     bool *mark = new bool[nk];
00686     unsigned i;
00687 
00688     /* true=throw away, false=keep*/
00689     for(i=0; i < k.size(); i++) mark[i]=true; 
00690     simplify(tol, mark, 0, nk-1);  
00691     
00692     /*never remove first or last vertex */
00693     mark[0] = false;
00694     mark[nk-1] = false;
00695     
00696     /* add all keeper vertexs, we can skip ednpoints
00697      * as they are already added 
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

Generated on Mon May 24 09:53:30 2010 for TUMBLE by  doxygen 1.5.2