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