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 an 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 //unitVector(x,y) is computed as (b,-a) where a and b are solutions of:
 161 //     ax+by=0 (eqn1)   and   a^2+b^2=1 (eqn2)
 162 Piecewise<D2<SBasis> >
 163 Geom::unitVector(D2<SBasis> const &V_in, double tol, unsigned order){
 164     D2<SBasis> V = RescaleForNonVanishingEnds(V_in);
 165     if (V[0].empty() && V[1].empty())
 166         return Piecewise<D2<SBasis> >(D2<SBasis>(Linear(1),SBasis()));
 167     SBasis x = V[0], y = V[1], a, b;
 168     SBasis r_eqn1, r_eqn2;
 169
 170     Point v0 = unit_vector(V.at0());
 171     Point v1 = unit_vector(V.at1());
 172     a.push_back(Linear(-v0[1],-v1[1]));
 173     b.push_back(Linear( v0[0], v1[0]));
 174
 175     r_eqn1 = -(a*x+b*y);
 176     r_eqn2 = Linear(1.)-(a*a+b*b);
 177
 178     for (unsigned k=1; k<=order; k++){
 179         double r0  = (k<r_eqn1.size())? r_eqn1.at(k).at0() : 0;
 180         double r1  = (k<r_eqn1.size())? r_eqn1.at(k).at1() : 0;
 181         double rr0 = (k<r_eqn2.size())? r_eqn2.at(k).at0() : 0;
 182         double rr1 = (k<r_eqn2.size())? r_eqn2.at(k).at1() : 0;
 183         double a0,a1,b0,b1;// coeffs in a[k] and b[k]
 184
 185         //the equations to solve at this point are:
 186         // a0*x(0)+b0*y(0)=r0 & 2*a0*a(0)+2*b0*b(0)=rr0
 187         //and
 188         // a1*x(1)+b1*y(1)=r1 & 2*a1*a(1)+2*b1*b(1)=rr1
 189         a0 = r0/dot(v0,V(0))*v0[0]-rr0/2*v0[1];
 190         b0 = r0/dot(v0,V(0))*v0[1]+rr0/2*v0[0];
 191         a1 = r1/dot(v1,V(1))*v1[0]-rr1/2*v1[1];
 192         b1 = r1/dot(v1,V(1))*v1[1]+rr1/2*v1[0];
 193
 194         a.push_back(Linear(a0,a1));
 195         b.push_back(Linear(b0,b1));
 196         //TODO: use "incremental" rather than explicit formulas.
 197         r_eqn1 = -(a*x+b*y);
 198         r_eqn2 = Linear(1)-(a*a+b*b);
 199     }
 200
 201     //our candidat is:
 202     D2<SBasis> unitV;
 203     unitV[0] =  b;
 204     unitV[1] = -a;
 205
 206     //is it good?
 207     double rel_tol = max(1.,max(V_in[0].tailError(0),V_in[1].tailError(0)))*tol;
 208
 209     if (r_eqn1.tailError(order)>rel_tol || r_eqn2.tailError(order)>tol){
 210         //if not: subdivide and concat results.
 211         Piecewise<D2<SBasis> > unitV0, unitV1;
 212         unitV0 = unitVector(compose(V,Linear(0,.5)),tol,order);
 213         unitV1 = unitVector(compose(V,Linear(.5,1)),tol,order);
 214         unitV0.setDomain(Interval(0.,.5));
 215         unitV1.setDomain(Interval(.5,1.));
 216         unitV0.concat(unitV1);
 217         return(unitV0);
 218     }else{
 219         //if yes: return it as pw.
 220         Piecewise<D2<SBasis> > result;
 221         result=(Piecewise<D2<SBasis> >)unitV;
 222         return result;
 223     }
 224 }
 225
 226 Piecewise<D2<SBasis> >
 227 Geom::unitVector(Piecewise<D2<SBasis> > const &V, double tol, unsigned order){
 228     Piecewise<D2<SBasis> > result;
 229     Piecewise<D2<SBasis> > VV = cutAtRoots(V);
 230     result.cuts.push_back(VV.cuts.front());
 231     for (unsigned i=0; i<VV.size(); i++){
 232         Piecewise<D2<SBasis> > unit_seg;
 233         unit_seg = unitVector(VV.segs[i],tol, order);
 234         unit_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1]));
 235         result.concat(unit_seg);
 236     }
 237     return result;
 238 }
 239
 240 Piecewise<SBasis>
 241 Geom::arcLengthSb(Piecewise<D2<SBasis> > const &M, double tol){
 242     Piecewise<D2<SBasis> > dM = derivative(M);
 243     Piecewise<SBasis> dMlength = sqrt(dot(dM,dM),tol,3);
 244     Piecewise<SBasis> length = integral(dMlength);
 245     length-=length.segs.front().at0();
 246     return length;
 247 }
 248 Piecewise<SBasis>
 249 Geom::arcLengthSb(D2<SBasis> const &M, double tol){
 250     return arcLengthSb(Piecewise<D2<SBasis> >(M), tol);
 251 }
 252
 253 double
 254 Geom::length(D2<SBasis> const &M,
 255                  double tol){
 256     Piecewise<SBasis> length = arcLengthSb(M, tol);
 257     return length.segs.back().at1();
 258 }
 259 double
 260 Geom::length(Piecewise<D2<SBasis> > const &M,
 261                  double tol){
 262     Piecewise<SBasis> length = arcLengthSb(M, tol);
 263     return length.segs.back().at1();
 264 }
 265
 266
 267 // incomplete.
 268 Piecewise<SBasis>
 269 Geom::curvature(D2<SBasis> const &M, double tol) {
 270     D2<SBasis> dM=derivative(M);
 271     Piecewise<SBasis> result;
 272     Piecewise<D2<SBasis> > unitv = unitVector(dM,tol);
 273     Piecewise<SBasis> dMlength = dot(Piecewise<D2<SBasis> >(dM),unitv);
 274     Piecewise<SBasis> k = cross(derivative(unitv),unitv);
 275     k = divide(k,dMlength,tol,3);
 276     return(k);
 277 }
 278
 279 Piecewise<SBasis>
 280 Geom::curvature(Piecewise<D2<SBasis> > const &V, double tol){
 281     Piecewise<SBasis> result;
 282     Piecewise<D2<SBasis> > VV = cutAtRoots(V);
 283     result.cuts.push_back(VV.cuts.front());
 284     for (unsigned i=0; i<VV.size(); i++){
 285         Piecewise<SBasis> curv_seg;
 286         curv_seg = curvature(VV.segs[i],tol);
 287         curv_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1]));
 288         result.concat(curv_seg);
 289     }
 290     return result;
 291 }
 292
 293 //=================================================================
 294
 295 Piecewise<D2<SBasis> >
 296 Geom::arc_length_parametrization(D2<SBasis> const &M,
 297                            unsigned order,
 298                            double tol){
 299     Piecewise<D2<SBasis> > u;
 300     u.push_cut(0);
 301
 302     Piecewise<SBasis> s = arcLengthSb(Piecewise<D2<SBasis> >(M),tol);
 303     for (unsigned i=0; i < s.size();i++){
 304         double t0=s.cuts[i],t1=s.cuts[i+1];
 305         D2<SBasis> sub_M = compose(M,Linear(t0,t1));
 306         D2<SBasis> sub_u;
 307         for (unsigned dim=0;dim<2;dim++){
 308             SBasis sub_s = s.segs[i];
 309             sub_s-=sub_s.at0();
 310             sub_s/=sub_s.at1();
 311             sub_u[dim]=compose_inverse(sub_M[dim],sub_s, order, tol);
 312         }
 313         u.push(sub_u,s(t1));
 314     }
 315     return u;
 316 }
 317
 318 Piecewise<D2<SBasis> >
 319 Geom::arc_length_parametrization(Piecewise<D2<SBasis> > const &M,
 320                                  unsigned order,
 321                                  double tol){
 322     Piecewise<D2<SBasis> > result;
 323     for (unsigned i=0; i<M.size(); i++ ){
 324         Piecewise<D2<SBasis> > uniform_seg=arc_length_parametrization(M[i],order,tol);
 325         result.concat(uniform_seg);
 326     }
 327     return(result);
 328 }
 329
 330 /** centroid using sbasis integration.
 331  * This approach uses green's theorem to compute the area and centroid using integrals.  For curved
 332  * shapes this is much faster than converting to polyline.
 333
 334  * Returned values:
 335     0 for normal execution;
 336     2 if area is zero, meaning centroid is meaningless.
 337
 338  * Copyright Nathan Hurst 2006
 339  */
 340
 341 unsigned Geom::centroid(Piecewise<D2<SBasis> > const &p, Point& centroid, double &area) {
 342     Point centroid_tmp(0,0);
 343     double atmp = 0;
 344     for(unsigned i = 0; i < p.size(); i++) {
 345         SBasis curl = dot(p[i], rot90(derivative(p[i])));
 346         SBasis A = integral(curl);
 347         D2<SBasis> C = integral(multiply(curl, p[i]));
 348         atmp += A.at1() - A.at0();
 349         centroid_tmp += C.at1()- C.at0(); // first moment.
 350     }
 351 // join ends
 352     centroid_tmp *= 2;
 353     Point final = p[p.size()-1].at1(), initial = p[0].at0();
 354     const double ai = cross(final, initial);
 355     atmp += ai;
 356     centroid_tmp += (final + initial)*ai; // first moment.
 357
 358     area = atmp / 2;
 359     if (atmp != 0) {
 360         centroid = centroid_tmp / (3 * atmp);
 361         return 0;
 362     }
 363     return 2;
 364 }
 365
 366 //}; // namespace
 367
 368
 369 /*
 370   Local Variables:
 371   mode:c++
 372   c-file-style:"stroustrup"
 373   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 374   indent-tabs-mode:nil
 375   fill-column:99
 376   End:
 377 */
 378 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :