realroot: include/realroot/fatarcs

00001 #include <vector>
00002 #include <realroot/Interval.hpp>
00003 #include <realroot/Seq.hpp>
00004 #define SMALL  C(10e-20)
00005 #define REP    poly.rep()
00006 #define SIZE   poly.size()
00007 
00008 #pragma once
00009 
00010 //#include <realroot/fatarcs.hpp>
00011 
00012 template<class C>
00013 struct arc_rep;
00014 template<class C>
00015 struct domain;
00016 template<class C>
00017 struct box_rep;
00018 
00019 using namespace mmx;
00020 
00022 //vec_t fcts
00023 
00024 template<class C>
00025 std::vector<C> vec (const C c1, const C c2) {
00026   std::vector<C> a(2, C(0));
00027    a[0]= c1; a[1]= c2;
00028   return a;
00029 }
00030 
00031 template<class C>
00032 std::vector<C> rotl(std::vector<C> p){
00033   std::vector<C> v; v=vec(C(-1)*p[1],p[0]); return(v);
00034 };       /*rotate left*/
00035 
00036 template<class C>
00037 C dot(std::vector<C> p ,std::vector<C> q){
00038   C v=C(0);
00039  for(unsigned i=0; i<p.size(); i++){
00040    v +=p[i]*q[i];
00041  } 
00042 return(v);
00043 };
00044 
00045 template<class C>
00046 std::vector<C> v_plus(std::vector<C> p ,std::vector<C> q){
00047   std::vector<C> v(p.size(),C(0));
00048  assert( p.size()==q.size() );
00049  for(unsigned i=0; i<p.size(); i++){
00050    v[i]=p[i]+q[i];
00051  } 
00052 return(v);
00053 };
00054 
00055 template<class C>
00056 std::vector<C> v_minus(std::vector<C> p ,std::vector<C> q){
00057   std::vector<C> v(p.size(),C(0));
00058   assert( p.size()==q.size() );
00059  for(unsigned i=0; i<p.size(); i++){
00060    v[i]=p[i]-q[i];
00061  } 
00062 return(v);
00063 };
00064 
00065 template<class C>
00066 std::vector<C> v_div(std::vector<C> p ,C q){
00067   if (q!=C(0)){
00068     std::vector<C>  v(p.size(),C(0));
00069     for(unsigned i=0; i<p.size(); i++){
00070       v[i]=p[i]/q;}; 
00071     return(v);}else{return std::vector<C>();}
00072 };
00073 
00074 template<class C>
00075 std::vector<C> v_mul(C q, std::vector<C> p){
00076   std::vector<C>  v(p.size(),C(0));
00077  for(unsigned i=0; i<p.size(); i++){
00078    v[i]=p[i]*q;
00079  } 
00080 return(v);
00081 };
00082 
00083 template<class C>
00084 C v_length(std::vector<C> p){
00085   return(sqrt(dot(p,p)));
00086 };   /*length of vector*/
00087 
00088 template<class C>
00089 std::vector<C> matrat(std::vector<C> v1,std::vector<C> v2){
00090   std::vector<C> mm; mm=vec(v1[0]*v2[0]+v1[1]*v2[1],v1[0]*v2[1]-v2[0]*v1[1]); 
00091   return(mm);
00092 };
00093 
00094 template<class C>
00095 std::vector<C> crossrat(std::vector<C> v1,std::vector<C> v2,std::vector<C> v3,std::vector<C> v4){ 
00096   std::vector<C> mm=matrat(matrat(v_minus(v1,v2),v_minus(v3,v2)),matrat(v_minus(v1,v4),v_minus(v3,v4))); 
00097 return(mm);
00098 };
00099 
00100 
00102 //solver and test fcts
00103 template<class C>
00104 bool is_zero(C p){
00105   if(p==C(0)){return true;}
00106   else{return false;}
00107   };
00108 
00109 template<class C>
00110 bool is_small(C p){
00111   if(p==C(0)){return true;}
00112   else{return false;}
00113   };
00114 
00115 /*
00116 template<class C>
00117 bool is_small(C p){
00118   if(p<SMALL && p>C(-1)*SMALL){return true;}
00119   else{return false;}
00120   };
00121 */
00122 
00123 template<class C>
00124 Seq<C> solve2(C a, C b, C c){
00125   Seq<C> r; r<<C(-1)<<C(-1);  
00126   C root1,root2;  
00127   if(is_small(a)){if(!is_small(b)){r[0]=C(-1)*c/b;r[1]=C(-1)*c/b;}}
00128   else{if(sign(b*b-C(4)*a*c) >= 0){
00129       root1=(C(-1)*b+sqrt(b*b-C(4)*a*c))/(C(2)*a);
00130       root2=(C(-1)*b-sqrt(b*b-C(4)*a*c))/(C(2)*a);
00131       r[0]=root1;r[1]=root2;}
00132     }
00133  
00134   return(r);
00135 };/*solve(a*x^2+b*x+c=0)*/
00136 
00137 template<class C>
00138 bool is_unit1(C p){
00139   if(p<C(0) || p>C(1) ){return false;}
00140   else{return true;}
00141 };/*1dim unit test*/
00142 
00143 
00144 template<class C>
00145 bool is_unit2( std::vector<C> p){
00146   if( is_unit1( p[0] ) && is_unit1( p[1] ) ){return true;}
00147   else{return false;}
00148 };/*2dim unit test*/
00149 
00150 
00152 //bezier poly comp. fcts
00153 
00154 int binomial(int n, int p) {
00155    int n1=1;
00156    if(p!=0) {
00157 
00158      if((n-p)<p){ p=n-p;};
00159      int n2=n;
00160      for(int i=1;i<=p;i++){ n1=n1*n2/i; n2--;};
00161     }
00162  
00163     return(n1);
00164 }/*binomials*/
00165 
00166 
00167 template<class C>
00168 polynomial< C ,with<Bernstein> > bbasis(C co, int n, int deg)
00169   {
00170     polynomial< C ,with<Bernstein> > ti(co*C(binomial(n,deg)),deg,0), t("1-x0"), tj(C(1),0,0); 
00171 
00172     for(int i=0; i<n-deg; i++){ mul(tj,t); }; 
00173 
00174     mul(ti, tj);
00175     return(ti);
00176   }/*BB basis polynomials*/
00177 
00178 
00179 template<class C>
00180 void pow( polynomial< C ,with<Bernstein> > & poly, int n)
00181   {
00182     polynomial< C ,with<Bernstein> > t=poly;
00183 
00184     for(int i=1; i<n; i++){ mul(poly, t); }; 
00185   }/*nth power of a bbpoly*/
00186 
00187 
00188 
00189 template<class C>
00190 polynomial< C ,with<Bernstein> >  seq2b( Seq<C>  coeffseq )
00191   { 
00192     int n=coeffseq.size()-1;
00193     polynomial<  C ,with<Bernstein> > pol, pi;
00194 
00195     for(int i=0; i<=n; i++ ){
00196       pi=bbasis(coeffseq[i],n,i); 
00197       add( pol , pi);}
00198 
00199     return(pol);
00200   };/*coeff vector -> bbpoly*/
00201 
00202 
00203 template<class C>
00204 C pint( polynomial<  C ,with<MonomialTensor> >  poly)
00205   {
00206     int n=SIZE;
00207     C res=C(0);     
00208 
00209     for(int i=0; i<n; i++ ){
00210       res += REP[i]/C(i+1) ;}
00211 
00212     return(res);
00213   };/*poly integration on (0,1)*/
00214 
00215     
00216 template<class C>
00217 C min_coeff( polynomial< C ,with<Bernstein> > poly){
00218 
00219   C  min =REP[0];
00220 
00221   for ( unsigned i = 1; i < SIZE; i++ ){
00222     if( min > REP[i] ){ 
00223       min = REP[i];
00224     }
00225 
00226   }
00227   return min;
00228 };/*minimal coeff*/
00229 
00230 
00231 template<class C>
00232 C max_coeff( polynomial< C ,with<Bernstein> > poly){
00233 
00234   C max =REP[0];
00235 
00236   for ( unsigned i = 1; i < SIZE; i++ ){
00237     if( max < REP[i] ){ 
00238       max = REP[i];
00239     }
00240 
00241   }
00242   return max;
00243 };/*max coeff*/
00244 
00245 
00246 
00247 template<class C>
00248 C abs_max_coeff( polynomial< C ,with<Bernstein> > poly){
00249   C min =min_coeff(poly);
00250   C max =max_coeff(poly);
00251 
00252   if( max*max < min*min){ max =C(-1)*min; };
00253  
00254   return max;
00255 };/* max abs coeff*/
00256 
00257 
00258 
00259 template<class C>
00260 Seq<C> corner_values(polynomial< C ,with<Bernstein> > p){
00261   C c00,c10,c01,c11;
00262 
00263   eval( c00, p.rep(),  vec(C(0),C(0)) );
00264   eval( c10, p.rep(),  vec(C(1),C(0)) );
00265   eval( c01, p.rep(),  vec(C(0),C(1)) );
00266   eval( c11, p.rep(),  vec(C(1),C(1)) );
00267   Seq<C> cval; cval<<c00<<c10<<c01<<c11;
00268   return(cval);
00269 
00270 };
00271 
00273 //arc fcts
00274 
00275 template<class C>
00276 std::vector<C> approx(polynomial< C ,with<Bernstein> > poly , std::vector<C> ep1, std::vector<C> ep2, int MTH){
00277   polynomial< C ,with<MonomialTensor> > xt, yt, res, monp, mp("x0-x0^2");
00278   polynomial< C ,with<Bernstein> > bxt, byt; 
00279     C t, I; 
00280     Seq<C> sols;
00281     std::vector<C> movec;
00282    
00283     if (ep2.size()==0 ){movec=ep1;}
00284     else{ 
00285  tensor::assign(monp.rep(),poly.rep());
00286    
00287  Seq<C> endptsx,endptsy;
00288 
00289     endptsx<<ep2[0]<<ep1[0];
00290     endptsy<<ep2[1]<<ep1[1];
00291     bxt=seq2b(endptsx); tensor::assign(xt.rep(),bxt.rep());
00292     byt=seq2b(endptsy); tensor::assign(yt.rep(),byt.rep());
00293        
00294     Seq<polynomial< C ,with<MonomialTensor> > > param; param<<xt<<yt;
00295 
00296     tensor::eval(res, monp.rep(), param, 1);
00297   
00298 
00299    
00300   C c0,c1;
00301   tensor::eval(c0,res.rep(),C(0));
00302   tensor::eval(c1,res.rep(),C(1));
00303   mul(res,mp);
00304 
00305    I=pint(res);
00306    C u=C(15)*I-C(0.75)*(c0+c1);
00307    sols=solve2(c0+c1-C(2)*u,C(2)*(u-c0),c0);
00308  
00309 
00310    if(is_unit1(sols[0]))
00311      {t=sols[0]; movec=vec(ep1[0]*t+ep2[0]*(C(1)-t),ep1[1]*t+ep2[1]*(C(1)-t));}
00312    else if(is_unit1(sols[1]))
00313      {t=sols[1]; movec=vec(ep1[0]*t+ep2[0]*(C(1)-t),ep1[1]*t+ep2[1]*(C(1)-t));}
00314    else{movec=std::vector<C>();}
00315 
00316     }
00317  
00318    return movec;
00319   }; /*approx bivar bbpoly along line*/
00320 
00321 
00322 template<class C>
00323 arc_rep< C >  median( box_rep< C > box, Seq<std::vector<C> > list, int MTH ){
00324   std::vector<C> endp[2], mpts[3];
00325   int s=0;
00326   arc_rep<C> medc;
00327   for(int i=0; i<3; i++){mpts[i]=std::vector<C>();};
00328 
00329   
00330   mpts[0]=approx(box.poly, list[0], list[1], MTH);
00331   mpts[1]=approx(box.poly, list[2], list[3], MTH);
00332  
00333     if( mpts[0].size()!=0 && mpts[1].size()!=0 ){
00334      
00335       std::vector<C> mid, rvec, v; 
00336       mid=v_div(v_plus(mpts[0],mpts[1]),C(2));
00337       rvec=rotl(v_minus(mpts[0],mpts[1])); 
00338        C bval[4]; 
00339     
00340        if(!is_small(rvec[0])){  
00341          bval[0]=rvec[1]*C(-1)*mid[0]/rvec[0]+mid[1];
00342          bval[1]=rvec[1]*(C(1)-mid[0])/rvec[0]+mid[1];}else{ bval[0]=C(-1); bval[1]=C(-1);}
00343 
00344        if(!is_small(rvec[1])){ 
00345          bval[2]=rvec[0]*C(-1)*mid[1]/rvec[1]+mid[0];
00346          bval[3]=rvec[0]*(C(1)-mid[1])/rvec[1]+mid[0];}else{ bval[2]=C(-1); bval[3]=C(-1);}
00347    
00348        if(is_unit1(bval[0]) && s<2){endp[s]= vec(C(0),bval[0]); s++;};
00349        if(is_unit1(bval[1]) && s<2){endp[s]= vec(C(1),bval[1]); s++;};
00350        if(is_unit1(bval[2]) && s<2){endp[s]= vec(bval[2],C(0)); s++;};
00351        if(is_unit1(bval[3]) && s<2){endp[s]= vec(bval[3],C(1)); s++;};
00352        mpts[2]=mpts[1]; 
00353   
00354        if(s==2){ mpts[1]=approx(box.poly, endp[0], endp[1], MTH);}
00355 
00356        if(mpts[1].size()!=0){ medc=arc_rep<C>(mpts);}else{medc=arc_rep<C>();}
00357        //else{ for(int i=0; i<3; i++){mpts[i]= vec(C(-1),C(-1));};}
00358       
00359     }else{ medc=arc_rep<C>(); };
00360 
00361 
00362     //std::cout <<"med: "<<mpts[0]<<" "<<mpts[1]<<" "<<mpts[2]<<std::endl;
00363    
00364      return(medc);
00365      }; /*median arc computation*/
00366 
00367 
00368   template<class C>
00369   void circ_is( std::vector<C> * ispts, arc_rep<C> c1, arc_rep<C> c2){
00370 
00371     Seq<C> eh1= c1.arc_eq(), eh2= c2.arc_eq(), sx, a;    
00372     C Aeh,Ceh; 
00373    if(!is_small(eh1[1]) && !is_small(eh1[2]) && !is_small(eh2[1]) && !is_small(eh2[2])){
00374 
00375     if(is_small(eh1[0]) && is_small(eh2[0])){
00376 
00377       Aeh=(C(-1)*eh2[1])/(eh2[2]); 
00378       Ceh=(C(-1)*eh2[3])/(eh2[2]);
00379       sx[0]=(C(-1)*eh1[2]*Ceh-eh1[3])/(eh1[2]*Aeh+eh1[1]);
00380         
00381           ispts[0]=vec(sx[0],Aeh*sx[0]+Ceh);
00382           ispts[1]=vec(sx[0],Aeh*sx[0]+Ceh);
00383       }
00384     else{
00385       if(!is_small(eh1[0]) && !is_small(eh2[0])){
00386         eh1=eh1/eh1[0];
00387         eh2=eh2/eh2[0];
00388 
00389         Aeh=(eh2[1]-eh1[1])/(eh1[2]-eh2[2]);
00390         Ceh=(eh2[3]-eh1[3])/(eh1[2]-eh2[2]);}   
00391       else{
00392         if(is_small(eh1[0]) && !is_small(eh2[0])){a=eh1; eh1=eh2; eh2=a;};
00393       
00394         eh1=eh1/eh1[0];
00395 
00396         Aeh=(C(-1)*eh2[1])/(eh2[2]); 
00397         Ceh=(C(-1)*eh2[3])/(eh2[2]);};
00398  
00399 
00400 
00401       sx=solve2(C(1)+Aeh*Aeh, C(2)*Aeh*Ceh+eh1[1]+eh1[2]*Aeh, Ceh*Ceh+eh1[2]*Ceh+eh1[3]);
00402          
00403       ispts[0]=vec(sx[0],Aeh*sx[0]+Ceh);
00404       ispts[1]=vec(sx[1],Aeh*sx[1]+Ceh); 
00405     
00406     }
00407    }
00408    else{  ispts[0]=vec(C(-1),C(-1));
00409      ispts[1]=vec(C(-1),C(-1)); 
00410    };
00411 };/*cicrcle intersections (numerical behaviour!)*/
00412 
00413 
00414   template<class C>  
00415  bool is_arc(arc_rep<C> c){
00416     if( c.pts[0].size()!=0 && c.pts[1].size()!=0 && c.pts[2].size()!=0 ){return true;}
00417   else{return false;}
00418 };
00419 
00420 
00421 template<class C> 
00422  bool is_infatarc(std::vector<C> p, arc_rep<C> c1, arc_rep<C> c2){
00423   if(is_arc(c1)&&is_arc(c2)){
00424     if( crossrat(c1.pts[2],c1.pts[1],c1.pts[0],p)[1]*crossrat(c2.pts[2],c2.pts[1],c2.pts[0],p)[1]<C(0))
00425       {return true;}else{return false;}
00426   }
00427   else{return false;}
00428 };
00429 
00430 
00431 template<class C> 
00432 Seq< std::vector<C> > extpts(arc_rep<C> mc1, C r1, arc_rep<C> mc2, C r2){
00433 
00434   arc_rep<C>
00435   c1p=mc1.offset(r1),c1m=mc1.offset(C(-1)*r1),
00436   c2p=mc2.offset(r2),c2m=mc2.offset(C(-1)*r2);
00437   std::vector<C> p[2];
00438   Seq< std::vector<C> > ispts;
00439 
00440   if(is_arc(c1p) && is_arc(c2p)){ circ_is(p,c1p,c2p); if(is_unit2(p[0])){ispts<<p[0];}; if(is_unit2(p[1])){ispts<<p[1];};};
00441   if(is_arc(c1p) && is_arc(c2m)){ circ_is(p,c1p,c2m); if(is_unit2(p[0])){ispts<<p[0];}; if(is_unit2(p[1])){ispts<<p[1];};};
00442   if(is_arc(c1m) && is_arc(c2m)){ circ_is(p,c1m,c2m); if(is_unit2(p[0])){ispts<<p[0];}; if(is_unit2(p[1])){ispts<<p[1];};};
00443   if(is_arc(c1m) && is_arc(c2p)){ circ_is(p,c1m,c2p); if(is_unit2(p[0])){ispts<<p[0];}; if(is_unit2(p[1])){ispts<<p[1];};};
00444  
00445   if(is_arc(c1p) && is_infatarc(c1p.pts[0],c2m,c2p)){ispts<<c1p.pts[0];};
00446   if(is_arc(c1p) && is_infatarc(c1p.pts[2],c2m,c2p)){ispts<<c1p.pts[2];};
00447   if(is_arc(c1m) && is_infatarc(c1m.pts[0],c2m,c2p)){ispts<<c1m.pts[0];};
00448   if(is_arc(c1m) && is_infatarc(c1m.pts[2],c2m,c2p)){ispts<<c1m.pts[2];};
00449 
00450   if(is_arc(c2p) && is_infatarc(c2p.pts[0],c1m,c1p)){ispts<<c2p.pts[0];};
00451   if(is_arc(c2p) && is_infatarc(c2p.pts[2],c1m,c1p)){ispts<<c2p.pts[2];};
00452   if(is_arc(c2m) && is_infatarc(c2m.pts[0],c1m,c1p)){ispts<<c2m.pts[0];};
00453   if(is_arc(c2m) && is_infatarc(c2m.pts[2],c1m,c1p)){ispts<<c2m.pts[2];};
00454  
00455   if(is_arc(c1p) && is_infatarc(c1p.critpt(0),c2m,c2p)){ispts<<c1p.critpt(0);}; 
00456   if(is_arc(c1p) && is_infatarc(c1p.critpt(1),c2m,c2p)){ispts<<c1p.critpt(1);}; 
00457   if(is_arc(c1m) && is_infatarc(c1m.critpt(0),c2m,c2p)){ispts<<c1m.critpt(0);}; 
00458   if(is_arc(c1m) && is_infatarc(c1m.critpt(1),c2m,c2p)){ispts<<c1m.critpt(1);}; 
00459 
00460   if(is_arc(c2p) && is_infatarc(c2p.critpt(0),c1m,c1p)){ispts<<c2p.critpt(0);}; 
00461   if(is_arc(c2p) && is_infatarc(c2p.critpt(1),c1m,c1p)){ispts<<c2p.critpt(1);}; 
00462   if(is_arc(c2m) && is_infatarc(c2m.critpt(0),c1m,c1p)){ispts<<c2m.critpt(0);}; 
00463   if(is_arc(c2m) && is_infatarc(c2m.critpt(1),c1m,c1p)){ispts<<c2m.critpt(1);};
00464 
00465   
00466   Seq< std::vector<C> > cpts; cpts<<vec(C(0),C(0));cpts<<vec(C(0),C(1));cpts<<vec(C(1),C(0));cpts<<vec(C(1),C(1));
00467 
00468   for(unsigned i=0; i<cpts.size(); i++){ if( is_infatarc(cpts[i],c1m,c1p) && 
00469                                               is_infatarc(cpts[i],c2m,c2p) ){ ispts<<cpts[i]; };
00470     }; 
00471  
00472   return ispts;
00473 };
00474 
00475 template<class C> 
00476 domain<C> minmax_box(Seq<std::vector<C> > ispts, domain<C> box){
00477   domain<C> newbox=box;
00478   //  C minx, maxx, miny, maxy; 
00479   if(ispts.size()==0) {
00480     newbox=domain<C>(int(0));
00481   } else {
00482   int k=0;
00483   C minx=ispts[0][0], maxx=ispts[0][0], miny=ispts[0][1], maxy=ispts[0][1]; 
00484   for(unsigned i=0; i<ispts.size(); i++){
00485       if(k==0){ minx=ispts[i][0]; maxx=ispts[i][0]; miny=ispts[i][1]; maxy=ispts[i][1]; k++;};
00486       if(ispts[i][0]>maxx){maxx=ispts[i][0];};
00487       if(ispts[i][0]<minx){minx=ispts[i][0];};
00488       if(ispts[i][1]>maxy){maxy=ispts[i][1];};
00489       if(ispts[i][1]<miny){miny=ispts[i][1];};
00490   }; 
00491 
00492   Interval<C>  Jx( minx, maxx), Jy( miny, maxy) ;
00493    if((maxx-minx)>C(0) && (maxy-miny)>C(0))
00494      { newbox= domain<C>( box.I[0].m + box.delta()[0]*Jx ,  box.I[1].m + box.delta()[1]*Jy ); };
00495   }
00496   return(newbox);
00497 };/*minmax box for the arc  intersection points and arc end points*/
00498 
00499 
00500 //end of arc_rep fcts
00501
include/realroot/fatarcs_fcts.hpp
