1 /*
2 This file is part of Kig, a KDE program for Interactive Geometry...
3 SPDX-FileCopyrightText: 2002 Maurizio Paolini <paolini@dmf.unicatt.it>
4
5 SPDX-License-Identifier: GPL-2.0-or-later
6 */
7
8 #include "conic-common.h"
9
10 #include <math.h>
11
12 #include "common.h"
13 #include "kigtransform.h"
14
15 #include <cmath>
16 #include <algorithm>
17
18 #include <config-kig.h>
19
20 #ifdef HAVE_IEEEFP_H
21 #include <ieeefp.h>
22 #endif
23
ConicCartesianData(const ConicPolarData & polardata)24 ConicCartesianData::ConicCartesianData(
25 const ConicPolarData& polardata
26 )
27 {
28 double ec = polardata.ecostheta0;
29 double es = polardata.esintheta0;
30 double p = polardata.pdimen;
31 double fx = polardata.focus1.x;
32 double fy = polardata.focus1.y;
33
34 double a = 1 - ec*ec;
35 double b = 1 - es*es;
36 double c = - 2*ec*es;
37 double d = - 2*p*ec;
38 double e = - 2*p*es;
39 double f = - p*p;
40
41 f += a*fx*fx + b*fy*fy + c*fx*fy - d*fx - e*fy;
42 d -= 2*a*fx + c*fy;
43 e -= 2*b*fy + c*fx;
44
45 coeffs[0] = a;
46 coeffs[1] = b;
47 coeffs[2] = c;
48 coeffs[3] = d;
49 coeffs[4] = e;
50 coeffs[5] = f;
51 }
52
ConicPolarData(const ConicCartesianData & cartdata)53 ConicPolarData::ConicPolarData( const ConicCartesianData& cartdata )
54 {
55 double a = cartdata.coeffs[0];
56 double b = cartdata.coeffs[1];
57 double c = cartdata.coeffs[2];
58 double d = cartdata.coeffs[3];
59 double e = cartdata.coeffs[4];
60 double f = cartdata.coeffs[5];
61
62 // 1. Compute theta (tilt of conic)
63 double theta = std::atan2(c, b - a)/2;
64
65 // now I should possibly add pi/2...
66 double costheta = std::cos(theta);
67 double sintheta = std::sin(theta);
68 // compute new coefficients (c should now be zero)
69 double aa = a*costheta*costheta + b*sintheta*sintheta - c*sintheta*costheta;
70 double bb = a*sintheta*sintheta + b*costheta*costheta + c*sintheta*costheta;
71 if (aa*bb < 0)
72 { // we have a hyperbola we need to check the correct orientation
73 double dd = d*costheta - e*sintheta;
74 double ee = d*sintheta + e*costheta;
75 double xc = - dd / ( 2*aa );
76 double yc = - ee / ( 2*bb );
77 double ff = f + aa*xc*xc + bb*yc*yc + dd*xc + ee*yc;
78 if (ff*aa > 0) // wrong orientation
79 {
80 if (theta > 0) theta -= M_PI/2;
81 else theta += M_PI/2;
82 costheta = cos(theta);
83 sintheta = sin(theta);
84 aa = a*costheta*costheta + b*sintheta*sintheta - c*sintheta*costheta;
85 bb = a*sintheta*sintheta + b*costheta*costheta + c*sintheta*costheta;
86 }
87 }
88 else
89 {
90 if ( std::fabs (bb) < std::fabs (aa) )
91 {
92 if (theta > 0) theta -= M_PI/2;
93 else theta += M_PI/2;
94 costheta = cos(theta);
95 sintheta = sin(theta);
96 aa = a*costheta*costheta + b*sintheta*sintheta - c*sintheta*costheta;
97 bb = a*sintheta*sintheta + b*costheta*costheta + c*sintheta*costheta;
98 }
99 }
100
101 double cc = 2*(a - b)*sintheta*costheta +
102 c*(costheta*costheta - sintheta*sintheta);
103 // cc should be zero!!! cout << "cc = " << cc << "\n";
104 double dd = d*costheta - e*sintheta;
105 double ee = d*sintheta + e*costheta;
106
107 a = aa;
108 b = bb;
109 c = cc;
110 d = dd;
111 e = ee;
112
113 // now b cannot be zero (otherwise conic is degenerate)
114 a /= b;
115 c /= b;
116 d /= b;
117 e /= b;
118 f /= b;
119 b = 1.0;
120
121 // 2. compute y coordinate of focuses
122
123 double yf = - e/2;
124
125 // new values:
126 f += yf*yf + e*yf;
127 e += 2*yf; // this should be zero!
128
129 // now: a > 0 -> ellipse
130 // a = 0 -> parabola
131 // a < 0 -> hyperbola
132
133 double eccentricity = sqrt(1.0 - a);
134
135 double sqrtdiscrim = sqrt(d*d - 4*a*f);
136 if (d < 0.0) sqrtdiscrim = -sqrtdiscrim;
137 double xf = (4*a*f - 4*f - d*d)/(d + eccentricity*sqrtdiscrim) / 2;
138
139 // 3. the focus needs to be rotated back into position
140 focus1.x = xf*costheta + yf*sintheta;
141 focus1.y = -xf*sintheta + yf*costheta;
142
143 // 4. final touch: the pdimen
144 pdimen = -sqrtdiscrim/2;
145
146 ecostheta0 = eccentricity*costheta;
147 esintheta0 = -eccentricity*sintheta;
148 if ( pdimen < 0)
149 {
150 pdimen = -pdimen;
151 ecostheta0 = -ecostheta0;
152 esintheta0 = -esintheta0;
153 }
154 }
155
calcConicThroughPoints(const std::vector<Coordinate> & points,const LinearConstraints c1,const LinearConstraints c2,const LinearConstraints c3,const LinearConstraints c4,const LinearConstraints c5)156 const ConicCartesianData calcConicThroughPoints (
157 const std::vector<Coordinate>& points,
158 const LinearConstraints c1,
159 const LinearConstraints c2,
160 const LinearConstraints c3,
161 const LinearConstraints c4,
162 const LinearConstraints c5 )
163 {
164 assert( 0 < points.size() && points.size() <= 5 );
165 // points is a vector of up to 5 points through which the conic is
166 // constrained.
167 // this routine should compute the coefficients in the cartesian equation
168 // a x^2 + b y^2 + c xy + d x + e y + f = 0
169 // they are defined up to a multiplicative factor.
170 // since we don't know (in advance) which one of them is nonzero, we
171 // simply keep all 6 parameters, obtaining a 5x6 linear system which
172 // we solve using gaussian elimination with complete pivoting
173 // If there are too few, then we choose some cool way to fill in the
174 // empty parts in the matrix according to the LinearConstraints
175 // given..
176
177 // 5 rows, 6 columns..
178 double row0[6];
179 double row1[6];
180 double row2[6];
181 double row3[6];
182 double row4[6];
183 double *matrix[5] = {row0, row1, row2, row3, row4};
184 double solution[6];
185 int scambio[6];
186 LinearConstraints constraints[] = {c1, c2, c3, c4, c5};
187
188 int numpoints = points.size();
189 int numconstraints = 5;
190
191 // fill in the matrix elements
192 for ( int i = 0; i < numpoints; ++i )
193 {
194 double xi = points[i].x;
195 double yi = points[i].y;
196 matrix[i][0] = xi*xi;
197 matrix[i][1] = yi*yi;
198 matrix[i][2] = xi*yi;
199 matrix[i][3] = xi;
200 matrix[i][4] = yi;
201 matrix[i][5] = 1.0;
202 }
203
204 for ( int i = 0; i < numconstraints; i++ )
205 {
206 if (numpoints >= 5) break; // don't add constraints if we have enough
207 for (int j = 0; j < 6; ++j) matrix[numpoints][j] = 0.0;
208 // force the symmetry axes to be
209 // parallel to the coordinate system (zero tilt): c = 0
210 if (constraints[i] == zerotilt) matrix[numpoints][2] = 1.0;
211 // force a parabola (if zerotilt): b = 0
212 if (constraints[i] == parabolaifzt) matrix[numpoints][1] = 1.0;
213 // force a circle (if zerotilt): a = b
214 if (constraints[i] == circleifzt) {
215 matrix[numpoints][0] = 1.0;
216 matrix[numpoints][1] = -1.0; }
217 // force an equilateral hyperbola: a + b = 0
218 if (constraints[i] == equilateral) {
219 matrix[numpoints][0] = 1.0;
220 matrix[numpoints][1] = 1.0; }
221 // force symmetry about y-axis: d = 0
222 if (constraints[i] == ysymmetry) matrix[numpoints][3] = 1.0;
223 // force symmetry about x-axis: e = 0
224 if (constraints[i] == xsymmetry) matrix[numpoints][4] = 1.0;
225
226 if (constraints[i] != noconstraint) ++numpoints;
227 }
228
229 if ( ! GaussianElimination( matrix, numpoints, 6, scambio ) )
230 return ConicCartesianData::invalidData();
231 // fine della fase di eliminazione
232 BackwardSubstitution( matrix, numpoints, 6, scambio, solution );
233
234 // now solution should contain the correct coefficients..
235 return ConicCartesianData( solution );
236 }
237
calcConicBFFP(const std::vector<Coordinate> & args,int type)238 const ConicPolarData calcConicBFFP(
239 const std::vector<Coordinate>& args,
240 int type )
241 {
242 assert( args.size() >= 2 && args.size() <= 3 );
243 assert( type == 1 || type == -1 );
244
245 ConicPolarData ret;
246
247 Coordinate f1 = args[0];
248 Coordinate f2 = args[1];
249 Coordinate d;
250 double eccentricity, d1, d2, dl;
251
252 Coordinate f2f1 = f2 - f1;
253 double f2f1l = f2f1.length();
254 ret.ecostheta0 = f2f1.x / f2f1l;
255 ret.esintheta0 = f2f1.y / f2f1l;
256
257 if ( args.size() == 3 )
258 {
259 d = args[2];
260 d1 = ( d - f1 ).length();
261 d2 = ( d - f2 ).length();
262 dl = fabs( d1 + type * d2 );
263 eccentricity = f2f1l/dl;
264 }
265 else
266 {
267 if ( type > 0 ) eccentricity = 0.7; else eccentricity = 2.0;
268 dl = f2f1l/eccentricity;
269 }
270
271 double rhomax = (dl + f2f1l) /2.0;
272
273 ret.ecostheta0 *= eccentricity;
274 ret.esintheta0 *= eccentricity;
275 ret.pdimen = type*(1 - eccentricity)*rhomax;
276 ret.focus1 = type == 1 ? f1 : f2;
277 return ret;
278 }
279
calcConicPolarLine(const ConicCartesianData & data,const Coordinate & cpole,bool & valid)280 const LineData calcConicPolarLine (
281 const ConicCartesianData& data,
282 const Coordinate& cpole,
283 bool& valid )
284 {
285 double x = cpole.x;
286 double y = cpole.y;
287 double a = data.coeffs[0];
288 double b = data.coeffs[1];
289 double c = data.coeffs[2];
290 double d = data.coeffs[3];
291 double e = data.coeffs[4];
292 double f = data.coeffs[5];
293
294 double alpha = 2*a*x + c*y + d;
295 double beta = c*x + 2*b*y + e;
296 double gamma = d*x + e*y + 2*f;
297
298 double normsq = alpha*alpha + beta*beta;
299
300 if (normsq < 1e-10) // line at infinity
301 {
302 valid = false;
303 return LineData();
304 }
305 valid = true;
306
307 Coordinate reta = -gamma/normsq * Coordinate (alpha, beta);
308 Coordinate retb = reta + Coordinate (-beta, alpha);
309 return LineData( reta, retb );
310 }
311
calcConicPolarPoint(const ConicCartesianData & data,const LineData & polar)312 const Coordinate calcConicPolarPoint (
313 const ConicCartesianData& data,
314 const LineData& polar )
315 {
316 Coordinate p1 = polar.a;
317 Coordinate p2 = polar.b;
318
319 double alpha = p2.y - p1.y;
320 double beta = p1.x - p2.x;
321 double gamma = p1.y*p2.x - p1.x*p2.y;
322
323 double a11 = data.coeffs[0];
324 double a22 = data.coeffs[1];
325 double a12 = data.coeffs[2]/2.0;
326 double a13 = data.coeffs[3]/2.0;
327 double a23 = data.coeffs[4]/2.0;
328 double a33 = data.coeffs[5];
329
330 // double detA = a11*a22*a33 - a11*a23*a23 - a22*a13*a13 - a33*a12*a12 +
331 // 2*a12*a23*a13;
332
333 double a11inv = a22*a33 - a23*a23;
334 double a22inv = a11*a33 - a13*a13;
335 double a33inv = a11*a22 - a12*a12;
336 double a12inv = a23*a13 - a12*a33;
337 double a23inv = a12*a13 - a23*a11;
338 double a13inv = a12*a23 - a13*a22;
339
340 double x = a11inv*alpha + a12inv*beta + a13inv*gamma;
341 double y = a12inv*alpha + a22inv*beta + a23inv*gamma;
342 double z = a13inv*alpha + a23inv*beta + a33inv*gamma;
343
344 if (fabs(z) < 1e-10) // point at infinity
345 {
346 return Coordinate::invalidCoord();
347 }
348
349 x /= z;
350 y /= z;
351 return Coordinate (x, y);
352 }
353
calcConicLineIntersect(const ConicCartesianData & c,const LineData & l,double knownparam,int which)354 const Coordinate calcConicLineIntersect( const ConicCartesianData& c,
355 const LineData& l,
356 double knownparam,
357 int which )
358 {
359 assert( which == 1 || which == -1 || which == 0 );
360
361 double aa = c.coeffs[0];
362 double bb = c.coeffs[1];
363 double cc = c.coeffs[2];
364 double dd = c.coeffs[3];
365 double ee = c.coeffs[4];
366 double ff = c.coeffs[5];
367
368 double x = l.a.x;
369 double y = l.a.y;
370 double dx = l.b.x - l.a.x;
371 double dy = l.b.y - l.a.y;
372
373 double aaa = aa*dx*dx + bb*dy*dy + cc*dx*dy;
374 double bbb = 2*aa*x*dx + 2*bb*y*dy + cc*x*dy + cc*y*dx + dd*dx + ee*dy;
375 double ccc = aa*x*x + bb*y*y + cc*x*y + dd*x + ee*y + ff;
376
377 double t;
378 if ( which == 0 ) /* i.e. we have a known intersection */
379 {
380 t = - bbb/aaa - knownparam;
381 return l.a + t*(l.b - l.a);
382 }
383
384 double discrim = bbb*bbb - 4*aaa*ccc;
385 if (discrim < 0.0)
386 {
387 return Coordinate::invalidCoord();
388 }
389 else
390 {
391 if ( which*bbb > 0 )
392 {
393 t = bbb + which*sqrt(discrim);
394 t = - 2*ccc/t;
395 } else {
396 t = -bbb + which*sqrt(discrim);
397 t /= 2*aaa;
398 /* mp: this threshold test for a point at infinity allows to
399 * solve Bug https://bugs.kde.org/show_bug.cgi?id=316693
400 */
401 if (fabs(t) > 1e15) return Coordinate::invalidCoord();
402 }
403
404 return l.a + t*(l.b - l.a);
405 }
406 }
407
ConicPolarData(const Coordinate & f,double d,double ec,double es)408 ConicPolarData::ConicPolarData(
409 const Coordinate& f, double d,
410 double ec, double es )
411 : focus1( f ), pdimen( d ), ecostheta0( ec ), esintheta0( es )
412 {
413 }
414
ConicPolarData()415 ConicPolarData::ConicPolarData()
416 : focus1(), pdimen( 0 ), ecostheta0( 0 ), esintheta0( 0 )
417 {
418 }
419
calcConicBDFP(const LineData & directrix,const Coordinate & cfocus,const Coordinate & cpoint)420 const ConicPolarData calcConicBDFP(
421 const LineData& directrix,
422 const Coordinate& cfocus,
423 const Coordinate& cpoint )
424 {
425 ConicPolarData ret;
426
427 Coordinate ba = directrix.dir();
428 double bal = ba.length();
429 ret.ecostheta0 = -ba.y / bal;
430 ret.esintheta0 = ba.x / bal;
431
432 Coordinate pa = cpoint - directrix.a;
433
434 double distpf = (cpoint - cfocus).length();
435 double distpd = ( pa.y*ba.x - pa.x*ba.y)/bal;
436
437 double eccentricity = distpf/distpd;
438 ret.ecostheta0 *= eccentricity;
439 ret.esintheta0 *= eccentricity;
440
441 Coordinate fa = cfocus - directrix.a;
442 ret.pdimen = ( fa.y*ba.x - fa.x*ba.y )/bal;
443 ret.pdimen *= eccentricity;
444 ret.focus1 = cfocus;
445
446 return ret;
447 }
448
ConicCartesianData(const double incoeffs[6])449 ConicCartesianData::ConicCartesianData( const double incoeffs[6] )
450 {
451 std::copy( incoeffs, incoeffs + 6, coeffs );
452 }
453
calcConicAsymptote(const ConicCartesianData & data,int which,bool & valid)454 const LineData calcConicAsymptote(
455 const ConicCartesianData &data,
456 int which, bool &valid )
457 {
458 assert( which == -1 || which == 1 );
459
460 LineData ret;
461 double a=data.coeffs[0];
462 double b=data.coeffs[1];
463 double c=data.coeffs[2];
464 double d=data.coeffs[3];
465 double e=data.coeffs[4];
466
467 double normabc = a*a + b*b + c*c;
468 double delta = c*c - 4*a*b;
469 if (fabs(delta) < 1e-6*normabc) { valid = false; return ret; }
470
471 double yc = (2*a*e - c*d)/delta;
472 double xc = (2*b*d - c*e)/delta;
473 // let c be nonnegative; we no longer need d, e, f.
474 if (c < 0)
475 {
476 c *= -1;
477 a *= -1;
478 b *= -1;
479 }
480
481 if ( delta < 0 )
482 {
483 valid = false;
484 return ret;
485 }
486
487 double sqrtdelta = sqrt(delta);
488 Coordinate displacement;
489 if (which > 0)
490 displacement = Coordinate(-2*b, c + sqrtdelta);
491 else
492 displacement = Coordinate(c + sqrtdelta, -2*a);
493 ret.a = Coordinate(xc, yc);
494 ret.b = ret.a + displacement;
495 return ret;
496 }
497
calcConicByAsymptotes(const LineData & line1,const LineData & line2,const Coordinate & p)498 const ConicCartesianData calcConicByAsymptotes(
499 const LineData& line1,
500 const LineData& line2,
501 const Coordinate& p )
502 {
503 Coordinate p1 = line1.a;
504 Coordinate p2 = line1.b;
505 double x = p.x;
506 double y = p.y;
507
508 double c1 = p1.x*p2.y - p2.x*p1.y;
509 double b1 = p2.x - p1.x;
510 double a1 = p1.y - p2.y;
511
512 p1 = line2.a;
513 p2 = line2.b;
514
515 double c2 = p1.x*p2.y - p2.x*p1.y;
516 double b2 = p2.x - p1.x;
517 double a2 = p1.y - p2.y;
518
519 double a = a1*a2;
520 double b = b1*b2;
521 double c = a1*b2 + a2*b1;
522 double d = a1*c2 + a2*c1;
523 double e = b1*c2 + c1*b2;
524
525 double f = a*x*x + b*y*y + c*x*y + d*x + e*y;
526 f = -f;
527
528 return ConicCartesianData( a, b, c, d, e, f );
529 }
530
calcConicRadical(const ConicCartesianData & cequation1,const ConicCartesianData & cequation2,int which,int zeroindex,bool & valid)531 const LineData calcConicRadical( const ConicCartesianData& cequation1,
532 const ConicCartesianData& cequation2,
533 int which, int zeroindex, bool& valid )
534 {
535 assert( which == 1 || which == -1 );
536 assert( 0 < zeroindex && zeroindex < 4 );
537 LineData ret;
538 valid = true;
539
540 double a = cequation1.coeffs[0];
541 double b = cequation1.coeffs[1];
542 double c = cequation1.coeffs[2];
543 double d = cequation1.coeffs[3];
544 double e = cequation1.coeffs[4];
545 double f = cequation1.coeffs[5];
546
547 double a2 = cequation2.coeffs[0];
548 double b2 = cequation2.coeffs[1];
549 double c2 = cequation2.coeffs[2];
550 double d2 = cequation2.coeffs[3];
551 double e2 = cequation2.coeffs[4];
552 double f2 = cequation2.coeffs[5];
553
554 // background: the family of conics c + lambda*c2 has members that
555 // degenerate into a union of two lines. The values of lambda giving
556 // such degenerate conics is the solution of a third degree equation.
557 // The coefficients of such equation are given by:
558 // (Thanks to Dominique Devriese for the suggestion of this approach)
559 // domi: (And thanks to Maurizio for implementing it :)
560
561 double df = 4*a*b*f - a*e*e - b*d*d - c*c*f + c*d*e;
562 double cf = 4*a2*b*f + 4*a*b2*f + 4*a*b*f2
563 - 2*a*e*e2 - 2*b*d*d2 - 2*f*c*c2
564 - a2*e*e - b2*d*d - f2*c*c
565 + c2*d*e + c*d2*e + c*d*e2;
566 double bf = 4*a*b2*f2 + 4*a2*b*f2 + 4*a2*b2*f
567 - 2*a2*e2*e - 2*b2*d2*d - 2*f2*c2*c
568 - a*e2*e2 - b*d2*d2 - f*c2*c2
569 + c*d2*e2 + c2*d*e2 + c2*d2*e;
570 double af = 4*a2*b2*f2 - a2*e2*e2 - b2*d2*d2 - c2*c2*f2 + c2*d2*e2;
571
572 // assume both conics are nondegenerate, renormalize so that af = 1
573
574 df /= af;
575 cf /= af;
576 bf /= af;
577 af = 1.0; // not needed, just for consistency
578
579 // computing the coefficients of the Sturm sequence
580
581 double p1a = 2*bf*bf - 6*cf;
582 double p1b = bf*cf - 9*df;
583 double p0a = cf*p1a*p1a + p1b*(3*p1b - 2*bf*p1a);
584 double fval, fpval, lambda;
585
586 if (p0a < 0 && p1a < 0)
587 {
588 // -+-- ---+
589 valid = false;
590 return ret;
591 }
592
593 lambda = -bf/3.0; //inflection point
594 double displace = 1.0;
595 if (p1a > 0) // with two stationary points
596 {
597 displace += sqrt(p1a); // should be enough. The important
598 // thing is that it is larger than the
599 // semidistance between the stationary points
600 }
601 // compute the value at the inflection point using Horner scheme
602 fval = bf + lambda; // b + x
603 fval = cf + lambda*fval; // c + xb + xx
604 fval = df + lambda*fval; // d + xc + xxb + xxx
605
606 if (fabs(p0a) < 1e-7)
607 { // this is the case if we intersect two vertical parabolas!
608 p0a = 1e-7; // fall back to the one zero case
609 }
610 if (p0a < 0)
611 {
612 // we have three roots..
613 // we select the one we want ( defined by mzeroindex.. )
614 lambda += ( 2 - zeroindex )* displace;
615 }
616 else
617 {
618 // we have just one root
619 if( zeroindex > 1 ) // cannot find second and third root
620 {
621 valid = false;
622 return ret;
623 }
624
625 if (fval > 0) // zero on the left
626 {
627 lambda -= displace;
628 } else { // zero on the right
629 lambda += displace;
630 }
631
632 // p0a = 0 means we have a root with multiplicity two
633 }
634
635 //
636 // find a root of af*lambda^3 + bf*lambda^2 + cf*lambda + df = 0
637 // (use a Newton method starting from lambda = 0. Hope...)
638 //
639
640 double delta;
641
642 int iterations = 0;
643 const int maxiterations = 30;
644 while (iterations++ < maxiterations) // using Newton, iterations should be very few
645 {
646 // compute value of function and polinomial
647 fval = fpval = af;
648 fval = bf + lambda*fval; // b + xa
649 fpval = fval + lambda*fpval; // b + 2xa
650 fval = cf + lambda*fval; // c + xb + xxa
651 fpval = fval + lambda*fpval; // c + 2xb + 3xxa
652 fval = df + lambda*fval; // d + xc + xxb + xxxa
653
654 delta = fval/fpval;
655 lambda -= delta;
656 if (fabs(delta) < 1e-6) break;
657 }
658 if (iterations >= maxiterations) { valid = false; return ret; }
659
660 // now we have the degenerate conic: a, b, c, d, e, f
661
662 a += lambda*a2;
663 b += lambda*b2;
664 c += lambda*c2;
665 d += lambda*d2;
666 e += lambda*e2;
667 f += lambda*f2;
668
669 // domi:
670 // this is the determinant of the matrix of the new conic. It
671 // should be zero, for the new conic to be degenerate.
672 df = 4*a*b*f - a*e*e - b*d*d - c*c*f + c*d*e;
673
674 //lets work in homogeneous coordinates...
675
676 double dis1 = e*e - 4*b*f;
677 double maxval = fabs(dis1);
678 int maxind = 1;
679 double dis2 = d*d - 4*a*f;
680 if (fabs(dis2) > maxval)
681 {
682 maxval = fabs(dis2);
683 maxind = 2;
684 }
685 double dis3 = c*c - 4*a*b;
686 if (fabs(dis3) > maxval)
687 {
688 maxval = fabs(dis3);
689 maxind = 3;
690 }
691 // one of these must be nonzero (otherwise the matrix is ...)
692 // exchange elements so that the largest is the determinant of the
693 // first 2x2 minor
694 double temp;
695 switch (maxind)
696 {
697 case 1: // exchange 1 <-> 3
698 temp = a; a = f; f = temp;
699 temp = c; c = e; e = temp;
700 temp = dis1; dis1 = dis3; dis3 = temp;
701 break;
702
703 case 2: // exchange 2 <-> 3
704 temp = b; b = f; f = temp;
705 temp = c; c = d; d = temp;
706 temp = dis2; dis2 = dis3; dis3 = temp;
707 break;
708 }
709
710 // domi:
711 // this is the negative of the determinant of the top left of the
712 // matrix. If it is 0, then the conic is a parabola, if it is < 0,
713 // then the conic is an ellipse, if positive, the conic is a
714 // hyperbola. In this case, it should be positive, since we have a
715 // degenerate conic, which is a degenerate case of a hyperbola..
716 // note that it is negative if there is no valid conic to be
717 // found ( e.g. not enough intersections.. )
718 // double discrim = c*c - 4*a*b;
719
720 if (dis3 < 0)
721 {
722 // domi:
723 // i would put an assertion here, but well, i guess it doesn't
724 // really matter, and this prevents crashes if the math i still
725 // recall from high school happens to be wrong :)
726 valid = false;
727 return ret;
728 };
729
730 double r[3]; // direction of the null space
731 r[0] = 2*b*d - c*e;
732 r[1] = 2*a*e - c*d;
733 r[2] = dis3;
734
735 // now remember the switch:
736 switch (maxind)
737 {
738 case 1: // exchange 1 <-> 3
739 temp = a; a = f; f = temp;
740 temp = c; c = e; e = temp;
741 temp = dis1; dis1 = dis3; dis3 = temp;
742 temp = r[0]; r[0] = r[2]; r[2] = temp;
743 break;
744
745 case 2: // exchange 2 <-> 3
746 temp = b; b = f; f = temp;
747 temp = c; c = d; d = temp;
748 temp = dis2; dis2 = dis3; dis3 = temp;
749 temp = r[1]; r[1] = r[2]; r[2] = temp;
750 break;
751 }
752
753 // Computing a Householder reflection transformation that
754 // maps r onto [0, 0, k]
755
756 double w[3];
757 double rnormsq = r[0]*r[0] + r[1]*r[1] + r[2]*r[2];
758 double k = sqrt( rnormsq );
759 if ( k*r[2] < 0) k = -k;
760 double wnorm = sqrt( 2*rnormsq + 2*k*r[2] );
761 w[0] = r[0]/wnorm;
762 w[1] = r[1]/wnorm;
763 w[2] = (r[2] + k)/wnorm;
764
765 // matrix transformation using Householder matrix, the resulting
766 // matrix is zero on third row and column
767 // [q0,q1,q2]^t = A w
768 // alpha = w^t A w
769 double q0 = a*w[0] + c*w[1]/2 + d*w[2]/2;
770 double q1 = b*w[1] + c*w[0]/2 + e*w[2]/2;
771 double alpha = a*w[0]*w[0] + b*w[1]*w[1] + c*w[0]*w[1] +
772 d*w[0]*w[2] + e*w[1]*w[2] + f*w[2]*w[2];
773 double a00 = a - 4*w[0]*q0 + 4*w[0]*w[0]*alpha;
774 double a11 = b - 4*w[1]*q1 + 4*w[1]*w[1]*alpha;
775 double a01 = c/2 - 2*w[1]*q0 - 2*w[0]*q1 + 4*w[0]*w[1]*alpha;
776
777 double dis = a01*a01 - a00*a11;
778 assert ( dis >= 0 );
779 double sqrtdis = sqrt( dis );
780 double px, py;
781 if ( which*a01 > 0 )
782 {
783 px = a01 + which*sqrtdis;
784 py = a11;
785 } else {
786 px = a00;
787 py = a01 - which*sqrtdis;
788 }
789 double p[3]; // vector orthogonal to one of the two planes
790 double pscalw = w[0]*px + w[1]*py;
791 p[0] = px - 2*pscalw*w[0];
792 p[1] = py - 2*pscalw*w[1];
793 p[2] = - 2*pscalw*w[2];
794
795 // "r" is the solution of the equation A*(x,y,z) = (0,0,0) where
796 // A is the matrix of the degenerate conic. This is what we
797 // called in the conic theory we saw in high school a "double
798 // point". It has the unique property that any line going through
799 // it is a "polar line" of the conic at hand. It only exists for
800 // degenerate conics. It has another unique property that if you
801 // take any other point on the conic, then the line between it and
802 // the double point is part of the conic.
803 // this is what we use here: we find the double point ( ret.a
804 // ), and then find another points on the conic.
805
806 ret.a = -p[2]/(p[0]*p[0] + p[1]*p[1]) * Coordinate (p[0],p[1]);
807 ret.b = ret.a + Coordinate (-p[1],p[0]);
808 valid = true;
809
810 return ret;
811 }
812
calcConicTransformation(const ConicCartesianData & data,const Transformation & t,bool & valid)813 const ConicCartesianData calcConicTransformation (
814 const ConicCartesianData& data, const Transformation& t, bool& valid )
815 {
816 double a[3][3];
817 double b[3][3];
818
819 a[1][1] = data.coeffs[0];
820 a[2][2] = data.coeffs[1];
821 a[1][2] = a[2][1] = data.coeffs[2]/2;
822 a[0][1] = a[1][0] = data.coeffs[3]/2;
823 a[0][2] = a[2][0] = data.coeffs[4]/2;
824 a[0][0] = data.coeffs[5];
825
826 Transformation ti = t.inverse( valid );
827 if ( ! valid ) return ConicCartesianData();
828
829 double supnorm = 0.0;
830 for (int i = 0; i < 3; i++)
831 {
832 for (int j = 0; j < 3; j++)
833 {
834 b[i][j] = 0.;
835 for (int ii = 0; ii < 3; ii++)
836 {
837 for (int jj = 0; jj < 3; jj++)
838 {
839 b[i][j] += a[ii][jj]*ti.data( ii, i )*ti.data( jj, j );
840 }
841 }
842 if ( std::fabs( b[i][j] ) > supnorm ) supnorm = std::fabs( b[i][j] );
843 }
844 }
845
846 for (int i = 0; i < 3; i++)
847 {
848 for (int j = 0; j < 3; j++)
849 {
850 b[i][j] /= supnorm;
851 }
852 }
853
854 return ConicCartesianData ( b[1][1], b[2][2], b[1][2] + b[2][1],
855 b[0][1] + b[1][0], b[0][2] + b[2][0], b[0][0] );
856 }
857
ConicCartesianData()858 ConicCartesianData::ConicCartesianData()
859 {
860 }
861
operator ==(const ConicPolarData & lhs,const ConicPolarData & rhs)862 bool operator==( const ConicPolarData& lhs, const ConicPolarData& rhs )
863 {
864 return lhs.focus1 == rhs.focus1 &&
865 lhs.pdimen == rhs.pdimen &&
866 lhs.ecostheta0 == rhs.ecostheta0 &&
867 lhs.esintheta0 == rhs.esintheta0;
868 }
869
invalidData()870 ConicCartesianData ConicCartesianData::invalidData()
871 {
872 ConicCartesianData ret;
873 ret.coeffs[0] = double_inf;
874 return ret;
875 }
876
valid() const877 bool ConicCartesianData::valid() const
878 {
879 return std::isfinite( coeffs[0] );
880 }
881
882