src/2geom/sbasis-geometric.cpp

   1 #include <2geom/sbasis-geometric.h>
   2 #include <2geom/sbasis.h>
   3 #include <2geom/sbasis-math.h>
   4 //#include <2geom/solver.h>
   5 #include <2geom/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 /**
  31  * Return a list of doubles that appear in both a and b to within error tol
  32  * a, b, vector of double
  33  * tol tolerance
  34  */
  35 static vector<double>
  36 vect_intersect(vector<double> const &a, vector<double> const &b, double tol=0.){
  37     vector<double> inter;
  38     unsigned i=0,j=0;
  39     while ( i<a.size() && j<b.size() ){
  40         if (fabs(a[i]-b[j])<tol){
  41             inter.push_back(a[i]);
  42             i+=1;
  43             j+=1;
  44         }else if (a[i]<b[j]){
  45             i+=1;
  46         }else if (a[i]>b[j]){
  47             j+=1;
  48         }
  49     }
  50     return inter;
  51 }
  52
  53 //------------------------------------------------------------------------------
  54 static SBasis divide_by_sk(SBasis const &a, int k) {
  55     if ( k>=(int)a.size()){
  56         //make sure a is 0?
  57         return SBasis();
  58     }
  59     if(k < 0) return shift(a,-k);
  60     SBasis c;
  61     c.insert(c.begin(), a.begin()+k, a.end());
  62     return c;
  63 }
  64
  65 static SBasis divide_by_t0k(SBasis const &a, int k) {
  66     if(k < 0) {
  67         SBasis c = Linear(0,1);
  68         for (int i=2; i<=-k; i++){
  69             c*=c;
  70         }
  71         c*=a;
  72         return(c);
  73     }else{
  74         SBasis c = Linear(1,0);
  75         for (int i=2; i<=k; i++){
  76             c*=c;
  77         }
  78         c*=a;
  79         return(divide_by_sk(c,k));
  80     }
  81 }
  82
  83 static SBasis divide_by_t1k(SBasis const &a, int k) {
  84     if(k < 0) {
  85         SBasis c = Linear(1,0);
  86         for (int i=2; i<=-k; i++){
  87             c*=c;
  88         }
  89         c*=a;
  90         return(c);
  91     }else{
  92         SBasis c = Linear(0,1);
  93         for (int i=2; i<=k; i++){
  94             c*=c;
  95         }
  96         c*=a;
  97         return(divide_by_sk(c,k));
  98     }
  99 }
 100
 101 static D2<SBasis> RescaleForNonVanishingEnds(D2<SBasis> const &MM, double ZERO=1.e-4){
 102     D2<SBasis> M = MM;
 103     //TODO: divide by all the s at once!!!
 104     while ((M[0].size()>0||M[1].size()>0) &&
 105            fabs(M[0].at0())<ZERO &&
 106            fabs(M[1].at0())<ZERO &&
 107            fabs(M[0].at1())<ZERO &&
 108            fabs(M[1].at1())<ZERO){
 109         M[0] = divide_by_sk(M[0],1);
 110         M[1] = divide_by_sk(M[1],1);
 111     }
 112     while ((M[0].size()>0||M[1].size()>0) &&
 113            fabs(M[0].at0())<ZERO && fabs(M[1].at0())<ZERO){
 114         M[0] = divide_by_t0k(M[0],1);
 115         M[1] = divide_by_t0k(M[1],1);
 116     }
 117     while ((M[0].size()>0||M[1].size()>0) &&
 118            fabs(M[0].at1())<ZERO && fabs(M[1].at1())<ZERO){
 119         M[0] = divide_by_t1k(M[0],1);
 120         M[1] = divide_by_t1k(M[1],1);
 121     }
 122     return M;
 123 }
 124
 125 /*static D2<SBasis> RescaleForNonVanishing(D2<SBasis> const &MM, double ZERO=1.e-4){
 126     std::vector<double> levels;
 127     levels.push_back(-ZERO);
 128     levels.push_back(ZERO);
 129     //std::vector<std::vector<double> > mr = multi_roots(MM, levels);
 130     }*/
 131
 132
 133 //=================================================================
 134 //TODO: what's this for?!?!
 135 Piecewise<D2<SBasis> >
 136 Geom::cutAtRoots(Piecewise<D2<SBasis> > const &M, double ZERO){
 137     vector<double> rts;
 138     for (unsigned i=0; i<M.size(); i++){
 139         vector<double> seg_rts = roots((M.segs[i])[0]);
 140         seg_rts = vect_intersect(seg_rts, roots((M.segs[i])[1]), ZERO);
 141         Linear mapToDom = Linear(M.cuts[i],M.cuts[i+1]);
 142         for (unsigned r=0; r<seg_rts.size(); r++){
 143             seg_rts[r]= mapToDom(seg_rts[r]);
 144         }
 145         rts.insert(rts.end(),seg_rts.begin(),seg_rts.end());
 146     }
 147     return partition(M,rts);
 148 }
 149
 150 /** Return a function which gives the angle of vect at each point.
 151  \param vect a piecewise parameteric curve.
 152  \param tol the maximum error allowed.
 153  \param order the maximum degree to use for approximation
 154
 155 */
 156 Piecewise<SBasis>
 157 Geom::atan2(Piecewise<D2<SBasis> > const &vect, double tol, unsigned order){
 158     Piecewise<SBasis> result;
 159     Piecewise<D2<SBasis> > v = cutAtRoots(vect,tol);
 160     result.cuts.push_back(v.cuts.front());
 161     for (unsigned i=0; i<v.size(); i++){
 162
 163         D2<SBasis> vi = RescaleForNonVanishingEnds(v.segs[i]);
 164         SBasis x=vi[0], y=vi[1];
 165         Piecewise<SBasis> angle;
 166         angle = divide (x*derivative(y)-y*derivative(x), x*x+y*y, tol, order);
 167
 168         //TODO: I don't understand this - sign.
 169         angle = integral(-angle);
 170         Point vi0 = vi.at0();
 171         angle += -std::atan2(vi0[1],vi0[0]) - angle[0].at0();
 172         //TODO: deal with 2*pi jumps form one seg to the other...
 173         //TODO: not exact at t=1 because of the integral.
 174         //TODO: force continuity?
 175
 176         angle.setDomain(Interval(v.cuts[i],v.cuts[i+1]));
 177         result.concat(angle);
 178     }
 179     return result;
 180 }
 181 /** Return a function which gives the angle of vect at each point.
 182  \param vect a piecewise parameteric curve.
 183  \param tol the maximum error allowed.
 184  \param order the maximum degree to use for approximation
 185
 186 */
 187 Piecewise<SBasis>
 188 Geom::atan2(D2<SBasis> const &vect, double tol, unsigned order){
 189     return atan2(Piecewise<D2<SBasis> >(vect),tol,order);
 190 }
 191
 192 /** tan2 is the pseudo-inverse of atan2.  It takes an angle and returns a unit_vector that points in the direction of angle.
 193  \param angle a piecewise function of angle wrt t.
 194  \param tol the maximum error allowed.
 195  \param order the maximum degree to use for approximation
 196
 197 */
 198 D2<Piecewise<SBasis> >
 199 Geom::tan2(SBasis const &angle, double tol, unsigned order){
 200     return tan2(Piecewise<SBasis>(angle), tol, order);
 201 }
 202
 203 /** tan2 is the pseudo-inverse of atan2.  It takes an angle and returns a unit_vector that points in the direction of angle.
 204  \param angle a piecewise function of angle wrt t.
 205  \param tol the maximum error allowed.
 206  \param order the maximum degree to use for approximation
 207
 208 */
 209 D2<Piecewise<SBasis> >
 210 Geom::tan2(Piecewise<SBasis> const &angle, double tol, unsigned order){
 211     return D2<Piecewise<SBasis> >(cos(angle, tol, order), sin(angle, tol, order));
 212 }
 213
 214 /** Return a Piecewise<D2<SBasis> > which points in the same direction as V_in, but has unit_length.
 215  \param V_in the original path.
 216  \param tol the maximum error allowed.
 217  \param order the maximum degree to use for approximation
 218
 219 unitVector(x,y) is computed as (b,-a) where a and b are solutions of:
 220      ax+by=0 (eqn1)   and   a^2+b^2=1 (eqn2)
 221 */
 222 Piecewise<D2<SBasis> >
 223 Geom::unitVector(D2<SBasis> const &V_in, double tol, unsigned order){
 224     //TODO: Handle vanishing vectors...
 225     // -This approach is numerically bad. Find a stable way to rescale V_in to have non vanishing ends.
 226     // -This done, unitVector will have jumps at zeros: fill the gaps with arcs of circles.
 227     D2<SBasis> V = RescaleForNonVanishingEnds(V_in);
 228
 229     if (V[0].empty() && V[1].empty())
 230         return Piecewise<D2<SBasis> >(D2<SBasis>(Linear(1),SBasis()));
 231     SBasis x = V[0], y = V[1];
 232     SBasis r_eqn1, r_eqn2;
 233
 234     Point v0 = unit_vector(V.at0());
 235     Point v1 = unit_vector(V.at1());
 236     SBasis a = SBasis(order+1, Linear(0.));
 237     a[0] = Linear(-v0[1],-v1[1]);
 238     SBasis b = SBasis(order+1, Linear(0.));
 239     b[0] = Linear( v0[0], v1[0]);
 240
 241     r_eqn1 = -(a*x+b*y);
 242     r_eqn2 = Linear(1.)-(a*a+b*b);
 243
 244     for (unsigned k=1; k<=order; k++){
 245         double r0  = (k<r_eqn1.size())? r_eqn1.at(k).at0() : 0;
 246         double r1  = (k<r_eqn1.size())? r_eqn1.at(k).at1() : 0;
 247         double rr0 = (k<r_eqn2.size())? r_eqn2.at(k).at0() : 0;
 248         double rr1 = (k<r_eqn2.size())? r_eqn2.at(k).at1() : 0;
 249         double a0,a1,b0,b1;// coeffs in a[k] and b[k]
 250
 251         //the equations to solve at this point are:
 252         // a0*x(0)+b0*y(0)=r0 & 2*a0*a(0)+2*b0*b(0)=rr0
 253         //and
 254         // a1*x(1)+b1*y(1)=r1 & 2*a1*a(1)+2*b1*b(1)=rr1
 255         a0 = r0/dot(v0,V.at0())*v0[0]-rr0/2*v0[1];
 256         b0 = r0/dot(v0,V.at0())*v0[1]+rr0/2*v0[0];
 257         a1 = r1/dot(v1,V.at1())*v1[0]-rr1/2*v1[1];
 258         b1 = r1/dot(v1,V.at1())*v1[1]+rr1/2*v1[0];
 259
 260         a[k] = Linear(a0,a1);
 261         b[k] = Linear(b0,b1);
 262
 263         //TODO: use "incremental" rather than explicit formulas.
 264         r_eqn1 = -(a*x+b*y);
 265         r_eqn2 = Linear(1)-(a*a+b*b);
 266     }
 267
 268     //our candidate is:
 269     D2<SBasis> unitV;
 270     unitV[0] =  b;
 271     unitV[1] = -a;
 272
 273     //is it good?
 274     double rel_tol = std::max(1.,std::max(V_in[0].tailError(0),V_in[1].tailError(0)))*tol;
 275     if (r_eqn1.tailError(order)>rel_tol || r_eqn2.tailError(order)>tol){
 276         //if not: subdivide and concat results.
 277         Piecewise<D2<SBasis> > unitV0, unitV1;
 278         unitV0 = unitVector(compose(V,Linear(0,.5)),tol,order);
 279         unitV1 = unitVector(compose(V,Linear(.5,1)),tol,order);
 280         unitV0.setDomain(Interval(0.,.5));
 281         unitV1.setDomain(Interval(.5,1.));
 282         unitV0.concat(unitV1);
 283         return(unitV0);
 284     }else{
 285         //if yes: return it as pw.
 286         Piecewise<D2<SBasis> > result;
 287         result=(Piecewise<D2<SBasis> >)unitV;
 288         return result;
 289     }
 290 }
 291
 292 /** Return a Piecewise<D2<SBasis> > which points in the same direction as V_in, but has unit_length.
 293  \param V_in the original path.
 294  \param tol the maximum error allowed.
 295  \param order the maximum degree to use for approximation
 296
 297 unitVector(x,y) is computed as (b,-a) where a and b are solutions of:
 298      ax+by=0 (eqn1)   and   a^2+b^2=1 (eqn2)
 299 */
 300 Piecewise<D2<SBasis> >
 301 Geom::unitVector(Piecewise<D2<SBasis> > const &V, double tol, unsigned order){
 302     Piecewise<D2<SBasis> > result;
 303     Piecewise<D2<SBasis> > VV = cutAtRoots(V);
 304     result.cuts.push_back(VV.cuts.front());
 305     for (unsigned i=0; i<VV.size(); i++){
 306         Piecewise<D2<SBasis> > unit_seg;
 307         unit_seg = unitVector(VV.segs[i],tol, order);
 308         unit_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1]));
 309         result.concat(unit_seg);
 310     }
 311     return result;
 312 }
 313
 314 /** returns a function giving the arclength at each point in M.
 315  \param M the Element.
 316  \param tol the maximum error allowed.
 317
 318 */
 319 Piecewise<SBasis>
 320 Geom::arcLengthSb(Piecewise<D2<SBasis> > const &M, double tol){
 321     Piecewise<D2<SBasis> > dM = derivative(M);
 322     Piecewise<SBasis> dMlength = sqrt(dot(dM,dM),tol,3);
 323     Piecewise<SBasis> length = integral(dMlength);
 324     length-=length.segs.front().at0();
 325     return length;
 326 }
 327
 328 /** returns a function giving the arclength at each point in M.
 329  \param M the Element.
 330  \param tol the maximum error allowed.
 331
 332 */
 333 Piecewise<SBasis>
 334 Geom::arcLengthSb(D2<SBasis> const &M, double tol){
 335     return arcLengthSb(Piecewise<D2<SBasis> >(M), tol);
 336 }
 337
 338 #if 0
 339 double
 340 Geom::length(D2<SBasis> const &M,
 341                  double tol){
 342     Piecewise<SBasis> length = arcLengthSb(M, tol);
 343     return length.segs.back().at1();
 344 }
 345 double
 346 Geom::length(Piecewise<D2<SBasis> > const &M,
 347                  double tol){
 348     Piecewise<SBasis> length = arcLengthSb(M, tol);
 349     return length.segs.back().at1();
 350 }
 351 #endif
 352
 353 /** returns a function giving the curvature at each point in M.
 354  \param M the Element.
 355  \param tol the maximum error allowed.
 356
 357  Todo:
 358  * claimed incomplete.  Check.
 359 */
 360 Piecewise<SBasis>
 361 Geom::curvature(D2<SBasis> const &M, double tol) {
 362     D2<SBasis> dM=derivative(M);
 363     Piecewise<SBasis> result;
 364     Piecewise<D2<SBasis> > unitv = unitVector(dM,tol);
 365     Piecewise<SBasis> dMlength = dot(Piecewise<D2<SBasis> >(dM),unitv);
 366     Piecewise<SBasis> k = cross(derivative(unitv),unitv);
 367     k = divide(k,dMlength,tol,3);
 368     return(k);
 369 }
 370
 371 /** returns a function giving the curvature at each point in M.
 372  \param M the Element.
 373  \param tol the maximum error allowed.
 374
 375  Todo:
 376  * claimed incomplete.  Check.
 377 */
 378 Piecewise<SBasis>
 379 Geom::curvature(Piecewise<D2<SBasis> > const &V, double tol){
 380     Piecewise<SBasis> result;
 381     Piecewise<D2<SBasis> > VV = cutAtRoots(V);
 382     result.cuts.push_back(VV.cuts.front());
 383     for (unsigned i=0; i<VV.size(); i++){
 384         Piecewise<SBasis> curv_seg;
 385         curv_seg = curvature(VV.segs[i],tol);
 386         curv_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1]));
 387         result.concat(curv_seg);
 388     }
 389     return result;
 390 }
 391
 392 //=================================================================
 393
 394 /** Reparameterise M to have unit speed.
 395  \param M the Element.
 396  \param tol the maximum error allowed.
 397  \param order the maximum degree to use for approximation
 398
 399 */
 400 Piecewise<D2<SBasis> >
 401 Geom::arc_length_parametrization(D2<SBasis> const &M,
 402                            unsigned order,
 403                            double tol){
 404     Piecewise<D2<SBasis> > u;
 405     u.push_cut(0);
 406
 407     Piecewise<SBasis> s = arcLengthSb(Piecewise<D2<SBasis> >(M),tol);
 408     for (unsigned i=0; i < s.size();i++){
 409         double t0=s.cuts[i],t1=s.cuts[i+1];
 410         D2<SBasis> sub_M = compose(M,Linear(t0,t1));
 411         D2<SBasis> sub_u;
 412         for (unsigned dim=0;dim<2;dim++){
 413             SBasis sub_s = s.segs[i];
 414             sub_s-=sub_s.at0();
 415             sub_s/=sub_s.at1();
 416             sub_u[dim]=compose_inverse(sub_M[dim],sub_s, order, tol);
 417         }
 418         u.push(sub_u,s(t1));
 419     }
 420     return u;
 421 }
 422
 423 /** Reparameterise M to have unit speed.
 424  \param M the Element.
 425  \param tol the maximum error allowed.
 426  \param order the maximum degree to use for approximation
 427
 428 */
 429 Piecewise<D2<SBasis> >
 430 Geom::arc_length_parametrization(Piecewise<D2<SBasis> > const &M,
 431                                  unsigned order,
 432                                  double tol){
 433     Piecewise<D2<SBasis> > result;
 434     for (unsigned i=0; i<M.size(); i++ ){
 435         Piecewise<D2<SBasis> > uniform_seg=arc_length_parametrization(M[i],order,tol);
 436         result.concat(uniform_seg);
 437     }
 438     return(result);
 439 }
 440
 441 #include <gsl/gsl_integration.h>
 442 static double sb_length_integrating(double t, void* param) {
 443     SBasis* pc = (SBasis*)param;
 444     return sqrt((*pc)(t));
 445 }
 446
 447 /** Calculates the length of a D2<SBasis> through gsl integration.
 448  \param B the Element.
 449  \param tol the maximum error allowed.
 450  \param result variable to be incremented with the length of the path
 451  \param abs_error variable to be incremented with the estimated error
 452
 453 If you only want the length, this routine may be faster/more accurate.
 454 */
 455 void Geom::length_integrating(D2<SBasis> const &B, double &result, double &abs_error, double tol) {
 456     D2<SBasis> dB = derivative(B);
 457     SBasis dB2 = dot(dB, dB);
 458
 459     gsl_function F;
 460     gsl_integration_workspace * w
 461         = gsl_integration_workspace_alloc (20);
 462     F.function = &sb_length_integrating;
 463     F.params = (void*)&dB2;
 464     double quad_result, err;
 465     /* We could probably use the non adaptive code here if we removed any cusps first. */
 466
 467     gsl_integration_qag (&F, 0, 1, 0, tol, 20,
 468                          GSL_INTEG_GAUSS21, w, &quad_result, &err);
 469
 470     abs_error += err;
 471     result += quad_result;
 472 }
 473
 474 /** Calculates the length of a D2<SBasis> through gsl integration.
 475  \param s the Element.
 476  \param tol the maximum error allowed.
 477
 478 If you only want the total length, this routine faster and more accurate than constructing an arcLengthSb.
 479 */
 480 double
 481 Geom::length(D2<SBasis> const &s,
 482                  double tol){
 483     double result = 0;
 484     double abs_error = 0;
 485     length_integrating(s, result, abs_error, tol);
 486     return result;
 487 }
 488 /** Calculates the length of a Piecewise<D2<SBasis> > through gsl integration.
 489  \param s the Element.
 490  \param tol the maximum error allowed.
 491
 492 If you only want the total length, this routine faster and more accurate than constructing an arcLengthSb.
 493 */
 494 double
 495 Geom::length(Piecewise<D2<SBasis> > const &s,
 496                  double tol){
 497     double result = 0;
 498     double abs_error = 0;
 499     for (unsigned i=0; i < s.size();i++){
 500         length_integrating(s[i], result, abs_error, tol);
 501     }
 502     return result;
 503 }
 504
 505 /**
 506  * Centroid using sbasis integration.
 507  \param p the Element.
 508  \param centroid on return contains the centroid of the shape
 509  \param area on return contains the signed area of the shape.
 510
 511 This approach uses green's theorem to compute the area and centroid using integrals.  For curved shapes this is much faster than converting to polyline.  Note that without an uncross operation the output is not the absolute area.
 512
 513  * Returned values:
 514     0 for normal execution;
 515     2 if area is zero, meaning centroid is meaningless.
 516
 517  */
 518 unsigned Geom::centroid(Piecewise<D2<SBasis> > const &p, Point& centroid, double &area) {
 519     Point centroid_tmp(0,0);
 520     double atmp = 0;
 521     for(unsigned i = 0; i < p.size(); i++) {
 522         SBasis curl = dot(p[i], rot90(derivative(p[i])));
 523         SBasis A = integral(curl);
 524         D2<SBasis> C = integral(multiply(curl, p[i]));
 525         atmp += A.at1() - A.at0();
 526         centroid_tmp += C.at1()- C.at0(); // first moment.
 527     }
 528 // join ends
 529     centroid_tmp *= 2;
 530     Point final = p[p.size()-1].at1(), initial = p[0].at0();
 531     const double ai = cross(final, initial);
 532     atmp += ai;
 533     centroid_tmp += (final + initial)*ai; // first moment.
 534
 535     area = atmp / 2;
 536     if (atmp != 0) {
 537         centroid = centroid_tmp / (3 * atmp);
 538         return 0;
 539     }
 540     return 2;
 541 }
 542
 543 /**
 544  * Find cubics with prescribed curvatures at both ends.
 545  *
 546  *  this requires to solve a system of the form
 547  *
 548  * \f[
 549  *  \lambda_1 = a_0 \lambda_0^2 + c_0
 550  *  \lambda_0 = a_1 \lambda_1^2 + c_1
 551  * \f]
 552  *
 553  * which is a deg 4 equation in lambda 0.
 554  * Below are basic functions dedicated to solving this assuming a0 and a1 !=0.
 555  */
 556
 557 static OptInterval
 558 find_bounds_for_lambda0(double aa0,double aa1,double cc0,double cc1,
 559     int insist_on_speeds_signs){
 560
 561     double a0=aa0,a1=aa1,c0=cc0,c1=cc1;
 562     Interval result;
 563     bool flip = a1<0;
 564     if (a1<0){a1=-a1; c1=-c1;}
 565     if (a0<0){a0=-a0; c0=-c0;}
 566     double a = (a0<a1 ? a0 : a1);
 567     double c = (c0<c1 ? c0 : c1);
 568     double delta = 1-4*a*c;
 569     if ( delta < 0 )
 570         return OptInterval();//return empty interval
 571     double lambda_max = (1+std::sqrt(delta))/2/a;
 572
 573     result = Interval(c,lambda_max);
 574     if (flip)
 575         result *= -1;
 576     if (insist_on_speeds_signs == 1){
 577         if (result.max() < 0)//Caution: setMin with max<new min...
 578             return OptInterval();//return empty interval
 579         result.setMin(0);
 580     }
 581     result = Interval(result.min()-.1,result.max()+.1);//just in case all our approx. were exact...
 582     return result;
 583 }
 584
 585 static
 586 std::vector<double>
 587 solve_lambda0(double a0,double a1,double c0,double c1,
 588              int insist_on_speeds_signs){
 589
 590     SBasis p(3, Linear());
 591     p[0] = Linear( a1*c0*c0+c1, a1*a0*(a0+ 2*c0) +a1*c0*c0 +c1 -1  );
 592     p[1] = Linear( -a1*a0*(a0+2*c0), -a1*a0*(3*a0+2*c0) );
 593     p[2] = Linear( a1*a0*a0 );
 594
 595     OptInterval domain = find_bounds_for_lambda0(a0,a1,c0,c1,insist_on_speeds_signs);
 596     if ( !domain )
 597         return std::vector<double>();
 598     p = compose(p,Linear(domain->min(),domain->max()));
 599     std::vector<double>rts = roots(p);
 600     for (unsigned i=0; i<rts.size(); i++){
 601         rts[i] = domain->min() + rts[i] * domain->extent();
 602     }
 603     return rts;
 604 }
 605
 606 /**
 607 * \brief returns the cubics fitting direction and curvature of a given
 608 * input curve at two points.
 609 *
 610 * The input can be the
 611 *    value, speed, and acceleration
 612 * or
 613 *    value, speed, and cross(acceleration,speed)
 614 * of the original curve at the both ends.
 615 * (the second is often technically usefull, as it avoids unnecessary division by |v|^2)
 616 * Recall that K=1/R=cross(acceleration,speed)/|speed|^3.
 617 *
 618 * Moreover, a 7-th argument 'insist_on_speed_signs' can be supplied to select solutions:
 619 * If insist_on_speed_signs == 1, only consider solutions where speeds at both ends are positively
 620 * proportional to the given ones.
 621 * If insist_on_speed_signs == 0, allow speeds to point in the opposite direction (both at the same time)
 622 * If insist_on_speed_signs == -1, allow speeds to point in both direction independantly.
 623 */
 624 std::vector<D2<SBasis> >
 625 Geom::cubics_fitting_curvature(Point const &M0,   Point const &M1,
 626                          Point const &dM0,  Point const &dM1,
 627                          double d2M0xdM0,  double d2M1xdM1,
 628                          int insist_on_speed_signs,
 629                          double epsilon){
 630     std::vector<D2<SBasis> > result;
 631
 632     //speed of cubic bezier will be lambda0*dM0 and lambda1*dM1,
 633     //with lambda0 and lambda1 s.t. curvature at both ends is the same
 634     //as the curvature of the given curve.
 635     std::vector<double> lambda0,lambda1;
 636     double dM1xdM0=cross(dM1,dM0);
 637     if (fabs(dM1xdM0)<epsilon){
 638         if (fabs(d2M0xdM0)<epsilon || fabs(d2M1xdM1)<epsilon){
 639             return result;
 640         }
 641         double lbda02 = 6.*cross(M1-M0,dM0)/d2M0xdM0;
 642         double lbda12 =-6.*cross(M1-M0,dM1)/d2M1xdM1;
 643         if (lbda02<0 || lbda12<0){
 644             return result;
 645         }
 646         lambda0.push_back(std::sqrt(lbda02) );
 647         lambda1.push_back(std::sqrt(lbda12) );
 648     }else{
 649         //solve:  lambda1 = a0 lambda0^2 + c0
 650         //        lambda0 = a1 lambda1^2 + c1
 651         double a0,c0,a1,c1;
 652         a0 = -d2M0xdM0/2/dM1xdM0;
 653         c0 =  3*cross(M1-M0,dM0)/dM1xdM0;
 654         a1 = -d2M1xdM1/2/dM1xdM0;
 655         c1 = -3*cross(M1-M0,dM1)/dM1xdM0;
 656
 657         if (fabs(a0)<epsilon){
 658             lambda1.push_back( c0 );
 659             lambda0.push_back( a1*c0*c0 + c1 );
 660         }else if (fabs(a1)<epsilon){
 661             lambda0.push_back( c1 );
 662             lambda1.push_back( a0*c1*c1 + c0 );
 663         }else{
 664             //find lamda0 by solving a deg 4 equation d0+d1*X+...+d4*X^4=0
 665             double a[5];
 666             a[0] = c1+a1*c0*c0;
 667             a[1] = -1;
 668             a[2] = 2*a1*a0*c0;
 669             a[3] = 0;
 670             a[4] = a1*a0*a0;
 671             //vector<double> solns=solve_poly(a,4);
 672             vector<double> solns=solve_lambda0(a0,a1,c0,c1,insist_on_speed_signs);
 673             for (unsigned i=0;i<solns.size();i++){
 674                 double lbda0=solns[i];
 675                 double lbda1=c0+a0*lbda0*lbda0;
 676                 //is this solution pointing in the + direction at both ends?
 677                 if (lbda0>=0. && lbda1>=0.){
 678                     lambda0.push_back( lbda0);
 679                     lambda1.push_back( lbda1);
 680                 }
 681                 //is this solution pointing in the - direction at both ends?
 682                 else if (lbda0<=0. && lbda1<=0. && insist_on_speed_signs<=0){
 683                     lambda0.push_back( lbda0);
 684                     lambda1.push_back( lbda1);
 685                 }
 686                 //ok,this solution is pointing in the + and - directions.
 687                 else if (insist_on_speed_signs<0){
 688                     lambda0.push_back( lbda0);
 689                     lambda1.push_back( lbda1);
 690                 }
 691             }
 692         }
 693     }
 694
 695     for (unsigned i=0; i<lambda0.size(); i++){
 696         Point V0 = lambda0[i]*dM0;
 697         Point V1 = lambda1[i]*dM1;
 698         D2<SBasis> cubic;
 699         for(unsigned dim=0;dim<2;dim++){
 700             SBasis c(2, Linear());
 701             c[0] = Linear(M0[dim],M1[dim]);
 702             c[1] = Linear( M0[dim]-M1[dim]+V0[dim],
 703                            -M0[dim]+M1[dim]-V1[dim]);
 704             cubic[dim] = c;
 705         }
 706 #if 0
 707            Piecewise<SBasis> k = curvature(result);
 708            double dM0_l = dM0.length();
 709            double dM1_l = dM1.length();
 710            g_warning("Target radii: %f, %f", dM0_l*dM0_l*dM0_l/d2M0xdM0,dM1_l*dM1_l*dM1_l/d2M1xdM1);
 711            g_warning("Obtained radii: %f, %f",1/k.valueAt(0),1/k.valueAt(1));
 712 #endif
 713         result.push_back(cubic);
 714     }
 715     return(result);
 716 }
 717
 718 std::vector<D2<SBasis> >
 719 Geom::cubics_fitting_curvature(Point const &M0,   Point const &M1,
 720                          Point const &dM0,  Point const &dM1,
 721                          Point const &d2M0, Point const &d2M1,
 722                          int insist_on_speed_signs,
 723                          double epsilon){
 724     double d2M0xdM0 = cross(d2M0,dM0);
 725     double d2M1xdM1 = cross(d2M1,dM1);
 726     return cubics_fitting_curvature(M0,M1,dM0,dM1,d2M0xdM0,d2M1xdM1,insist_on_speed_signs,epsilon);
 727 }
 728
 729 std::vector<D2<SBasis> >
 730 Geom::cubics_with_prescribed_curvature(Point const &M0,   Point const &M1,
 731                                  Point const &dM0,  Point const &dM1,
 732                                  double k0, double k1,
 733                                  int insist_on_speed_signs,
 734                                  double epsilon){
 735     double length;
 736     length = dM0.length();
 737     double d2M0xdM0 = k0*length*length*length;
 738     length = dM1.length();
 739     double d2M1xdM1 = k1*length*length*length;
 740     return cubics_fitting_curvature(M0,M1,dM0,dM1,d2M0xdM0,d2M1xdM1,insist_on_speed_signs,epsilon);
 741 }
 742
 743
 744 /**
 745 * \brief returns all the parameter values of A whose tangent passes through P.
 746 */
 747 std::vector<double> find_tangents(Point P, D2<SBasis> const &A) {
 748     SBasis crs (cross(A - P, derivative(A)));
 749     crs = shift(crs*Linear(-1, 0)*Linear(-1, 0), -2); // We know that there is a double root at t=0 so we divide out t^2
 750 // JFB points out that this is equivalent to (t-1)^2 followed by a divide by s^2 (shift)
 751     return roots(crs);
 752 }
 753
 754
 755 //}; // namespace
 756
 757
 758 /*
 759   Local Variables:
 760   mode:c++
 761   c-file-style:"stroustrup"
 762   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 763   indent-tabs-mode:nil
 764   fill-column:99
 765   End:
 766 */
 767 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :