src/2geom/sbasis-geometric.cpp

   1 #include "sbasis-geometric.h"
   2 #include "sbasis.h"
   3 #include "sbasis-math.h"
   4 //#include "solver.h"
   5 #include "sbasis-geometric.h"
   6
   7 /** Geometric operators on D2<SBasis> (1D->2D).
   8  * Copyright 2007 JF Barraud
   9  * Copyright 2007 N Hurst
  10  *
  11  * The functions defined in this header related to 2d geometric operations such as arc length,
  12  * unit_vector, curvature, and centroid.  Most are built on top of unit_vector, which takes an
  13  * arbitrary D2 and returns a D2 with unit length with the same direction.
  14  *
  15  * Todo/think about:
  16  *  arclength D2 -> sbasis (giving arclength function)
  17  *  does uniform_speed return natural parameterisation?
  18  *  integrate sb2d code from normal-bundle
  19  *  angle(md<2>) -> sbasis (gives angle from vector - discontinuous?)
  20  *  osculating circle center?
  21  *
  22  **/
  23
  24 //namespace Geom{
  25 using namespace Geom;
  26 using namespace std;
  27
  28 //Some utils first.
  29 //TODO: remove this!!
  30 static vector<double>
  31 vect_intersect(vector<double> const &a, vector<double> const &b, double tol=0.){
  32     vector<double> inter;
  33     unsigned i=0,j=0;
  34     while ( i<a.size() && j<b.size() ){
  35         if (fabs(a[i]-b[j])<tol){
  36             inter.push_back(a[i]);
  37             i+=1;
  38             j+=1;
  39         }else if (a[i]<b[j]){
  40             i+=1;
  41         }else if (a[i]>b[j]){
  42             j+=1;
  43         }
  44     }
  45     return inter;
  46 }
  47
  48 static SBasis divide_by_sk(SBasis const &a, int k) {
  49     assert( k<(int)a.size());
  50     if(k < 0) return shift(a,-k);
  51     SBasis c;
  52     c.insert(c.begin(), a.begin()+k, a.end());
  53     return c;
  54 }
  55
  56 static SBasis divide_by_t0k(SBasis const &a, int k) {
  57     if(k < 0) {
  58         SBasis c = Linear(0,1);
  59         for (int i=2; i<=-k; i++){
  60             c*=c;
  61         }
  62         c*=a;
  63         return(c);
  64     }else{
  65         SBasis c = Linear(1,0);
  66         for (int i=2; i<=k; i++){
  67             c*=c;
  68         }
  69         c*=a;
  70         return(divide_by_sk(c,k));
  71     }
  72 }
  73
  74 static SBasis divide_by_t1k(SBasis const &a, int k) {
  75     if(k < 0) {
  76         SBasis c = Linear(1,0);
  77         for (int i=2; i<=-k; i++){
  78             c*=c;
  79         }
  80         c*=a;
  81         return(c);
  82     }else{
  83         SBasis c = Linear(0,1);
  84         for (int i=2; i<=k; i++){
  85             c*=c;
  86         }
  87         c*=a;
  88         return(divide_by_sk(c,k));
  89     }
  90 }
  91
  92 static D2<SBasis> RescaleForNonVanishingEnds(D2<SBasis> const &MM, double ZERO=1.e-4){
  93     D2<SBasis> M = MM;
  94     //TODO: divide by all the s at once!!!
  95     while (fabs(M[0].at0())<ZERO &&
  96            fabs(M[1].at0())<ZERO &&
  97            fabs(M[0].at1())<ZERO &&
  98            fabs(M[1].at1())<ZERO){
  99         M[0] = divide_by_sk(M[0],1);
 100         M[1] = divide_by_sk(M[1],1);
 101     }
 102     while (fabs(M[0].at0())<ZERO && fabs(M[1].at0())<ZERO){
 103         M[0] = divide_by_t0k(M[0],1);
 104         M[1] = divide_by_t0k(M[1],1);
 105     }
 106     while (fabs(M[0].at1())<ZERO && fabs(M[1].at1())<ZERO){
 107         M[0] = divide_by_t1k(M[0],1);
 108         M[1] = divide_by_t1k(M[1],1);
 109     }
 110     return M;
 111 }
 112
 113 //=================================================================
 114 //TODO: what's this for?!?!
 115 Piecewise<D2<SBasis> >
 116 Geom::cutAtRoots(Piecewise<D2<SBasis> > const &M, double ZERO){
 117     vector<double> rts;
 118     for (unsigned i=0; i<M.size(); i++){
 119         vector<double> seg_rts = roots((M.segs[i])[0]);
 120         seg_rts = vect_intersect(seg_rts, roots((M.segs[i])[1]), ZERO);
 121         Linear mapToDom = Linear(M.cuts[i],M.cuts[i+1]);
 122         for (unsigned r=0; r<seg_rts.size(); r++){
 123             seg_rts[r]= mapToDom(seg_rts[r]);
 124         }
 125         rts.insert(rts.end(),seg_rts.begin(),seg_rts.end());
 126     }
 127     return partition(M,rts);
 128 }
 129
 130 Piecewise<SBasis>
 131 Geom::atan2(Piecewise<D2<SBasis> > const &vect, double tol, unsigned order){
 132     Piecewise<SBasis> result;
 133     Piecewise<D2<SBasis> > v = cutAtRoots(vect);
 134     result.cuts.push_back(v.cuts.front());
 135     for (unsigned i=0; i<v.size(); i++){
 136
 137         D2<SBasis> vi = RescaleForNonVanishingEnds(v.segs[i]);
 138         SBasis x=vi[0], y=vi[1];
 139         Piecewise<SBasis> angle;
 140         angle = divide (x*derivative(y)-y*derivative(x), x*x+y*y, tol, order);
 141
 142         //TODO: I don't understand this - sign.
 143         angle = integral(-angle);
 144         Point vi0 = vi.at0();
 145         angle += -std::atan2(vi0[1],vi0[0]) - angle[0].at0();
 146         //TODO: deal with 2*pi jumps form one seg to the other...
 147         //TODO: not exact at t=1 because of the integral.
 148         //TODO: force continuity?
 149
 150         angle.setDomain(Interval(v.cuts[i],v.cuts[i+1]));
 151         result.concat(angle);
 152     }
 153     return result;
 154 }
 155 Piecewise<SBasis>
 156 Geom::atan2(D2<SBasis> const &vect, double tol, unsigned order){
 157     return atan2(Piecewise<D2<SBasis> >(vect),tol,order);
 158 }
 159
 160 //tan2 is the pseudo-inverse of atan2.  It takes an angle and returns a unit_vector that points in the direction of angle.
 161 D2<Piecewise<SBasis> >
 162 Geom::tan2(SBasis const &angle, double tol, unsigned order){
 163     return tan2(Piecewise<SBasis>(angle), tol, order);
 164 }
 165
 166 D2<Piecewise<SBasis> >
 167 Geom::tan2(Piecewise<SBasis> const &angle, double tol, unsigned order){
 168     return D2<Piecewise<SBasis> >(cos(angle, tol, order), sin(angle, tol, order));
 169 }
 170
 171 //unitVector(x,y) is computed as (b,-a) where a and b are solutions of:
 172 //     ax+by=0 (eqn1)   and   a^2+b^2=1 (eqn2)
 173 Piecewise<D2<SBasis> >
 174 Geom::unitVector(D2<SBasis> const &V_in, double tol, unsigned order){
 175     D2<SBasis> V = RescaleForNonVanishingEnds(V_in);
 176     if (V[0].empty() && V[1].empty())
 177         return Piecewise<D2<SBasis> >(D2<SBasis>(Linear(1),SBasis()));
 178     SBasis x = V[0], y = V[1], a, b;
 179     SBasis r_eqn1, r_eqn2;
 180
 181     Point v0 = unit_vector(V.at0());
 182     Point v1 = unit_vector(V.at1());
 183     a.push_back(Linear(-v0[1],-v1[1]));
 184     b.push_back(Linear( v0[0], v1[0]));
 185
 186     r_eqn1 = -(a*x+b*y);
 187     r_eqn2 = Linear(1.)-(a*a+b*b);
 188
 189     for (unsigned k=1; k<=order; k++){
 190         double r0  = (k<r_eqn1.size())? r_eqn1.at(k).at0() : 0;
 191         double r1  = (k<r_eqn1.size())? r_eqn1.at(k).at1() : 0;
 192         double rr0 = (k<r_eqn2.size())? r_eqn2.at(k).at0() : 0;
 193         double rr1 = (k<r_eqn2.size())? r_eqn2.at(k).at1() : 0;
 194         double a0,a1,b0,b1;// coeffs in a[k] and b[k]
 195
 196         //the equations to solve at this point are:
 197         // a0*x(0)+b0*y(0)=r0 & 2*a0*a(0)+2*b0*b(0)=rr0
 198         //and
 199         // a1*x(1)+b1*y(1)=r1 & 2*a1*a(1)+2*b1*b(1)=rr1
 200         a0 = r0/dot(v0,V(0))*v0[0]-rr0/2*v0[1];
 201         b0 = r0/dot(v0,V(0))*v0[1]+rr0/2*v0[0];
 202         a1 = r1/dot(v1,V(1))*v1[0]-rr1/2*v1[1];
 203         b1 = r1/dot(v1,V(1))*v1[1]+rr1/2*v1[0];
 204
 205         a.push_back(Linear(a0,a1));
 206         b.push_back(Linear(b0,b1));
 207         //TODO: use "incremental" rather than explicit formulas.
 208         r_eqn1 = -(a*x+b*y);
 209         r_eqn2 = Linear(1)-(a*a+b*b);
 210     }
 211
 212     //our candidate is:
 213     D2<SBasis> unitV;
 214     unitV[0] =  b;
 215     unitV[1] = -a;
 216
 217     //is it good?
 218     double rel_tol = std::max(1.,std::max(V_in[0].tailError(0),V_in[1].tailError(0)))*tol;
 219
 220     if (r_eqn1.tailError(order)>rel_tol || r_eqn2.tailError(order)>tol){
 221         //if not: subdivide and concat results.
 222         Piecewise<D2<SBasis> > unitV0, unitV1;
 223         unitV0 = unitVector(compose(V,Linear(0,.5)),tol,order);
 224         unitV1 = unitVector(compose(V,Linear(.5,1)),tol,order);
 225         unitV0.setDomain(Interval(0.,.5));
 226         unitV1.setDomain(Interval(.5,1.));
 227         unitV0.concat(unitV1);
 228         return(unitV0);
 229     }else{
 230         //if yes: return it as pw.
 231         Piecewise<D2<SBasis> > result;
 232         result=(Piecewise<D2<SBasis> >)unitV;
 233         return result;
 234     }
 235 }
 236
 237 Piecewise<D2<SBasis> >
 238 Geom::unitVector(Piecewise<D2<SBasis> > const &V, double tol, unsigned order){
 239     Piecewise<D2<SBasis> > result;
 240     Piecewise<D2<SBasis> > VV = cutAtRoots(V);
 241     result.cuts.push_back(VV.cuts.front());
 242     for (unsigned i=0; i<VV.size(); i++){
 243         Piecewise<D2<SBasis> > unit_seg;
 244         unit_seg = unitVector(VV.segs[i],tol, order);
 245         unit_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1]));
 246         result.concat(unit_seg);
 247     }
 248     return result;
 249 }
 250
 251 Piecewise<SBasis>
 252 Geom::arcLengthSb(Piecewise<D2<SBasis> > const &M, double tol){
 253     Piecewise<D2<SBasis> > dM = derivative(M);
 254     Piecewise<SBasis> dMlength = sqrt(dot(dM,dM),tol,3);
 255     Piecewise<SBasis> length = integral(dMlength);
 256     length-=length.segs.front().at0();
 257     return length;
 258 }
 259 Piecewise<SBasis>
 260 Geom::arcLengthSb(D2<SBasis> const &M, double tol){
 261     return arcLengthSb(Piecewise<D2<SBasis> >(M), tol);
 262 }
 263
 264 double
 265 Geom::length(D2<SBasis> const &M,
 266                  double tol){
 267     Piecewise<SBasis> length = arcLengthSb(M, tol);
 268     return length.segs.back().at1();
 269 }
 270 double
 271 Geom::length(Piecewise<D2<SBasis> > const &M,
 272                  double tol){
 273     Piecewise<SBasis> length = arcLengthSb(M, tol);
 274     return length.segs.back().at1();
 275 }
 276
 277
 278 // incomplete.
 279 Piecewise<SBasis>
 280 Geom::curvature(D2<SBasis> const &M, double tol) {
 281     D2<SBasis> dM=derivative(M);
 282     Piecewise<SBasis> result;
 283     Piecewise<D2<SBasis> > unitv = unitVector(dM,tol);
 284     Piecewise<SBasis> dMlength = dot(Piecewise<D2<SBasis> >(dM),unitv);
 285     Piecewise<SBasis> k = cross(derivative(unitv),unitv);
 286     k = divide(k,dMlength,tol,3);
 287     return(k);
 288 }
 289
 290 Piecewise<SBasis>
 291 Geom::curvature(Piecewise<D2<SBasis> > const &V, double tol){
 292     Piecewise<SBasis> result;
 293     Piecewise<D2<SBasis> > VV = cutAtRoots(V);
 294     result.cuts.push_back(VV.cuts.front());
 295     for (unsigned i=0; i<VV.size(); i++){
 296         Piecewise<SBasis> curv_seg;
 297         curv_seg = curvature(VV.segs[i],tol);
 298         curv_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1]));
 299         result.concat(curv_seg);
 300     }
 301     return result;
 302 }
 303
 304 //=================================================================
 305
 306 Piecewise<D2<SBasis> >
 307 Geom::arc_length_parametrization(D2<SBasis> const &M,
 308                            unsigned order,
 309                            double tol){
 310     Piecewise<D2<SBasis> > u;
 311     u.push_cut(0);
 312
 313     Piecewise<SBasis> s = arcLengthSb(Piecewise<D2<SBasis> >(M),tol);
 314     for (unsigned i=0; i < s.size();i++){
 315         double t0=s.cuts[i],t1=s.cuts[i+1];
 316         D2<SBasis> sub_M = compose(M,Linear(t0,t1));
 317         D2<SBasis> sub_u;
 318         for (unsigned dim=0;dim<2;dim++){
 319             SBasis sub_s = s.segs[i];
 320             sub_s-=sub_s.at0();
 321             sub_s/=sub_s.at1();
 322             sub_u[dim]=compose_inverse(sub_M[dim],sub_s, order, tol);
 323         }
 324         u.push(sub_u,s(t1));
 325     }
 326     return u;
 327 }
 328
 329 Piecewise<D2<SBasis> >
 330 Geom::arc_length_parametrization(Piecewise<D2<SBasis> > const &M,
 331                                  unsigned order,
 332                                  double tol){
 333     Piecewise<D2<SBasis> > result;
 334     for (unsigned i=0; i<M.size(); i++ ){
 335         Piecewise<D2<SBasis> > uniform_seg=arc_length_parametrization(M[i],order,tol);
 336         result.concat(uniform_seg);
 337     }
 338     return(result);
 339 }
 340
 341 /** centroid using sbasis integration.
 342  * This approach uses green's theorem to compute the area and centroid using integrals.  For curved
 343  * shapes this is much faster than converting to polyline.
 344
 345  * Returned values:
 346     0 for normal execution;
 347     2 if area is zero, meaning centroid is meaningless.
 348
 349  * Copyright Nathan Hurst 2006
 350  */
 351
 352 unsigned Geom::centroid(Piecewise<D2<SBasis> > const &p, Point& centroid, double &area) {
 353     Point centroid_tmp(0,0);
 354     double atmp = 0;
 355     for(unsigned i = 0; i < p.size(); i++) {
 356         SBasis curl = dot(p[i], rot90(derivative(p[i])));
 357         SBasis A = integral(curl);
 358         D2<SBasis> C = integral(multiply(curl, p[i]));
 359         atmp += A.at1() - A.at0();
 360         centroid_tmp += C.at1()- C.at0(); // first moment.
 361     }
 362 // join ends
 363     centroid_tmp *= 2;
 364     Point final = p[p.size()-1].at1(), initial = p[0].at0();
 365     const double ai = cross(final, initial);
 366     atmp += ai;
 367     centroid_tmp += (final + initial)*ai; // first moment.
 368
 369     area = atmp / 2;
 370     if (atmp != 0) {
 371         centroid = centroid_tmp / (3 * atmp);
 372         return 0;
 373     }
 374     return 2;
 375 }
 376
 377 //}; // namespace
 378
 379
 380 /*
 381   Local Variables:
 382   mode:c++
 383   c-file-style:"stroustrup"
 384   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 385   indent-tabs-mode:nil
 386   fill-column:99
 387   End:
 388 */
 389 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :