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 "kignumerics.h"
9 #include "common.h"
10
11 using std::fabs;
12
13 /*
14 * compute one of the roots of a cubic polynomial
15 * if xmin << 0 or xmax >> 0 then autocompute a bound for all the
16 * roots
17 */
18
calcCubicRoot(double xmin,double xmax,double a,double b,double c,double d,int root,bool & valid,int & numroots)19 double calcCubicRoot ( double xmin, double xmax, double a,
20 double b, double c, double d, int root, bool& valid, int& numroots )
21 {
22 // renormalize: positive a and infinity norm = 1
23
24 double infnorm = fabs(a);
25 if ( infnorm < fabs(b) ) infnorm = fabs(b);
26 if ( infnorm < fabs(c) ) infnorm = fabs(c);
27 if ( infnorm < fabs(d) ) infnorm = fabs(d);
28 if ( a < 0 ) infnorm = -infnorm;
29 a /= infnorm;
30 b /= infnorm;
31 c /= infnorm;
32 d /= infnorm;
33
34 const double small = 1e-7;
35 valid = false;
36 if ( fabs(a) < small )
37 {
38 if ( fabs(b) < small )
39 {
40 if ( fabs(c) < small )
41 { // degree = 0;
42 numroots = 0;
43 return 0.0;
44 }
45 // degree = 1
46 double rootval = -d/c;
47 numroots = 1;
48 if ( rootval < xmin || xmax < rootval ) numroots--;
49 if ( root > numroots ) return 0.0;
50 valid = true;
51 return rootval;
52 }
53 // degree = 2
54 if ( b < 0 ) { b = -b; c = -c; d = -d; }
55 double discrim = c*c - 4*b*d;
56 numroots = 2;
57 if ( discrim < 0 )
58 {
59 numroots = 0;
60 return 0.0;
61 }
62 discrim = std::sqrt(discrim)/(2*fabs(b));
63 double rootmiddle = -c/(2*b);
64 if ( rootmiddle - discrim < xmin ) numroots--;
65 if ( rootmiddle + discrim > xmax ) numroots--;
66 if ( rootmiddle + discrim < xmin ) numroots--;
67 if ( rootmiddle - discrim > xmax ) numroots--;
68 if ( root > numroots ) return 0.0;
69 valid = true;
70 if ( root == 2 || rootmiddle - discrim < xmin ) return rootmiddle + discrim;
71 return rootmiddle - discrim;
72 }
73
74 if ( xmin < -1e8 || xmax > 1e8 )
75 {
76
77 // compute a bound for all the real roots:
78
79 xmax = fabs(d/a);
80 if ( fabs(c/a) + 1 > xmax ) xmax = fabs(c/a) + 1;
81 if ( fabs(b/a) + 1 > xmax ) xmax = fabs(b/a) + 1;
82 xmin = -xmax;
83 }
84
85 // computing the coefficients of the Sturm sequence
86 double p1a = 2*b*b - 6*a*c;
87 double p1b = b*c - 9*a*d;
88 double p0a = c*p1a*p1a + p1b*(3*a*p1b - 2*b*p1a);
89
90 int varbottom = calcCubicVariations (xmin, a, b, c, d, p1a, p1b, p0a);
91 int vartop = calcCubicVariations (xmax, a, b, c, d, p1a, p1b, p0a);
92 numroots = vartop - varbottom;
93 valid = false;
94 if (root <= varbottom || root > vartop ) return 0.0;
95
96 valid = true;
97
98 // now use bisection to separate the required root
99 double dx = (xmax - xmin)/2;
100 while ( vartop - varbottom > 1 )
101 {
102 if ( fabs( dx ) < 1e-8 ) return (xmin + xmax)/2;
103 double xmiddle = xmin + dx;
104 int varmiddle = calcCubicVariations (xmiddle, a, b, c, d, p1a, p1b, p0a);
105 if ( varmiddle < root ) // I am below
106 {
107 xmin = xmiddle;
108 varbottom = varmiddle;
109 } else {
110 xmax = xmiddle;
111 vartop = varmiddle;
112 }
113 dx /= 2;
114 }
115
116 /*
117 * now [xmin, xmax] enclose a single root, try using Newton
118 */
119 if ( vartop - varbottom == 1 )
120 {
121 double fval1 = a; // double check...
122 double fval2 = a;
123 fval1 = b + xmin*fval1;
124 fval2 = b + xmax*fval2;
125 fval1 = c + xmin*fval1;
126 fval2 = c + xmax*fval2;
127 fval1 = d + xmin*fval1;
128 fval2 = d + xmax*fval2;
129 assert ( fval1 * fval2 <= 0 );
130 return calcCubicRootwithNewton ( xmin, xmax, a, b, c, d, 1e-8 );
131 }
132 else // probably a double root here!
133 return ( xmin + xmax )/2;
134 }
135
136 /*
137 * computation of the number of sign changes in the sturm sequence for
138 * a third degree polynomial at x. This number counts the number of
139 * roots of the polynomial on the left of point x.
140 *
141 * a, b, c, d: coefficients of the third degree polynomial (a*x^3 + ...)
142 *
143 * the second degree polynomial in the sturm sequence is just minus the
144 * derivative, so we don't need to compute it.
145 *
146 * p1a*x + p1b: is the third (first degree) polynomial in the sturm sequence.
147 *
148 * p0a: is the (constant) fourth polynomial of the sturm sequence.
149 */
150
calcCubicVariations(double x,double a,double b,double c,double d,double p1a,double p1b,double p0a)151 int calcCubicVariations (double x, double a, double b, double c,
152 double d, double p1a, double p1b, double p0a)
153 {
154 double fval, fpval;
155 fval = fpval = a;
156 fval = b + x*fval;
157 fpval = fval + x*fpval;
158 fval = c + x*fval;
159 fpval = fval + x*fpval;
160 fval = d + x*fval;
161
162 double f1val = p1a*x + p1b;
163
164 bool f3pos = fval >= 0;
165 bool f2pos = fpval <= 0;
166 bool f1pos = f1val >= 0;
167 bool f0pos = p0a >= 0;
168
169 int variations = 0;
170 if ( f3pos != f2pos ) variations++;
171 if ( f2pos != f1pos ) variations++;
172 if ( f1pos != f0pos ) variations++;
173 return variations;
174 }
175
176 /*
177 * use newton to solve a third degree equation with already isolated
178 * root
179 */
180
calcCubicDerivatives(double x,double a,double b,double c,double d,double & fval,double & fpval,double & fppval)181 inline void calcCubicDerivatives ( double x, double a, double b, double c,
182 double d, double& fval, double& fpval, double& fppval )
183 {
184 fval = fpval = fppval = a;
185 fval = b + x*fval;
186 fpval = fval + x*fpval;
187 fppval = fpval + x*fppval; // this is really half the second derivative
188 fval = c + x*fval;
189 fpval = fval + x*fpval;
190 fval = d + x*fval;
191 }
192
calcCubicRootwithNewton(double xmin,double xmax,double a,double b,double c,double d,double tol)193 double calcCubicRootwithNewton ( double xmin, double xmax, double a,
194 double b, double c, double d, double tol )
195 {
196 double fval, fpval, fppval;
197
198 double fval1, fval2, fpval1, fpval2, fppval1, fppval2;
199 calcCubicDerivatives ( xmin, a, b, c, d, fval1, fpval1, fppval1 );
200 calcCubicDerivatives ( xmax, a, b, c, d, fval2, fpval2, fppval2 );
201 assert ( fval1 * fval2 <= 0 );
202
203 assert ( xmax > xmin );
204 while ( xmax - xmin > tol )
205 {
206 // compute the values of function, derivative and second derivative:
207 assert ( fval1 * fval2 <= 0 );
208 if ( fppval1 * fppval2 < 0 || fpval1 * fpval2 < 0 )
209 {
210 double xmiddle = (xmin + xmax)/2;
211 calcCubicDerivatives ( xmiddle, a, b, c, d, fval, fpval, fppval );
212 if ( fval1*fval <= 0 )
213 {
214 xmax = xmiddle;
215 fval2 = fval;
216 fpval2 = fpval;
217 fppval2 = fppval;
218 } else {
219 xmin = xmiddle;
220 fval1 = fval;
221 fpval1 = fpval;
222 fppval1 = fppval;
223 }
224 } else
225 {
226 // now we have first and second derivative of constant sign, we
227 // can start with Newton from the Fourier point.
228 double x = xmin;
229 if ( fval2*fppval2 > 0 ) x = xmax;
230 double p = 1.0;
231 int iterations = 0;
232 while ( fabs(p) > tol && iterations++ < 100 )
233 {
234 calcCubicDerivatives ( x, a, b, c, d, fval, fpval, fppval );
235 p = fval/fpval;
236 x -= p;
237 }
238 if( iterations >= 100 )
239 {
240 // Newton scheme did not converge..
241 // we should end up with an invalid Coordinate
242 return double_inf;
243 };
244 return x;
245 }
246 }
247
248 // we cannot apply Newton, (perhaps we are at an inflection point)
249
250 return (xmin + xmax)/2;
251 }
252
253 /*
254 * This function computes the LU factorization of a mxn matrix, with
255 * m typically less than n. This is done with complete pivoting; the
256 * exchanges in columns are recorded in the integer vector "exchange"
257 */
GaussianElimination(double * matrix[],int numrows,int numcols,int exchange[])258 bool GaussianElimination( double *matrix[], int numrows,
259 int numcols, int exchange[] )
260 {
261 // start gaussian elimination
262 for ( int k = 0; k < numrows; ++k )
263 {
264 // ricerca elemento di modulo massimo
265 double maxval = -double_inf;
266 int imax = k;
267 int jmax = k;
268 for( int i = k; i < numrows; ++i )
269 {
270 for( int j = k; j < numcols; ++j )
271 {
272 if (fabs(matrix[i][j]) > maxval)
273 {
274 maxval = fabs(matrix[i][j]);
275 imax = i;
276 jmax = j;
277 }
278 }
279 }
280
281 // row exchange
282 if ( imax != k )
283 for( int j = k; j < numcols; ++j )
284 {
285 double t = matrix[k][j];
286 matrix[k][j] = matrix[imax][j];
287 matrix[imax][j] = t;
288 }
289
290 // column exchange
291 if ( jmax != k )
292 for( int i = 0; i < numrows; ++i )
293 {
294 double t = matrix[i][k];
295 matrix[i][k] = matrix[i][jmax];
296 matrix[i][jmax] = t;
297 }
298
299 // remember this column exchange at step k
300 exchange[k] = jmax;
301
302 // we can't usefully eliminate a singular matrix..
303 if ( maxval == 0. ) return false;
304
305 // ciclo sulle righe
306 for( int i = k+1; i < numrows; ++i)
307 {
308 double mik = matrix[i][k]/matrix[k][k];
309 matrix[i][k] = mik; //ricorda il moltiplicatore... (not necessary)
310 // ciclo sulle colonne
311 for( int j = k+1; j < numcols; ++j )
312 {
313 matrix[i][j] -= mik*matrix[k][j];
314 }
315 }
316 }
317 return true;
318 }
319
320 /*
321 * solve an undetermined homogeneous triangular system. the matrix is nonzero
322 * on its diagonal. The last unknown(s) are chosen to be 1. The
323 * vector "exchange" contains exchanges to be performed on the
324 * final solution components.
325 */
326
BackwardSubstitution(double * matrix[],int numrows,int numcols,int exchange[],double solution[])327 void BackwardSubstitution( double *matrix[], int numrows, int numcols,
328 int exchange[], double solution[] )
329 {
330 // the system is homogeneous and underdetermined, the last unknown(s)
331 // are chosen = 1
332 for ( int j = numrows; j < numcols; ++j )
333 {
334 solution[j] = 1.0; // other choices are possible here
335 };
336
337 for( int k = numrows - 1; k >= 0; --k )
338 {
339 // backward substitution
340 solution[k] = 0.0;
341 for ( int j = k+1; j < numcols; ++j)
342 {
343 solution[k] -= matrix[k][j]*solution[j];
344 }
345 solution[k] /= matrix[k][k];
346 }
347
348 // ultima fase: riordinamento incognite
349
350 for( int k = numrows - 1; k >= 0; --k )
351 {
352 int jmax = exchange[k];
353 double t = solution[k];
354 solution[k] = solution[jmax];
355 solution[jmax] = t;
356 }
357 }
358
Invert3by3matrix(const double m[3][3],double inv[3][3])359 bool Invert3by3matrix ( const double m[3][3], double inv[3][3] )
360 {
361 double det = m[0][0]*(m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
362 m[0][1]*(m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
363 m[0][2]*(m[1][0]*m[2][1] - m[1][1]*m[2][0]);
364 if (det == 0) return false;
365
366 for (int i=0; i < 3; i++)
367 {
368 for (int j=0; j < 3; j++)
369 {
370 int i1 = (i+1)%3;
371 int i2 = (i+2)%3;
372 int j1 = (j+1)%3;
373 int j2 = (j+2)%3;
374 inv[j][i] = (m[i1][j1]*m[i2][j2] - m[i1][j2]*m[i2][j1])/det;
375 }
376 }
377 return true;
378 }
379