| 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 |
} |