shape: include/shape/qsc_approximation

00001 /*****************************************************************************
00002  * M a t h e m a g i x
00003  *****************************************************************************
00004  * Quadratically Supported Curve approximation functions
00005  * 2012
00006  * Adrien Poteaux & Angelos Mantzaflaris
00007  *****************************************************************************
00008  *               Copyright (C) 2012 INRIA Sophia-Antipolis
00009  *****************************************************************************
00010  * Comments : Basic functions for the QSC computations.
00011  ****************************************************************************/
00012 
00013 # ifndef qsc_approximation_fcts_hpp
00014 # define qsc_approximation_fcts_hpp
00015 
00016 #define PI 3.14159265
00017 
00018 #include <math.h>
00019 # include <shape/point.hpp>
00020 # include <shape/viewer_axel.hpp>
00021 
00022 # include <realroot/Seq.hpp>
00023 # include <realroot/polynomial.hpp> // polynomials
00024 # include <realroot/linear_doolittle.hpp> // linear polynomial system solver
00025 # include <realroot/ring_monomial_tensor.hpp>
00026 # include <realroot/ring_bernstein_tensor.hpp>
00027 
00028 # define AXEL viewer<axel, default_env>
00029 
00030 # define TMPL template<class K> // field K
00031 # define VECT point<K> // point with coefficients in K
00032 
00033 # define KVPOL polynomial<VECT,with<MonomialTensor> > // dense monomial representation
00034 # define POL polynomial<K,with<MonomialTensor> > // dense monomial representation
00035 
00036 # define BPOL polynomial<VECT,with<Bernstein> > // dense Bernstein representation
00037 
00038 namespace mmx {
00039 namespace shape {
00040 
00041 //====================================================================
00042 
00043 TMPL
00044 inline VECT orth_vect(const VECT &a) // associates [b,-a] to [a,b]
00045 {
00046   return VECT(a[1],-a[0]); // for a CCW curve, this is the outer normal.
00047 }
00048 
00049 TMPL
00050 inline VECT myeval(const KVPOL &P, const K&t) //VECT-polynomial evaluation
00051 {
00052   int n= degree(P);
00053   VECT res= P[n];
00054 
00055   for ( int i=n-1; i!=-1;i--)
00056     res= res*t + P[i];
00057 
00058   return res;
00059 }
00060 
00061   TMPL class Rsc_curve;
00062 
00063 //====================================================================
00064 //====================================================================
00065 
00067 TMPL
00068 class Bezier_curve //Bezier curve over [0,1]
00069 {
00070 public:
00071   Bezier_curve() {};
00072   //Quadratic
00073   Bezier_curve(const VECT &a, const VECT &b, const VECT &c)
00074     {
00075       cpoints<< a << b << c ;
00076     };
00077   //Cubic
00078   Bezier_curve(const VECT &a, const VECT &b, const VECT &c, const VECT &d)
00079     {
00080       cpoints<< a << b << c << d;
00081     };
00082   //Arbitrary
00083   Bezier_curve(int n) { cpoints= Seq<VECT>(n+1); };
00084   Bezier_curve(Seq<VECT> & cpts) { cpoints=cpts; };
00085 
00086 public:
00087   inline bool ccw( const int& i = 0) const {
00088 
00089     return ( cpoints[ i ].x()*cpoints[i+1].y()-cpoints[ i ].y()*cpoints[i+1].x() + 
00090              cpoints[i+1].x()*cpoints[i+2].y()-cpoints[i+1].y()*cpoints[i+2].x() + 
00091              cpoints[i+2].x()*cpoints[ i ].y()-cpoints[i+2].y()*cpoints[ i ].x() < 0 ?
00092              false : true );
00093   };
00094 
00095   VECT normal(const K &t) const{ 
00096     return orth_vect( diff().eval(t) ); 
00097   };
00098   VECT unormal(const K &t) const{ 
00099     VECT nn=normal(t);
00100     return nn/nn.norm(); 
00101   };
00102 
00103 
00104   VECT  operator  [](const int & i ) const { return cpoints[i]; };
00105   VECT & operator [](const int & i) { return cpoints[i]; };
00106   Seq<VECT> control_points() const { return cpoints; };
00107   int degree() const { return cpoints.size()-1; };
00108   int npoints() const { return cpoints.size(); };
00109 
00110   inline VECT eval(const K & t) const
00111     {
00112       Seq<VECT> tmp(cpoints);
00113       for (int k=degree() ; k!=0; k-- )
00114         for (int i=0; i!=k; i++ )
00115           tmp[i]= (1.0-t)*tmp[i]+t*tmp[i+1] ;
00116       return tmp[0];
00117     }
00118 
00119   Bezier_curve diff() const{
00120     int n=degree();
00121     Bezier_curve D(n-1);
00122     for (int k=0; k<n ; k++ )
00123       D[k]= n* ( cpoints[k+1]-cpoints[k] );
00124     return D;
00125   }
00126 
00127   Seq<Bezier_curve> subdiv(const K t= (K)0.5) const{
00128     int n=degree();
00129     Seq<VECT> l(n+1), r(cpoints);
00130     int k,i,j=0;
00131     
00132     for (k = degree(); k!= 0; k--, j++ )
00133     {
00134       l[j] = r[0];
00135       for ( i = 0; i != k; i++ )
00136         r[i] = ((K)1.0-t)*r[i]+t*r[i+1];
00137     }
00138     l[j] = r[0];
00139     
00140     Seq<Bezier_curve> S;
00141     S<< Bezier_curve(l) <<Bezier_curve(r);
00142     return S;
00143   }
00144 
00145 
00146   // Sample the curve with k samples.
00147   void sample(Seq<VECT>& P, const int& k) const
00148     {
00149       K t( 1.0/ K(k-1) );
00150 
00151       P << cpoints[0];
00152       for (int i=1; i!=k-1; i++ )
00153         P << eval(i*t);
00154       P << cpoints[ cpoints.size()-1 ];
00155     };
00156 
00157   // Sample curve on the given t-values.
00158   void sample(Seq<VECT> & P, const Seq<K>& TT) const
00159     {
00160       for (unsigned i=0; i!= TT.size(); i++ )
00161         P << eval( TT[i] );
00162     };
00163 
00164   //Export curve to Axel as polyline
00165   void print (AXEL& axl, const int& k= 50) {
00166 
00167     Seq<VECT> P;
00168     sample(P, k);
00169 
00170   axl<<" <curve type=\"mesh\">\n<vect>\nVECT\n";
00171   axl<< k-1 <<" "
00172      <<2*k-2<<" "
00173      <<k-1<<"\n";
00174   for(int i=1; i!=k;i++) axl<<"2 ";//
00175   axl<<"\n";
00176   for(int i=1; i!=k;i++) axl<<"1 ";//
00177   axl<<"\n";
00178 
00179   for(int i=1; i!=k;i+=1)
00180     axl<< P[i-1].x()<<" "<< P[i-1].y()<< " 0 " <<
00181       P[i].x()<<" "<< P[i].y()<< " 0 \n";
00182   
00183   for(int i=1; i!=k;i++)
00184     axl<< "0.314 0.979 1 1\n";
00185   axl<<" </vect>\n </curve>\n";
00186 
00187   };
00188 
00189 
00190 private:
00191   Seq<VECT> cpoints;
00192 };
00193 
00194 TMPL
00195 std::ostream&
00196 operator<<(std::ostream& os, Bezier_curve<K>  Q) {
00197   int k, n=Q.degree();
00198   Seq<VECT> P= Q.control_points();
00199   for (k=0; k<n ; k++ )
00200     os <<"("<<P[k][0]<<","<<P[k][1]<<","<<P[k][2] <<")*B["<<k<<","<<n<<"] + ";
00201   os <<"("<<P[k][0]<<","<<P[k][1]<<","<<P[k][2] <<")*B["<<k<<","<<n<<"]\n";
00202   return os;
00203 }
00204 
00205 TMPL
00206 AXEL&
00207 operator<<(AXEL& axl, Bezier_curve<K>  Q) {
00208   int n=Q.degree();
00209 
00210   axl<<"<curve type=\"bspline\" name=\""<< "Bezier" <<"\">\n";
00211   axl<<"<dimension>2</dimension>\n";
00212   axl<<"<number>"<< n+1 <<"</number>\n";
00213   axl<<"<order>"<< n+1 <<"</order>\n";
00214   axl<<"<knots>";
00215   for (int i=0; i!=n+1; i++ )
00216     axl<< 0 << " ";
00217   for (int i=0; i!=n+1; i++ )
00218     axl<< 1 << " ";
00219   axl<<"</knots>\n";
00220   axl<<"<points>\n";
00221   for (int i=0; i!=n+1; i++ )
00222     axl<< Q[i].x() << " "<< Q[i].y() <<" "<< Q[i].z() << "\n";
00223   axl<<"</points>\n";
00224   axl<< "</curve>\n";
00225 
00226   return axl;
00227 }
00228 
00229 //====================================================================
00230 
00231 TMPL
00232 class Qsc_curve //Qsc curve
00233 {
00234 public:
00235   Qsc_curve() {};
00236 
00237   // Curve defined by:
00238   // * Support function h(n)= nt*A*n + at*n,
00239   //   with matrix A = [ u  w/2] and vector a = [a1]
00240   //                   [w/2  v ]                [a2]
00241   // that is, h(x,y)= u*x^2 + v*y^2 + w*x*z + a1*x + a2*y
00242   // * Starting- ending vectors n0,n1 and orientation pp.
00243   Qsc_curve(const K& u, const K& v, const K& w,const K& a1,const K& a2,
00244             const VECT& nn0, const VECT& nn1, const int& pp=1)
00245     {
00246       //h=POL(u,2,0)+POL(v,2,1)+
00247       //  POL(w,1,0)*POL(1,1,1)+
00248       //  POL(a1,1,0)+POL(a2,1,1);
00249       h << u << v << w << a1 << a2;
00250 
00251       path=(pp<0 ? -1 : 1 );
00252       n0= nn0 / nn0.norm();
00253       n1= nn1 / nn1.norm();
00254     };
00255 
00256   // Make a Qsc approximation of a (Quadratic) Bezier curve
00257   Qsc_curve(const Bezier_curve<K>& B)
00258     {
00259       Rsc_curve<K> Rc(B);
00260       n0= Rc.nstart();
00261       n1= Rc.nend();
00262       *this=Qsc_curve<K>(n0, Rc.seval(n0), Rc.sdiff(n0),
00263                    Rc.nmiddle(), Rc.seval(Rc.nmiddle()),
00264                    n1, Rc.seval(n1), Rc.sdiff(n1),
00265                    Rc.orientation() );
00266 
00267       return;//finished -- old version below
00268 
00269       Rsc_curve<K> R(B);
00270       path= R.orientation();
00271       n0= R.nstart();
00272       n1= R.nend();
00273 
00274       K Vh[5]; // the values of h and its derivative at the control points
00275       K Mq[25]; // the values of q and its derivative at the control points (exp
00276       VECT nn, dh;// temporary vectors
00277 
00278       // first line: value at n0
00279       nn=R.nstart(); 
00280       Vh[0]= R.seval(nn);
00281       Mq[0]= nn[0]*nn[0]; // x^2
00282       Mq[1]= nn[1]*nn[1]; // y^2
00283       Mq[2]= nn[0]*nn[1]; // x*y
00284       Mq[3]= nn[0]; // x
00285       Mq[4]= nn[1]; // y
00286 
00287       // second line: fi-derivative at n0
00288       dh= R.sgrad(nn);// fi-deriv: [h( n(fi))]' = h'(n(fi))*n'(fi)
00289       Vh[1]= -dh[0]*nn[1] + dh[1]*nn[0];
00290       Mq[5]=-2*nn[0]*nn[1]; // -2*x*y
00291       Mq[6]=2*nn[0]*nn[1]; // 2*x*y
00292       Mq[7]=(nn[0]*nn[0]-nn[1]*nn[1]); // x^2-y^2
00293       Mq[8]=-nn[1]; // -y
00294       Mq[9]=nn[0]; // x
00295 
00296       // third line: value at n1
00297       nn= R.nend(); 
00298       Vh[2]= R.seval(nn);
00299       Mq[10]= nn[0]*nn[0]; // x^2
00300       Mq[11]= nn[1]*nn[1]; // y^2
00301       Mq[12]= nn[0]*nn[1]; // x*y
00302       Mq[13]= nn[0]; // x
00303       Mq[14]= nn[1]; // y
00304 
00305       // forth line: fi-derivative at n1
00306       dh= R.sgrad(nn);// fi-deriv: [h( n(fi))]' = h'(n(fi))*n'(fi)
00307       Vh[3]= -dh[0]*nn[1] + dh[1]*nn[0];
00308       Mq[15]=-2*nn[0]*nn[1]; // -2*x*y
00309       Mq[16]=2*nn[0]*nn[1]; // 2*x*y
00310       Mq[17]=(nn[0]*nn[0]-nn[1]*nn[1]); // x^2-y^2
00311       Mq[18]=-nn[1]; // -y
00312       Mq[19]=nn[0]; // x
00313 
00314       // fifth line: value at the middle (n0+n1)/2
00315       nn=R.nmiddle();
00316       //nn=B.unormal(.5); 
00317       Vh[4]= R.seval(nn);
00318       Mq[20]= nn[0]*nn[0]; // x^2
00319       Mq[21]= nn[1]*nn[1]; // y^2
00320       Mq[22]= nn[0]*nn[1]; // x*y
00321       Mq[23]= nn[0]; // x
00322       Mq[24]= nn[1]; // y
00323 
00324       // Solve
00325       K q[5];
00326       linear::LUsolve(q,Mq,Vh,5); // Mq*q=Vh ; size 5
00327         for (int j=0; j<5; j++)
00328           h << q[j];
00329 
00330       std::cout<< support() <<"\n";
00331     };
00332 
00333   // Make a Qsc approximation given normals and .....
00334   Qsc_curve(const VECT& nn0, const K  & vn0, const K  & dn0,
00335             const VECT& nnm, const K  & vnm,
00336             const VECT& nn1, const K  & vn1, const K  & dn1, const int& pp= 1)
00337     {
00338       path= pp;
00339       n0  = nn0;
00340       n1  = nn1;
00341 
00342       K Vh[5]; // the values of h and its derivative at the control points
00343       K Mq[25]; // the values of q and its derivative at the control points (exp
00344 
00345       // first line: value at n0
00346       Vh[0]= vn0;//R.seval(n0);
00347       Mq[0]= n0[0]*n0[0]; // x^2
00348       Mq[1]= n0[1]*n0[1]; // y^2
00349       Mq[2]= n0[0]*n0[1]; // x*y
00350       Mq[3]= n0[0]; // x
00351       Mq[4]= n0[1]; // y
00352 
00353       // second line: fi-derivative at n0
00354       Vh[1]= dn0;
00355       Mq[5]=-2*n0[0]*n0[1]; // -2*x*y
00356       Mq[6]=2*n0[0]*n0[1]; // 2*x*y
00357       Mq[7]=(n0[0]*n0[0]-n0[1]*n0[1]); // x^2-y^2
00358       Mq[8]=-n0[1]; // -y
00359       Mq[9]=n0[0]; // x
00360 
00361       // third line: value at n1
00362       Vh[2]= vn1; //R.seval(n1);
00363       Mq[10]= n1[0]*n1[0]; // x^2
00364       Mq[11]= n1[1]*n1[1]; // y^2
00365       Mq[12]= n1[0]*n1[1]; // x*y
00366       Mq[13]= n1[0]; // x
00367       Mq[14]= n1[1]; // y
00368 
00369       // forth line: fi-derivative at n1
00370       Vh[3]= dn1;
00371       Mq[15]=-2*n1[0]*n1[1]; // -2*x*y
00372       Mq[16]=2*n1[0]*n1[1]; // 2*x*y
00373       Mq[17]=(n1[0]*n1[0]-n1[1]*n1[1]); // x^2-y^2
00374       Mq[18]=-n1[1]; // -y
00375       Mq[19]=n1[0]; // x
00376 
00377       // fifth line: value at the middle (n0+n1)/2
00378       Vh[4]= vnm; // R.seval(nn);
00379       Mq[20]= nnm[0]*nnm[0]; // x^2
00380       Mq[21]= nnm[1]*nnm[1]; // y^2
00381       Mq[22]= nnm[0]*nnm[1]; // x*y
00382       Mq[23]= nnm[0]; // x
00383       Mq[24]= nnm[1]; // y
00384 
00385       // Solve
00386       K q[5];
00387       linear::LUsolve(q,Mq,Vh,5); // Mq*q=Vh ; size 5
00388         for (int j=0; j<5; j++)
00389           h << q[j];
00390 
00391       std::cout<< support() <<"\n";
00392     };
00393 
00394 
00395   // Evaluate suppport function at n
00396   inline K seval(const VECT& n) const
00397     {
00398       return 
00399         h[0]*std::pow(n[0],2)+
00400         h[1]*std::pow(n[1],2)+
00401         h[2]*n[0]*n[1]+
00402         h[3]*n[0]+
00403         h[4]*n[1];
00404     };
00405 
00406   // Evaluate curve at n
00407   inline VECT eval(const VECT& n) const
00408     {
00409       return VECT( -h[0]*std::pow(n[0],3)
00410                    -h[2]*std::pow(n[0],2)*n[1]
00411                    -h[1]*std::pow(n[1],2)*n[0]
00412                    +h[0]*2*n[0]
00413                    +h[2]*n[1]
00414                    +h[3] , 
00415                    -h[1]*std::pow(n[1],3)
00416                    -h[2]*std::pow(n[1],2)*n[0]
00417                    -h[0]*std::pow(n[0],2)*n[1]
00418                    +h[1]*2*n[1]
00419                    +h[2]*n[0]
00420                    +h[4] ); 
00421     };
00422 
00423   // Gradient of the support function h=u/v
00424   inline VECT sgrad(const VECT& n) const 
00425     {
00426       POL hh= support();
00427       return VECT( mmx::diff(hh,0), mmx::diff(hh,1) ) ;
00428     };
00429 
00430  inline int ccw() const { 
00431    //return (path==1 ? : true : false);
00432    return ( n0[0]*n1[1]- n0[1]*n1[0] <0 ? -1 : 1 );
00433   };
00434 
00435 
00436   // Sample the curve on the circle with k samples.
00437   void sample(Seq<VECT>& P, const int& k= 50) const
00438     {
00439       K f0,fi;
00440       f0= acos( n0[0] );
00441       if (sin(n0[1])<0) f0= 2*PI- f0;
00442 
00443       if ( path * ccw() > 0 ) 
00444       {
00445         fi =  path * acos( dot(n0,n1)) / K(k-1) ;
00446         if ( n0==n1) 
00447           fi=  2*PI / K(k-1) ;
00448       }
00449       else
00450       {
00451         fi =  path * (2*PI - acos(dot(n0,n1))) / K(k-1);
00452       }
00453       std::cout<<"f0: "<< f0  <<"\n";
00454       std::cout<<"cos : "<< cos( f0 )  <<"\n";
00455       std::cout<<"cosn: "<< cos( acos(n0[0]) )  <<"\n";
00456       std::cout<<"sin : "<< sin( f0 )  <<"\n";
00457       std::cout<<"sinn: "<< sin( asin(n0[1]) )  <<"\n";
00458 
00459       //P<< eval(n0);
00460       P << eval( VECT(cos(f0), sin(f0)) ) ;
00461       for(int i=2; i!=k; i++ )
00462       {
00463         f0+= fi;
00464         P << eval( VECT(cos(f0), sin(f0)) ) ;
00465       }
00466       //P<< eval(n1);
00467       P << eval( VECT(cos(f0+fi), sin(f0+fi)) ) ;
00468 
00469     };
00470 
00471 
00472   // Sample curve on the given n-vectors.
00473   void sample(Seq<VECT>& P, const Seq<VECT>& NN) const
00474     {
00475       for (unsigned i=0; i!= NN.size(); i++ )
00476         P << eval( NN[i] );
00477     };
00478 
00479   // Sample the support function on the circle with k samples.
00480   void ssample(Seq<K>& P, const int& k= 50) const
00481     {
00482       K f0,fi, fs;
00483       f0= acos( n0[0] );
00484       if (sin(n0[1])<0) f0= 2*PI- f0;
00485       //std::cout<<"ccw: "<< ccw() <<"\n";
00486 
00487       if ( path * ccw() > 0 ) 
00488       {
00489         fs= acos( dot(n0,n1));
00490         fi = path * fs / K(k-1) ;
00491 //        if ( abs(fi)<0.000001) 
00492 //          fi=  2*PI / K(k-1) ;
00493       }
00494       else
00495       {
00496         fs= (2*PI - acos(dot(n0,n1)));
00497         fi =  path * fs / K(k-1);
00498       }
00499 
00500       P<< 0.0 ;
00501       P<< seval(n0);
00502       for(int i=2; i!=k; i++ )
00503       {
00504         f0+= fi ;
00505         P<< (i-1)*fi;
00506         P << seval( VECT(cos(f0), sin(f0)) ) ;
00507       }
00508       P<< k*fi ;
00509       P<< seval(n1);
00510 
00511     };
00512 
00513   //Export curve to Axel as polyline
00514   void print (AXEL& axl, const int& k= 50) {
00515 
00516     Seq<VECT> P;
00517     sample(P, k);
00518 
00519     //axl<<" <point>"<< P[0] << " </point>\n";
00520     //axl<<" <point>"<< P[P.size()-1] << " </point>\n";
00521 
00522   axl<<" <curve type=\"mesh\">\n<vect>\nVECT\n";
00523   axl<< k-1 <<" "
00524      <<2*k-2<<" "
00525      <<k-1<<"\n";
00526   for(int i=1; i!=k;i++) axl<<"2 ";//
00527   axl<<"\n";
00528   for(int i=1; i!=k;i++) axl<<"1 ";//
00529   axl<<"\n";
00530 
00531   for(int i=1; i!=k;i+=1)
00532     axl<< P[i-1].x()<<" "<< P[i-1].y()<< " 0 " <<
00533       P[i].x()<<" "<< P[i].y()<< " 0 \n";
00534   
00535   for(int i=1; i!=k;i++)
00536     axl<< "0.314 0.979 1 1\n";
00537   axl<<" </vect>\n </curve>\n";
00538 
00539   };
00540 
00541   //Export the graph of the support function to Axel
00542   void print_supp_graph (AXEL& axl, const int& k= 50) {
00543 
00544     Seq<K> P;
00545 
00546     ssample(P, k);
00547     
00548     axl<<" <curve type=\"mesh\">\n<vect>\nVECT\n";
00549     axl<< k-1 <<" "
00550        <<2*k-2<<" "
00551        <<k-1<<"\n";
00552     for(int i=1; i!=k;i++) axl<<"2 ";//
00553     axl<<"\n";
00554     for(int i=1; i!=k;i++) axl<<"1 ";//
00555     axl<<"\n";
00556     
00557     for(int i=3; i<2*k;i+=2)
00558       axl<< P[i-3]<<" "<< P[i-2]<< " 0 " <<
00559         P[i-1]<<" "<< P[ i ]<< " 0 \n";
00560     
00561     for(int i=1; i!=k;i++)
00562       axl<< "0.314 0.979 1 1\n";
00563     axl<<" </vect>\n </curve>\n";
00564   };
00565 
00566 
00567   VECT & nstart() {return n0; };
00568   VECT & nend  () {return n1; };
00569   VECT nmiddle  () { VECT m= n0+n1; return m/m.norm(); };
00570   int orientation() {return path; };
00571 
00572   K support_size()
00573     {
00574       return acos(dot(n0,n1));
00575     };
00576 
00577   POL support()
00578     {
00579       POL sf(h[0],2,0);
00580       sf += POL(h[1],2,1)+POL(h[2],1,0)*POL(1,1,1)
00581         +POL(h[3],1,0)+POL(h[4],1,1);
00582       return sf;
00583     };
00584 
00585   //POL h;
00586 
00587 private:
00588 
00589   Seq<K> h; // support quadratic
00590 
00591   VECT n0, n1;// start end n0 --> n1
00592   int path; //   path (orientation), +-1
00593 
00594     };
00595 
00596 
00597 //Export rational curve to Axel as rational curve
00598 TMPL
00599 AXEL&
00600 operator<<(AXEL& axl, Qsc_curve<K> Q) {
00601 
00602   axl<<"<curve type=\"rational\" name=\""<< "QSC" <<"\">\n";
00603   axl<<"<domain>"<< -20<< " "<< 20 <<"</domain>\n";
00604 
00605   POL x("t^6",variables("t") );
00606   x[0] = Q.h[0]+Q.h[3];
00607   x[2] = 5*Q.h[0]+3*Q.h[3]-4*Q.h[1];
00608   x[3] = 8*Q.h[2];
00609   x[4] = -5*Q.h[0]+4*Q.h[1]+3*Q.h[3] ;
00610   x[6] = -Q.h[0]+Q.h[3] ;
00611   axl<<"<polynomial variables=\"x0\">"<<x <<"</polynomial>\n";
00612   x[0] = Q.h[4]+Q.h[2];
00613   x[1] = -2*Q.h[0]+4*Q.h[1];
00614   x[2] = 3*Q.h[4]-3*Q.h[2];
00615   x[3] = 4*Q.h[0];
00616   x[4] = 3*Q.h[2]+3*Q.h[4];
00617   x[5] =-2*Q.h[0]+4*Q.h[1] ;
00618   x[6] = -Q.h[2]+Q.h[4] ;
00619   axl<<"<polynomial variables=\"x0\">"<<x<<"</polynomial>\n";
00620 /*
00621   axl<<"<polynomial variables=\"t\">"<<
00622      -Q.h[0]+Q.h[3] << "*t^6+" <<
00623      -5*Q.h[0]+4*Q.h[1]+3*Q.h[3]<< "*t^4+" <<
00624      8*Q.h[2]<< "*t^3+" <<
00625      5*Q.h[0]+3*Q.h[3]-4*Q.h[1]<< "*t^2+" << 
00626      Q.h[0]+Q.h[3]<<"</polynomial>\n";
00627   axl<<"<polynomial variables=\"t\">"<<
00628      -Q.h[2]+Q.h[4]<< "*t^6+" <<
00629      -2*Q.h[0]+4*Q.h[1]<< "*t^5+" <<
00630      3*Q.h[2]+3*Q.h[4]<< "*t^4+" <<
00631      4*Q.h[0]<< "*t^3+" <<
00632      3*Q.h[4]-3*Q.h[2]<< "*t^2+" <<
00633      -2*Q.h[0]+4*Q.h[1]<< "*t+" << 
00634      Q.h[4]+Q.h[2]<<"</polynomial>\n";
00635 */
00636   axl<<"<polynomial variables=\"x0\">0</polynomial>\n";//3rd coordinate
00637   axl<<"<polynomial variables=\"x0\">1+3*x0^2+3*x0^4+x0^6</polynomial>\n";
00638   axl<< "</curve>\n";
00639 
00640   return axl;
00641 }
00642 
00643 
00644 //====================================================================
00645 
00646 TMPL
00647 class Rsc_curve //Rsc curve
00648 {
00649 public:
00650   Rsc_curve() {};
00651 
00652   // Rational support function h= u/v
00653   Rsc_curve( const POL& px, const POL& py, const VECT& nn0=VECT(), const VECT& nn1=VECT(), const int& pp=1 ) 
00654     {
00655       u= POL(0,0,0)*POL(0,0,1);
00656       v= POL(0,0,0)*POL(0,0,1);
00657       u+=px;
00658       v+=py;
00659 
00660       n0=nn0;
00661       n1=nn1;
00662       path=pp;
00663     };
00664 
00665   Rsc_curve( const Bezier_curve<K>& B)
00666     {
00667       VECT Va=B[0]-2*B[1]+B[2];
00668       VECT Vb=2*(B[1]-B[0]);
00669       VECT Vc=B[0];
00670 
00671       // POL( coefficient, power, variable)
00672       u= POL(4*Vc[0]*Va[0]-Vb[0]*Vb[0], 2, 0) +
00673         POL(4*Vc[0]*Va[1]-2*Vb[0]*Vb[1]+4*Vc[1]*Va[0],1,0)*POL(1, 1, 1) +
00674         POL(-Vb[1]*Vb[1]+4*Vc[1]*Va[1], 2, 1) ;
00675       
00676       v= POL(4*Va[0],1,0) + POL(4*Va[1],1,1);
00677 
00678       n0= B.normal(0);
00679       n0= n0/n0.norm();
00680       n1= B.normal(1);
00681       n1= n1/n1.norm();
00682 
00683       path= ( B.ccw()==1 ? 1 : -1 );
00684 
00685       //std::cout<<"normal 0 =" <<B.normal(0) << "\n";
00686       //std::cout<<"normal 1 =" <<B.normal(1) << "\n";
00687       //std::cout<<"angle =" << acos( dot(n0,n1)) << "\n";
00688     };
00689 
00690 
00691   // Evaluate the curve at n
00692   inline VECT eval(const VECT& n) const
00693     {
00694       Seq<K> nn;
00695       K h=  u(n[0], n[1])  / v(n[0], n[1]) ;
00696       VECT dh= sgrad( n );
00697 
00698       return ( n*h + dh - n*dot(dh,n) ) ;
00699     };
00700 
00701   // Evaluate suppport function at n
00702   inline K seval(const VECT& n) const
00703     {
00704       return u(n[0], n[1])  / v(n[0], n[1]) ;
00705     };
00706 
00707 
00708   // Gradient of the support function h=u/v
00709   inline VECT sgrad(const VECT& n) const 
00710     {
00711       POL hx, hy ;
00712       hx= (mmx::diff(u,0)* v) - (u * mmx::diff(v,0) );
00713       hy= (mmx::diff(u,1)* v) - (u * mmx::diff(v,1) );
00714       K dn = v( n[0],n[1] ); dn*= dn;
00715 
00716       return VECT( hx( n[0],n[1] ) / dn, hy( n[0],n[1] ) / dn   ); 
00717     };
00718 
00719   // Derivative of the support function h( fi )
00720   inline K sdiff(const VECT& n) const 
00721     {
00722       VECT dh= sgrad(n);
00723       
00724       return -dh[0]*n[1] + dh[1]*n[0];;
00725     };
00726 
00727 
00728  inline int ccw() const { 
00729    //return (path==1 ? : true : false);
00730    return ( n0[0]*n1[1]- n0[1]*n1[0] <0 ? -1 : 1 );
00731   };
00732 
00733   // Sample the curve on the circle with k samples.
00734   void sample(Seq<VECT>& P, const int& k= 50) const
00735     {
00736       K f0,fi;
00737       f0= acos( n0[0] );
00738       if (sin(n0[1])<0) f0= 2*PI- f0;
00739       //std::cout<<"ccw: "<< ccw() <<"\n";
00740 
00741       if ( path * ccw() > 0 ) 
00742       {
00743         fi =  path * acos( dot(n0,n1)) / K(k-1) ;
00744 //        if ( abs(fi)<0.000001) 
00745 //          fi=  2*PI / K(k-1) ;
00746       }
00747       else
00748       {
00749         fi =  path * (2*PI - acos(dot(n0,n1))) / K(k-1);
00750       }
00751 
00752       P<< eval(n0);
00753       for(int i=2; i!=k; i++ )
00754       {
00755         f0+= fi;
00756         P << eval( VECT(cos(f0), sin(f0)) ) ;
00757       }
00758       P<< eval(n1);
00759 
00760     };
00761 
00762   // Sample the support function on the circle with k samples.
00763   void ssample(Seq<K>& P, const int& k= 50) const
00764     {
00765       K f0,fi, fs;
00766       f0= acos( n0[0] );
00767       if (sin(n0[1])<0) f0= 2*PI- f0;
00768       //std::cout<<"ccw: "<< ccw() <<"\n";
00769 
00770       if ( path * ccw() > 0 ) 
00771       {
00772         fs= acos( dot(n0,n1));
00773         fi = path * fs / K(k-1) ;
00774 //        if ( abs(fi)<0.000001) 
00775 //          fi=  2*PI / K(k-1) ;
00776       }
00777       else
00778       {
00779         fs= (2*PI - acos(dot(n0,n1)));
00780         fi =  path * fs / K(k-1);
00781       }
00782 
00783       P<< 0.0 ;
00784       P<< seval(n0);
00785       for(int i=2; i!=k; i++ )
00786       {
00787         f0+= fi ;
00788         P<< (i-1)*fi;
00789         P << seval( VECT(cos(f0), sin(f0)) ) ;
00790       }
00791       P<< k*fi ;
00792       P<< seval(n1);
00793 
00794     };
00795 
00796   //Export curve to Axel as polyline
00797   void print (AXEL& axl, const int& k= 50) {
00798 
00799     Seq<VECT> P;
00800     sample(P, k);
00801 
00802   axl<<" <curve type=\"mesh\">\n<vect>\nVECT\n";
00803   axl<< k-1 <<" "
00804      <<2*k-2<<" "
00805      <<k-1<<"\n";
00806   for(int i=1; i!=k;i++) axl<<"2 ";//
00807   axl<<"\n";
00808   for(int i=1; i!=k;i++) axl<<"1 ";//
00809   axl<<"\n";
00810 
00811   for(int i=1; i!=k;i+=1)
00812     axl<< P[i-1].x()<<" "<< P[i-1].y()<< " 0 " <<
00813       P[i].x()<<" "<< P[i].y()<< " 0 \n";
00814  
00815   for(int i=1; i!=k;i++)
00816     axl<< "0.314 0.979 1 1\n";
00817   axl<<" </vect>\n </curve>\n";
00818 
00819   };
00820 
00821   //Export the graph of the support function to Axel
00822   void print_supp_graph (AXEL& axl, const int& k= 50) {
00823 
00824     Seq<K> P;
00825 
00826     ssample(P, k);
00827     
00828     axl<<" <curve type=\"mesh\">\n<vect>\nVECT\n";
00829     axl<< k-1 <<" "
00830        <<2*k-2<<" "
00831        <<k-1<<"\n";
00832     for(int i=1; i!=k;i++) axl<<"2 ";//
00833     axl<<"\n";
00834     for(int i=1; i!=k;i++) axl<<"1 ";//
00835     axl<<"\n";
00836     
00837     for(int i=3; i<2*k;i+=2)
00838       axl<< P[i-3]<<" "<< P[i-2]<< " 0 " <<
00839         P[i-1]<<" "<< P[ i ]<< " 0 \n";
00840     
00841     for(int i=1; i!=k;i++)
00842       axl<< "0.314 0.979 1 1\n";
00843     axl<<" </vect>\n </curve>\n";
00844   };
00845 
00846 
00847   VECT nstart() {return n0; };
00848   VECT nend  () {return n1; };
00849   VECT nmiddle  () { VECT m= (n0+n1)*path; return m/m.norm(); };
00850 
00851   int orientation() {return path; };  
00852 
00853   POL numer() { return u; };
00854   POL denom() { return v; };
00855 
00856   double support_size()
00857     {
00858       return acos(dot(n0,n1));
00859     };
00860 
00861 
00862 
00863 
00864 private:
00865   POL u, v;
00866   VECT n0, n1;// support start end n0 --> n1
00867   int path; //   path (orientation), +-1
00868 
00869 
00870 };
00871 //====================================================================
00872 
00873 TMPL
00874 class Piecewise_Qsc_curve //Qsc curve
00875 {
00876 public:
00877   Piecewise_Qsc_curve() {};
00878 
00879   Piecewise_Qsc_curve(const Bezier_curve<K>& B, const double& step= .3)
00880     {
00881       Rsc_curve<K> R(B);
00882       n0= R.nstart();
00883       n1= R.nend();
00884       path= R.orientation();
00885 
00886       int k = R.support_size() / step ;
00887       std::cout<<"got "<<R.support_size() << ", pieces: "<< k <<"\n";      
00888 
00889       K fi, f0= acos( n0[0] );
00890       if (sin(n0[1])<0) f0= 2*PI- f0;
00891 
00892       if ( path * R.ccw() > 0 ) 
00893         fi =  path * acos( dot(n0,n1)) / K(k) ;
00894       else
00895         fi =  path * (2*PI - acos(dot(n0,n1))) / K(k);
00896 
00897       f0+=fi;
00898       VECT nn0(n0);
00899       VECT nnm(cos(f0+ 0.5*fi), sin(f0+ 0.5*fi));
00900       VECT nn1(cos(f0), sin(f0));
00901 
00902       P<< Qsc_curve<K>(nn0, R.seval(nn0), R.sdiff(nn0),
00903                        nnm, R.seval(nnm),
00904                        nn1, R.seval(nn1), R.sdiff(nn1),
00905                        path );
00906       //return;
00907 
00908       for (int i=1; i<k; i++ )
00909       {
00910         nn0= nn1;
00911         nnm= VECT(cos(f0+(0.5*fi)), sin(f0+ (0.5*fi)) );
00912         f0+=fi; 
00913         nn1= VECT(cos(f0), sin(f0));
00914         P<< Qsc_curve<K>(nn0, R.seval(nn0), R.sdiff(nn0),
00915                          nnm, R.seval(nnm),
00916                          nn1, R.seval(nn1), R.sdiff(nn1),
00917                          path );
00918      }
00919       nn0= nn1;
00920       nnm= VECT(cos(f0+ (0.5*fi) ), sin(f0+ (0.5*fi)));
00921 //      P<< Qsc_curve<K>(nn0, R.seval(nn0), R.sdiff(nn0),
00922 //                       nnm, R.seval(nnm),
00923 //                       n1, R.seval(n1), R.sdiff(n1),
00924 //                       path );
00925 
00926       std::cout<< P.size() <<"\n";
00927       //  for( double s=0.0; s
00928 
00929     };
00930 
00931   void print (AXEL& axl, const int& k= 50) {
00932 //    int i= 1 + k/P.size();
00933     for ( unsigned j=0; j!=P.size(); j++ )
00934       P[j].print( axl  );
00935   };
00936       
00937   Qsc_curve<K> & piece(const unsigned& i) {return P[i]; };
00938 
00939   VECT nstart() {return n0; };
00940   VECT nend  () {return n1; };
00941   int orientation() {return path; };
00942 
00943 
00944     private:      
00945       Seq< Qsc_curve<K> > P;
00946       VECT n0, n1;// start end n0 --> n1
00947       int path; //   path (orientation), +-1
00948 };
00949 
00950 //====================================================================
00951 
00952 
00953 TMPL 
00954 class normal_vector 
00955 {
00956 public:
00957   normal_vector () {} ;
00958   normal_vector (const VECT &a, const VECT &b, const VECT &c);
00959   VECT middle();
00960   VECT eval_unit(const K & t);
00961 private:
00962   KVPOL data;
00963 };
00964 
00965 
00966   /* CHECK */    
00967 TMPL
00968 normal_vector<K>::normal_vector(const VECT &a, const VECT &b, const VECT &c) // this computes the non unit normal vector function associated to the B�zier curve defined by a, b and c.
00969 {
00970   data = KVPOL(1,1); //defines a degree one polynomial
00971   data[0]=orth_vect(2*(b-a));
00972   data[1]=orth_vect(2*(a-2*b+c));
00973 
00974   std::cout<< Bezier_curve<K>(a,b,c).normal(0)<<" toto " <<data[0]<<"\n";
00975   std::cout<< Bezier_curve<K>(a,b,c).normal(1)<<" toto " <<data[0]+data[1];
00976 
00977 }
00978   
00979 TMPL
00980 VECT normal_vector<K>::eval_unit(const K & t) // this returns the unit normal vector for value t.
00981 {
00982   VECT v;
00983   v= myeval(data,t);
00984   return v / std::sqrt(dot(v,v));
00985 }
00986   
00987 TMPL
00988 VECT normal_vector<K>::middle() // this computes the middle of the arc of circle defined by n([0,1]).
00989 /* question : est ce meilleur que de prendre tout simplement t=1/2 ? */
00990 {
00991   VECT n0=eval_unit(0);
00992   VECT n1=eval_unit(1);
00993   VECT n2;
00994   if ( n0 == -n1 ) 
00995     n2=orth_vect(n0); // Achtung ! check later if there are some numerical problems (with numerical computations, we should never have n0=-n1, but n0+n1 could be very small !
00996   else 
00997     {
00998       n2=n0+n1;
00999       n2=n2/std::sqrt(dot(n2,n2));
01000     }
01001   VECT n=eval_unit(0.5);
01002   K p1=dot(n,n2);
01003   K p2=dot(n0,n2);
01004   if ( p1 < p2 ) 
01005     return -n2;
01006   else 
01007     return n2;
01008 }
01009   
01010 TMPL
01011 class support_function
01012 {
01013 public:
01014   support_function () {} ;
01015   support_function (const VECT &a, const VECT &b, const VECT &c);
01016   K eval_diff (const VECT & n);
01017   K eval (const VECT & n);
01018 private:
01019   VECT Va, Vb, Vc; // B(t)=Va * t^2 + Vb * t + Vc
01020 };
01021   
01022 TMPL
01023 support_function<K>::support_function(const VECT &a, const VECT &b, const VECT &c) // this computes the support function associated to the B�zier curve defined by a, b and c, namely a*(1-t)^2 + 2*b*t*(1-t) + t^2*c.
01024 {
01025   Va=a-2*b+c;
01026   Vb=2*(b-a);
01027   Vc=a;
01028 }
01029   
01030 TMPL
01031 K support_function<K>::eval(const VECT & n) // evaluation of a support function of a quadratic B�zier curve at the normal vector n
01032 {
01033   return -std::pow(dot(n,Vb),2)/(4*dot(n,Va))+dot(n,Vc);
01034 }
01035   
01036 TMPL
01037 K support_function<K>::eval_diff(const VECT & n)
01038 {
01039   return dot(n,Vb)*
01040     (-dot(n,orth_vect(Va))*dot(n,Vb)+2*dot(n,orth_vect(Vb))*dot(n,Va))/
01041     (4*std::pow(dot(n,Va),2))
01042 -dot(n,orth_vect(Vc));
01043 }
01044   
01045 TMPL
01046 class quad_supp_func
01047 {
01048 public:
01049   quad_supp_func () {} ;
01050   quad_supp_func (support_function<K> h, normal_vector<K> n);
01051   // K eval_diff (VECT n);
01052   // K eval (VECT n);
01053   Seq<K> Cq();
01054 private:
01055   K q[5];
01056 };
01057     
01058 TMPL
01059 Seq<K> quad_supp_func<K>::Cq() // ???
01060 {
01061   // mmout << "z\n";
01062   Seq<K> Q;
01063   // mmout << Q << "\n";
01064   for (int i=0; i<5; i++) 
01065     {
01066       Q << q[i];
01067     }
01068   return Q;
01069 }
01070   
01071 TMPL
01072 quad_supp_func<K>::quad_supp_func(support_function<K> h,normal_vector<K> n) // this computes the quadratic support function that approximates the support function h defined over n.
01073 {
01074   // mmout << "b\n";
01075   VECT N[3];
01076   N[0]=n.eval_unit(0);
01077   N[1]=n.eval_unit(1);
01078   N[2]=n.middle();
01079   // mmout << "c\n";
01080   K Vh[5]; // the values of h and its derivative at the control points
01081   K Mq[25]; // the values of q and its derivative at the control points (expressed in the base of the unknown we want to find
01082   for (int i=0; i<3; i++) 
01083     {
01084       VECT nn=N[i]; // 3 first lines : values at n0, n1, n1/2 
01085       Vh[i]=h.eval(nn);
01086       Mq[5*i]=std::pow(nn[0],2); // x^2
01087       Mq[5*i+1]=std::pow(nn[1],2); // y^2
01088       Mq[5*i+2]=nn[0]*nn[1]; // x*y CHANGE *2 ??
01089       Mq[5*i+3]=nn[0]; // x
01090       Mq[5*i+4]=nn[1]; // y
01091     }
01092   // mmout << "d\n";
01093   for (int i=3; i<5; i++) 
01094     {
01095       VECT nn=N[i-3]; // 2 last lines : derivative at n0, n1
01096       Vh[i]=h.eval_diff(nn);
01097       Mq[5*i+0]=-2*nn[0]*nn[1]; // -2*x*y
01098       Mq[5*i+1]=2*nn[0]*nn[1]; // 2*x*y
01099       Mq[5*i+2]=(std::pow(nn[0],2)-std::pow(nn[1],2)); // x^2-y^2 CHANGE *2 ?
01100       Mq[5*i+3]=-nn[1]; // -y
01101       Mq[5*i+4]=nn[0]; // x
01102     }
01103 
01104   linear::LUsolve(q,Mq,Vh,5); // Mq*q=Vh ; size 5
01105   // output : q[0]*x^2+q[1]*y^2+q[2]*x*y+q[3]*x+q[4]*y
01106 
01107   std::cout << "Valeur de Vh et Mq" << "\n";
01108   for (int i=0; i<5; i++){
01109     std::cout << Vh[i] << " = ";
01110     for (int j=0; j<5; j++)
01111       std::cout << Mq[5*i+j] << " ";
01112     std::cout << "\n";}
01113     std::cout << "\n";
01114 
01115 }
01116 
01117   // Debut des reprises
01118 
01119 TMPL
01120 class supp_func_eval{
01121   supp_func_eval () {};
01122   supp_func_eval (Bezier_curve<K> B,VECT n); // mettre les bons types
01123   supp_func_eval (quad_supp_func<K> q, VECT n); // idem
01124 };
01125 
01126   
01127 // TO DO : evaluation of h and q at a given t (with my formulae and by taking count of the unit point of view.
01128  
01129 TMPL
01130 class error_func
01131 {
01132 public:
01133   error_func () {} ;
01134   error_func (support_function<K> h, normal_vector<K> n);
01135   // K eval_diff (VECT n);
01136   // K eval (VECT n);
01137   VECT Cq();
01138 private:
01139   K q[5];
01140 };
01141   
01142 } ; // namespace shape
01143 } ; // namespace mmx
01144 //====================================================================
01145 # undef TMPL
01146 # undef AXEL
01147 # undef VECT
01148 # undef KVPOL
01149 # undef BPOL
01150 # undef POL
01151 # undef PI
01152 # endif // qsc_approximation_hpp
include/shape/qsc_approximation_fcts.hpp
