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>
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 **/
24 //namespace Geom{
25 using namespace Geom;
26 using namespace std;
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 }
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 }
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 }
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 }
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 }
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 }*/
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 }
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
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++){
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);
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?
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
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 }
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
197 */
198 D2<Piecewise<SBasis> >
199 Geom::tan2(SBasis const &angle, double tol, unsigned order){
200 return tan2(Piecewise<SBasis>(angle), tol, order);
201 }
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
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 }
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
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);
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;
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]);
241 r_eqn1 = -(a*x+b*y);
242 r_eqn2 = Linear(1.)-(a*a+b*b);
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]
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];
260 a[k] = Linear(a0,a1);
261 b[k] = Linear(b0,b1);
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 }
268 //our candidate is:
269 D2<SBasis> unitV;
270 unitV[0] = b;
271 unitV[1] = -a;
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 }
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
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 }
314 /** returns a function giving the arclength at each point in M.
315 \param M the Element.
316 \param tol the maximum error allowed.
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 }
328 /** returns a function giving the arclength at each point in M.
329 \param M the Element.
330 \param tol the maximum error allowed.
332 */
333 Piecewise<SBasis>
334 Geom::arcLengthSb(D2<SBasis> const &M, double tol){
335 return arcLengthSb(Piecewise<D2<SBasis> >(M), tol);
336 }
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
353 /** returns a function giving the curvature at each point in M.
354 \param M the Element.
355 \param tol the maximum error allowed.
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 }
371 /** returns a function giving the curvature at each point in M.
372 \param M the Element.
373 \param tol the maximum error allowed.
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 }
392 //=================================================================
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
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);
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 }
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
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 }
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 }
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
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);
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. */
467 gsl_integration_qag (&F, 0, 1, 0, tol, 20,
468 GSL_INTEG_GAUSS21, w, &quad_result, &err);
470 abs_error += err;
471 result += quad_result;
472 }
474 /** Calculates the length of a D2<SBasis> through gsl integration.
475 \param s the Element.
476 \param tol the maximum error allowed.
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.
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 }
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.
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.
513 * Returned values:
514 0 for normal execution;
515 2 if area is zero, meaning centroid is meaningless.
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.
535 area = atmp / 2;
536 if (atmp != 0) {
537 centroid = centroid_tmp / (3 * atmp);
538 return 0;
539 }
540 return 2;
541 }
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 */
557 static OptInterval
558 find_bounds_for_lambda0(double aa0,double aa1,double cc0,double cc1,
559 int insist_on_speeds_signs){
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;
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 }
585 static
586 std::vector<double>
587 solve_lambda0(double a0,double a1,double c0,double c1,
588 int insist_on_speeds_signs){
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 );
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 }
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;
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;
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 }
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 }
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 }
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 }
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 }
755 //}; // namespace
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 :