Debian Codegraph @

3rdparty/mshmet/src/sourceslib/eigenv.c

raw
1 #include <stdio.h>
2 #include <string.h>
3 #include <math.h>
4
5 /* seeking 1.e-05 accuracy */
6 /* Modif F. Hecht because in some case the result a wrong */
7 #define EPSD 1.e-12
8 #define EPSD2 1.e-10
9 #define EPS6 5.e-06
10 #define EPS 1.e-06
11 #define EPSX2 2.e-06
12 #define MAXTOU 50
13
14 /* check if numbers are equal */
15 #define egal(x,y) ( \
16 ( ((x) == 0.0f) ? (fabs(y) < EPS) : \
17 ( ((y) == 0.0f) ? (fabs(x) < EPS) : \
18 (fabs((x)-(y)) / (fabs(x) + fabs(y)) < EPSX2) ) ) )
19
20
21 static double Id[3][3] = {
22 { 1.0, 0.0, 0.0},
23 { 0.0, 1.0, 0.0},
24 { 0.0, 0.0, 1.0} };
25
26
27 /* find root(s) of polynomial: P(x)= x^3+bx^2+cx+d
28 return 1: 3 roots, 2: 2 roots, 3: 1 root */
29 static int newton3(double p[4],double x[3]) {
30 double b,c,d,da,db,dc,epsd;
31 double delta,fx,dfx,dxx;
32 double fdx0,fdx1,dx0,dx1,x1,x2;
33 int it,n;
34
35 /* coeffs polynomial, a=1 */
36 b = p[2];
37 c = p[1];
38 d = p[0];
39 n = 1;
40
41 /* 1st derivative of f */
42 da = 3.0;
43 db = 2.0*b;
44
45 /* solve 2nd order eqn */
46 delta = db*db - 4.0*da*c;
47 epsd = db*db*EPSD2;
48
49 /* inflexion (f'(x)=0, x=-b/2a) */
50 x1 = -db / 6.0f;
51
52 n = 1;
53 if ( delta > epsd ) {
54 delta = sqrt(delta);
55 dx0 = (-db + delta) / 6.0;
56 dx1 = (-db - delta) / 6.0;
57 /* Horner */
58 fdx0 = d + dx0*(c+dx0*(b+dx0));
59 fdx1 = d + dx1*(c+dx1*(b+dx1));
60
61 if ( fabs(fdx0) < EPSD ) {
62 /* dx0: double root, compute single root */
63 n = 2;
64 x[0] = dx0;
65 x[1] = dx0;
66 x[2] = -b - 2.0*dx0;
67 /* check if P(x) = 0 */
68 fx = d + x[2]*(c+x[2]*(b+x[2]));
69 if ( fabs(fx) > EPSD2 ) {
70 #ifdef DDEBUG
71 fprintf(stderr," ## ERR 9100, newton3: fx= %E\n",fx);
72 #endif
73 return(0);
74 }
75 return(n);
76 }
77 else if ( fabs(fdx1) < EPSD ) {
78 /* dx1: double root, compute single root */
79 n = 2;
80 x[0] = dx1;
81 x[1] = dx1;
82 x[2] = -b - 2.0*dx1;
83 /* check if P(x) = 0 */
84 fx = d + x[2]*(c+x[2]*(b+x[2]));
85 if ( fabs(fx) > EPSD2 ) {
86 #ifdef DDEBUG
87 fprintf(stderr," ## ERR 9100, newton3: fx= %E\n",fx);
88 #endif
89 return(0);
90 }
91 return(n);
92 }
93 }
94
95 else if ( fabs(delta) < epsd ) {
96 /* triple root */
97 n = 3;
98 x[0] = x1;
99 x[1] = x1;
100 x[2] = x1;
101 /* check if P(x) = 0 */
102 fx = d + x[0]*(c+x[0]*(b+x[0]));
103 if ( fabs(fx) > EPSD2 ) {
104 #ifdef DDEBUG
105 fprintf(stderr," ## ERR 9100, newton3: fx= %E\n",fx);
106 #endif
107 return(0);
108 }
109 return(n);
110 }
111
112 else {
113 #ifdef DDEBUG
114 fprintf(stderr," ## ERR 9101, newton3: no real roots\n");
115 #endif
116 return(0);
117 }
118
119 /* Newton method: find one root (middle)
120 starting point: P"(x)=0 */
121 x1 = -b / 3.0;
122 dfx = c + b*x1;
123 fx = d + x1*(c -2.0*x1*x1);
124 it = 0;
125 do {
126 x2 = x1 - fx / dfx;
127 fx = d + x2*(c+x2*(b+x2));
128 if ( fabs(fx) < EPSD ) {
129 x[0] = x2;
130 break;
131 }
132 dfx = c + x2*(db + da*x2);
133
134 /* check for break-off condition */
135 dxx = fabs((x2-x1) / x2);
136 if ( dxx < 1.0e-10 ) {
137 x[0] = x2;
138 if ( fabs(fx) > EPSD2 ) {
139 fprintf(stderr," ## ERR 9102, newton3, no root found (fx %E).\n",fx);
140 return(0);
141 }
142 break;
143 }
144 else
145 x1 = x2;
146 }
147 while ( ++it < MAXTOU );
148
149 if ( it == MAXTOU ) {
150 x[0] = x1;
151 fx = d + x1*(c+(x1*(b+x1)));
152 if ( fabs(fx) > EPSD2 ) {
153 fprintf(stderr," ## ERR 9102, newton3, no root found (fx %E).\n",fx);
154 return(0);
155 }
156 }
157
158 /* solve 2nd order equation
159 P(x) = (x-sol(1))* (x^2+bb*x+cc) */
160 db = b + x[0];
161 dc = c + x[0]*db;
162 delta = db*db - 4.0*dc;
163
164 if ( delta <= 0.0 ) {
165 fprintf(stderr," ## ERR 9103, newton3, det = 0.\n");
166 return(0);
167 }
168
169 delta = sqrt(delta);
170 x[1] = 0.5 * (-db+delta);
171 x[2] = 0.5 * (-db-delta);
172
173 #ifdef DDEBUG
174 /* check for root accuracy */
175 fx = d + x[1]*(c+x[1]*(b+x[1]));
176 if ( fabs(fx) > EPSD2 ) {
177 fprintf(stderr," ## ERR 9104, newton3: fx= %E x= %E\n",fx,x[1]);
178 return(0);
179 }
180 fx = d + x[2]*(c+x[2]*(b+x[2]));
181 if ( fabs(fx) > EPSD2 ) {
182 fprintf(stderr," ## ERR 9104, newton3: fx= %E x= %E\n",fx,x[2]);
183 return(0);
184 }
185 #endif
186
187 return(n);
188 }
189
190
191 /* find eigenvalues and vectors of a 3x3 symmetric definite
192 * positive matrix
193 * return order of eigenvalues (1,2,3) or 0 if failed */
194 int eigenv(int symmat,double *mat,double lambda[3],double v[3][3]) {
195 double a11,a12,a13,a21,a22,a23,a31,a32,a33;
196 double aa,bb,cc,dd,ee,ii,vx1[3],vx2[3],vx3[3],dd1,dd2,dd3;
197 double maxd,maxm,valm,p[4],w1[3],w2[3],w3[3];
198 int k,n;
199
200 /* default */
201 memcpy(v,Id,9*sizeof(double));
202 if ( symmat ) {
203 lambda[0] = (double)mat[0];
204 lambda[1] = (double)mat[3];
205 lambda[2] = (double)mat[5];
206
207 maxm = fabs(mat[0]);
208 for (k=1; k<6; k++) {
209 valm = fabs(mat[k]);
210 if ( valm > maxm ) maxm = valm;
211 }
212 /* single float accuracy */
213 if ( maxm < EPS6 ) return(1);
214
215 /* normalize matrix */
216 dd = 1.0 / maxm;
217 a11 = mat[0] * dd;
218 a12 = mat[1] * dd;
219 a13 = mat[2] * dd;
220 a22 = mat[3] * dd;
221 a23 = mat[4] * dd;
222 a33 = mat[5] * dd;
223
224 /* diagonal matrix */
225 maxd = fabs(a12);
226 valm = fabs(a13);
227 if ( valm > maxd ) maxd = valm;
228 valm = fabs(a23);
229 if ( valm > maxd ) maxd = valm;
230 if ( maxd < EPSD ) return(1);
231
232 a21 = a12;
233 a31 = a13;
234 a32 = a23;
235
236 /* build characteristic polynomial
237 P(X) = X^3 - trace X^2 + (somme des mineurs)X - det = 0 */
238 aa = a11*a22;
239 bb = a23*a32;
240 cc = a12*a21;
241 dd = a13*a31;
242 p[0] = a11*bb + a33*(cc-aa) + a22*dd -2.0*a12*a13*a23;
243 p[1] = a11*(a22 + a33) + a22*a33 - bb - cc - dd;
244 p[2] = -a11 - a22 - a33;
245 p[3] = 1.0;
246 }
247 else {
248 lambda[0] = (double)mat[0];
249 lambda[1] = (double)mat[4];
250 lambda[2] = (double)mat[8];
251
252 maxm = fabs(mat[0]);
253 for (k=1; k<9; k++) {
254 valm = fabs(mat[k]);
255 if ( valm > maxm ) maxm = valm;
256 }
257 if ( maxm < EPS6 ) return(1);
258
259 /* normalize matrix */
260 dd = 1.0 / maxm;
261 a11 = mat[0] * dd;
262 a12 = mat[1] * dd;
263 a13 = mat[2] * dd;
264 a21 = mat[3] * dd;
265 a22 = mat[4] * dd;
266 a23 = mat[5] * dd;
267 a31 = mat[6] * dd;
268 a32 = mat[7] * dd;
269 a33 = mat[8] * dd;
270
271 /* diagonal matrix */
272 maxd = fabs(a12);
273 valm = fabs(a13);
274 if ( valm > maxd ) maxd = valm;
275 valm = fabs(a23);
276 if ( valm > maxd ) maxd = valm;
277 valm = fabs(a21);
278 if ( valm > maxd ) maxd = valm;
279 valm = fabs(a31);
280 if ( valm > maxd ) maxd = valm;
281 valm = fabs(a32);
282 if ( valm > maxd ) maxd = valm;
283 if ( maxd < EPSD ) return(1);
284
285 /* build characteristic polynomial
286 P(X) = X^3 - trace X^2 + (somme des mineurs)X - det = 0 */
287 aa = a22*a33 - a23*a32;
288 bb = a23*a31 - a21*a33;
289 cc = a21*a32 - a31*a22;
290 ee = a11*a33 - a13*a31;
291 ii = a11*a22 - a12*a21;
292
293 p[0] = -a11*aa - a12*bb - a13*cc;
294 p[1] = aa + ee + ii;
295 p[2] = -a11 - a22 - a33;
296 p[3] = 1.0;
297 }
298
299 /* solve polynomial (find roots using newton) */
300 n = newton3(p,lambda);
301 if ( n <= 0 ) return(0);
302
303 /* compute eigenvectors:
304 an eigenvalue belong to orthogonal of Im(A-lambda*Id) */
305 v[0][0] = 1.0; v[0][1] = v[0][2] = 0.0;
306 v[1][1] = 1.0; v[1][0] = v[1][2] = 0.0;
307 v[2][2] = 1.0; v[2][0] = v[2][1] = 0.0;
308
309 w1[1] = a12; w1[2] = a13;
310 w2[0] = a21; w2[2] = a23;
311 w3[0] = a31; w3[1] = a32;
312
313 if ( n == 1 ) {
314 /* vk = crsprd(wi,wj) */
315 for (k=0; k<3; k++) {
316 w1[0] = a11 - lambda[k];
317 w2[1] = a22 - lambda[k];
318 w3[2] = a33 - lambda[k];
319
320 /* cross product vectors in (Im(A-lambda(i) Id) ortho */
321 vx1[0] = w1[1]*w3[2] - w1[2]*w3[1];
322 vx1[1] = w1[2]*w3[0] - w1[0]*w3[2];
323 vx1[2] = w1[0]*w3[1] - w1[1]*w3[0];
324 dd1 = vx1[0]*vx1[0] + vx1[1]*vx1[1] + vx1[2]*vx1[2];
325
326 vx2[0] = w1[1]*w2[2] - w1[2]*w2[1];
327 vx2[1] = w1[2]*w2[0] - w1[0]*w2[2];
328 vx2[2] = w1[0]*w2[1] - w1[1]*w2[0];
329 dd2 = vx2[0]*vx2[0] + vx2[1]*vx2[1] + vx2[2]*vx2[2];
330
331 vx3[0] = w2[1]*w3[2] - w2[2]*w3[1];
332 vx3[1] = w2[2]*w3[0] - w2[0]*w3[2];
333 vx3[2] = w2[0]*w3[1] - w2[1]*w3[0];
334 dd3 = vx3[0]*vx3[0] + vx3[1]*vx3[1] + vx3[2]*vx3[2];
335
336 /* find vector of max norm */
337 if ( dd1 > dd2 ) {
338 if ( dd1 > dd3 ) {
339 dd1 = 1.0 / sqrt(dd1);
340 v[k][0] = vx1[0] * dd1;
341 v[k][1] = vx1[1] * dd1;
342 v[k][2] = vx1[2] * dd1;
343 }
344 else {
345 dd3 = 1.0 / sqrt(dd3);
346 v[k][0] = vx3[0] * dd3;
347 v[k][1] = vx3[1] * dd3;
348 v[k][2] = vx3[2] * dd3;
349 }
350 }
351 else {
352 if ( dd2 > dd3 ) {
353 dd2 = 1.0 / sqrt(dd2);
354 v[k][0] = vx2[0] * dd2;
355 v[k][1] = vx2[1] * dd2;
356 v[k][2] = vx2[2] * dd2;
357 }
358 else {
359 dd3 = 1.0 / sqrt(dd3);
360 v[k][0] = vx3[0] * dd3;
361 v[k][1] = vx3[1] * dd3;
362 v[k][2] = vx3[2] * dd3;
363 }
364 }
365 }
366 }
367
368 /* (vp1,vp2) double, vp3 simple root */
369 else if ( n == 2 ) {
370 w1[0] = a11 - lambda[2];
371 w2[1] = a22 - lambda[2];
372 w3[2] = a33 - lambda[2];
373
374 /* cross product */
375 vx1[0] = w1[1]*w3[2] - w1[2]*w3[1];
376 vx1[1] = w1[2]*w3[0] - w1[0]*w3[2];
377 vx1[2] = w1[0]*w3[1] - w1[1]*w3[0];
378 dd1 = vx1[0]*vx1[0] + vx1[1]*vx1[1] + vx1[2]*vx1[2];
379
380 vx2[0] = w1[1]*w2[2] - w1[2]*w2[1];
381 vx2[1] = w1[2]*w2[0] - w1[0]*w2[2];
382 vx2[2] = w1[0]*w2[1] - w1[1]*w2[0];
383 dd2 = vx2[0]*vx2[0] + vx2[1]*vx2[1] + vx2[2]*vx2[2];
384
385 vx3[0] = w2[1]*w3[2] - w2[2]*w3[1];
386 vx3[1] = w2[2]*w3[0] - w2[0]*w3[2];
387 vx3[2] = w2[0]*w3[1] - w2[1]*w3[0];
388 dd3 = vx3[0]*vx3[0] + vx3[1]*vx3[1] + vx3[2]*vx3[2];
389
390 /* find vector of max norm */
391 if ( dd1 > dd2 ) {
392 if ( dd1 > dd3 ) {
393 dd1 = 1.0 / sqrt(dd1);
394 v[2][0] = vx1[0] * dd1;
395 v[2][1] = vx1[1] * dd1;
396 v[2][2] = vx1[2] * dd1;
397 }
398 else {
399 dd3 = 1.0 / sqrt(dd3);
400 v[2][0] = vx3[0] * dd3;
401 v[2][1] = vx3[1] * dd3;
402 v[2][2] = vx3[2] * dd3;
403 }
404 }
405 else {
406 if ( dd2 > dd3 ) {
407 dd2 = 1.0 / sqrt(dd2);
408 v[2][0] = vx2[0] * dd2;
409 v[2][1] = vx2[1] * dd2;
410 v[2][2] = vx2[2] * dd2;
411 }
412 else {
413 dd3 = 1.0 / sqrt(dd3);
414 v[2][0] = vx3[0] * dd3;
415 v[2][1] = vx3[1] * dd3;
416 v[2][2] = vx3[2] * dd3;
417 }
418 }
419
420 /* compute v1 and v2 in Im(A-vp3*Id) */
421 dd1 = w1[0]*w1[0] + w1[1]*w1[1] + w1[2]*w1[2];
422 dd2 = w2[0]*w2[0] + w2[1]*w2[1] + w2[2]*w2[2];
423 if ( dd1 > dd2 ) {
424 dd1 = 1.0 / sqrt(dd1);
425 v[0][0] = w1[0]*dd1;
426 v[0][1] = w1[1]*dd1;
427 v[0][2] = w1[2]*dd1;
428 }
429 else {
430 dd2 = 1.0 / sqrt(dd2);
431 v[0][0] = w2[0]*dd2;
432 v[0][1] = w2[1]*dd2;
433 v[0][2] = w2[2]*dd2;
434 }
435
436 /* 3rd vector orthogonal */
437 v[1][0] = v[2][1]*v[0][2] - v[2][2]*v[0][1];
438 v[1][1] = v[2][2]*v[0][0] - v[2][0]*v[0][2];
439 v[1][2] = v[2][0]*v[0][1] - v[2][1]*v[0][0];
440 dd1 = v[1][0]*v[1][0] + v[1][1]*v[1][1] + v[1][2]*v[1][2];
441 dd1 = 1.0 / sqrt(dd1);
442 v[1][0] *= dd1;
443 v[1][1] *= dd1;
444 v[1][2] *= dd1;
445 }
446
447 lambda[0] *= maxm;
448 lambda[1] *= maxm;
449 lambda[2] *= maxm;
450
451 /* check accuracy */
452 /*-------------------------------------------------------------------
453 if ( ddebug && symmat ) {
454 double err,tmpx,tmpy,tmpz;
455 float m[6];
456 int i,j;
457
458 k = 0;
459 for (i=0; i<3; i++)
460 for (j=i; j<3; j++)
461 m[k++] = lambda[0]*v[i][0]*v[j][0]
462 + lambda[1]*v[i][1]*v[j][1]
463 + lambda[2]*v[i][2]*v[j][2];
464 err = fabs(mat[0]-m[0]);
465 for (i=1; i<6; i++)
466 if ( fabs(m[i]-mat[i]) > err ) err = fabs(m[i]-mat[i]);
467
468 if ( err > 1.e03*maxm ) {
469 printf("\nProbleme eigenv3: err= %f\n",err*maxm);
470 printf("mat depart :\n");
471 printf("%13.6f %13.6f %13.6f\n",mat[0],mat[1],mat[2]);
472 printf("%13.6f %13.6f %13.6f\n",mat[1],mat[3],mat[4]);
473 printf("%13.6f %13.6f %13.6f\n",mat[2],mat[4],mat[5]);
474 printf("mat finale :\n");
475 printf("%13.6f %13.6f %13.6f\n",m[0],m[1],m[2]);
476 printf("%13.6f %13.6f %13.6f\n",m[1],m[3],m[4]);
477 printf("%13.6f %13.6f %13.6f\n",m[2],m[4],m[5]);
478 printf("lambda : %f %f %f\n",lambda[0],lambda[1],lambda[2]);
479 printf(" ordre %d\n",n);
480 printf("\nOrtho:\n");
481 printf("v1.v2 = %.14f\n",
482 v[0][0]*v[1][0]+v[0][1]*v[1][1]+ v[0][2]*v[1][2]);
483 printf("v1.v3 = %.14f\n",
484 v[0][0]*v[2][0]+v[0][1]*v[2][1]+ v[0][2]*v[2][2]);
485 printf("v2.v3 = %.14f\n",
486 v[1][0]*v[2][0]+v[1][1]*v[2][1]+ v[1][2]*v[2][2]);
487
488 printf("Consistency\n");
489 for (i=0; i<3; i++) {
490 tmpx = v[0][i]*m[0] + v[1][i]*m[1]
491 + v[2][i]*m[2] - lambda[i]*v[0][i];
492 tmpy = v[0][i]*m[1] + v[1][i]*m[3]
493 + v[2][i]*m[4] - lambda[i]*v[1][i];
494 tmpz = v[0][i]*m[2] + v[1][i]*m[4]
495 + v[2][i]*m[5] - lambda[i]*v[2][i];
496 printf(" Av %d - lambda %d *v %d = %f %f %f\n",
497 i,i,i,tmpx,tmpy,tmpz);
498
499 printf("w1 %f %f %f\n",w1[0],w1[1],w1[2]);
500 printf("w2 %f %f %f\n",w2[0],w2[1],w2[2]);
501 printf("w3 %f %f %f\n",w3[0],w3[1],w3[2]);
502 }
503 exit(1);
504 }
505 }
506 -------------------------------------------------------------------*/
507
508 return(n);
509 }
510
511
512 /* eigen value + vector extraction */
513 int eigen2(double *mm,double *lambda,double vp[2][2]) {
514 double m[3],dd,a1,xn,ddeltb,rr1,rr2,ux,uy;
515
516 /* init */
517 ux = 1.0;
518 uy = 0.0;
519
520 /* normalize */
521 memcpy(m,mm,3*sizeof(double));
522 xn = fabs(m[0]);
523 if ( fabs(m[1]) > xn ) xn = fabs(m[1]);
524 if ( fabs(m[2]) > xn ) xn = fabs(m[2]);
525 if ( xn < EPSD2 ) {
526 lambda[0] = lambda[1] = 0.0;
527 vp[0][0] = 1.0;
528 vp[0][1] = 0.0;
529 vp[1][0] = 0.0;
530 vp[1][1] = 1.0;
531 return(1);
532 }
533 xn = 1.0 / xn;
534 m[0] *= xn;
535 m[1] *= xn;
536 m[2] *= xn;
537
538 if ( egal(m[1],0.0) ) {
539 rr1 = m[0];
540 rr2 = m[2];
541 goto vect;
542 }
543
544 /* eigenvalues of jacobian */
545 a1 = -(m[0] + m[2]);
546 ddeltb = a1*a1 - 4.0 * (m[0]*m[2] - m[1]*m[1]);
547
548 if ( ddeltb < 0.0 ) {
549 fprintf(stderr," Delta: %f\n",ddeltb);
550 ddeltb = 0.0;
551 }
552 ddeltb = sqrt(ddeltb);
553
554 if ( fabs(a1) < EPS ) {
555 rr1 = 0.5 * sqrt(ddeltb);
556 rr2 = -rr1;
557 }
558 else if ( a1 < 0.0 ) {
559 rr1 = 0.5 * (-a1 + ddeltb);
560 rr2 = (-m[1]*m[1] + m[0]*m[2]) / rr1;
561 }
562 else if ( a1 > 0.0 ) {
563 rr1 = 0.5 * (-a1 - ddeltb);
564 rr2 = (-m[1]*m[1] + m[0]*m[2]) / rr1;
565 }
566 else {
567 rr1 = 0.5 * ddeltb;
568 rr2 = -rr1;
569 }
570
571 vect:
572 xn = 1.0 / xn;
573 lambda[0] = rr1 * xn;
574 lambda[1] = rr2 * xn;
575
576 /* eigenvectors */
577 a1 = m[0] - rr1;
578 if ( fabs(a1)+fabs(m[1]) < EPS ) {
579 if (fabs(lambda[1]) < fabs(lambda[0]) ) {
580 ux = 1.0;
581 uy = 0.0;
582 }
583 else {
584 ux = 0.0;
585 uy = 1.0;
586 }
587 }
588 else if ( fabs(a1) < fabs(m[1]) ) {
589 ux = 1.0;
590 uy = -a1 / m[1];
591 }
592 else if ( fabs(a1) > fabs(m[1]) ) {
593 ux = -m[1] / a1;
594 uy = 1.0;
595 }
596 else if ( fabs(lambda[1]) > fabs(lambda[0]) ) {
597 ux = 0.0;
598 uy = 1.0;
599 }
600 else {
601 ux = 1.0;
602 uy = 0.0;
603 }
604
605 dd = sqrt(ux*ux + uy*uy);
606 dd = 1.0 / dd;
607 if ( fabs(lambda[0]) > fabs(lambda[1]) ) {
608 vp[0][0] = ux * dd;
609 vp[0][1] = uy * dd;
610 }
611 else {
612 vp[0][0] = uy * dd;
613 vp[0][1] = -ux * dd;
614 }
615
616 /* orthogonal vector */
617 vp[1][0] = -vp[0][1];
618 vp[1][1] = vp[0][0];
619
620 return(1);
621 }