1 /*
2 A* -------------------------------------------------------------------
3 B* This file contains source code for the PyMOL computer program
4 C* Copyright (c) Schrodinger, LLC.
5 D* -------------------------------------------------------------------
6 E* It is unlawful to modify or remove this copyright notice.
7 F* -------------------------------------------------------------------
8 G* Please see the accompanying LICENSE file for further information.
9 H* -------------------------------------------------------------------
10 I* Additional authors of this source file include:
11 -* Jacques Leroy (matrix inversion)
12 -* Thomas Malik (matrix multiplication)
13 -* Whoever wrote EISPACK
14 Z* -------------------------------------------------------------------
15 */
16 #include"os_python.h"
17 #include"os_predef.h"
18 #include"os_std.h"
19
20 #include"Base.h"
21 #include"Vector.h"
22 #include"Matrix.h"
23 #include"MemoryDebug.h"
24 #include"Ortho.h"
25 #include"Feedback.h"
26 #include"Setting.h"
27
28
29 /* Jenarix owned types, aliases, and defines */
30
31 #define xx_os_malloc malloc
32 #define xx_os_free free
33 #define xx_os_memcpy memcpy
34 #define xx_os_memset memset
35 #define xx_fabs fabs
36 #define xx_sqrt sqrt
37 #define xx_sizeof sizeof
38 #define xx_float64 double
39 #define xx_word int
40 #define xx_boolean int
41
42 #ifndef XX_TRUE
43 #define XX_TRUE 1
44 #endif
45 #ifndef XX_FALSE
46 #define XX_FALSE 0
47 #endif
48 #ifndef XX_NULL
49 #define XX_NULL NULL
50 #endif
51
52 #define XX_MATRIX_STACK_STORAGE_MAX 5
53
54
55 /*
56 for column-major(Fortran/OpenGL) use: (col*size + row)
57 for row-major(C) use: (row*size + col)
58 */
59
60 #define XX_MAT(skip,row,col) (((row)*(skip)) + col)
61
xx_matrix_jacobi_solve(xx_float64 * e_vec,xx_float64 * e_val,xx_word * n_rot,const xx_float64 * input,xx_word size)62 xx_boolean xx_matrix_jacobi_solve(xx_float64 * e_vec, xx_float64 * e_val,
63 xx_word * n_rot, const xx_float64 * input, xx_word size)
64
65
66 /* eigenvalues and eigenvectors for a real symmetric matrix
67 NOTE: transpose the eigenvector matrix to get eigenvectors */
68 {
69 xx_float64 stack_A_tmp[XX_MATRIX_STACK_STORAGE_MAX * XX_MATRIX_STACK_STORAGE_MAX];
70 xx_float64 stack_b_tmp[XX_MATRIX_STACK_STORAGE_MAX];
71 xx_float64 stack_z_tmp[XX_MATRIX_STACK_STORAGE_MAX];
72 xx_float64 *A_tmp = XX_NULL;
73 xx_float64 *b_tmp = XX_NULL;
74 xx_float64 *z_tmp = XX_NULL;
75 xx_boolean ok = XX_TRUE;
76
77 if(size > XX_MATRIX_STACK_STORAGE_MAX) {
78 A_tmp = (xx_float64 *) xx_os_malloc(xx_sizeof(xx_float64) * size * size);
79 b_tmp = (xx_float64 *) xx_os_malloc(xx_sizeof(xx_float64) * size);
80 z_tmp = (xx_float64 *) xx_os_malloc(xx_sizeof(xx_float64) * size);
81 if(!(A_tmp && b_tmp && z_tmp))
82 ok = XX_FALSE;
83 } else {
84 A_tmp = stack_A_tmp;
85 b_tmp = stack_b_tmp;
86 z_tmp = stack_z_tmp;
87 }
88
89 if(ok) {
90
91 xx_os_memset(e_vec, 0, xx_sizeof(xx_float64) * size * size);
92 xx_os_memcpy(A_tmp, input, xx_sizeof(xx_float64) * size * size);
93
94 {
95 xx_word p;
96 for(p = 0; p < size; p++) {
97 e_vec[XX_MAT(size, p, p)] = 1.0;
98 e_val[p] = b_tmp[p] = A_tmp[XX_MAT(size, p, p)];
99 z_tmp[p] = 0.0;
100 }
101 *n_rot = 0;
102 }
103 {
104 xx_word i;
105 for(i = 0; i < 50; i++) {
106 xx_float64 thresh;
107
108 {
109 xx_word p, q;
110 xx_float64 sm = 0.0;
111 for(p = 0; p < (size - 1); p++) {
112 for(q = p + 1; q < size; q++) {
113 sm += xx_fabs(A_tmp[XX_MAT(size, p, q)]);
114 }
115 }
116 if(sm == 0.0)
117 break;
118 if(i < 3) {
119 thresh = 0.2 * sm / (size * size);
120 } else {
121 thresh = 0.0;
122 }
123 }
124
125 {
126 xx_word p, q;
127 xx_float64 g;
128
129 for(p = 0; p < (size - 1); p++) {
130 for(q = p + 1; q < size; q++) {
131 g = 100.0 * xx_fabs(A_tmp[XX_MAT(size, p, q)]);
132 if((i > 3) &&
133 ((xx_fabs(e_val[p]) + g) == xx_fabs(e_val[p])) &&
134 ((xx_fabs(e_val[q]) + g) == xx_fabs(e_val[q]))) {
135 A_tmp[XX_MAT(size, p, q)] = 0.0;
136 } else if(xx_fabs(A_tmp[XX_MAT(size, p, q)]) > thresh) {
137 xx_float64 t;
138 xx_float64 h = e_val[q] - e_val[p];
139 if((xx_fabs(h) + g) == xx_fabs(h)) {
140 t = A_tmp[XX_MAT(size, p, q)] / h;
141 } else {
142 xx_float64 theta = 0.5 * h / A_tmp[XX_MAT(size, p, q)];
143 t = 1.0 / (xx_fabs(theta) + xx_sqrt(1.0 + theta * theta));
144 if(theta < 0.0)
145 t = -t;
146 }
147 {
148 xx_float64 c = 1.0 / xx_sqrt(1.0 + t * t);
149 xx_float64 s = t * c;
150 xx_float64 tau = s / (1.0 + c);
151
152 h = t * A_tmp[XX_MAT(size, p, q)];
153 z_tmp[p] -= h;
154 z_tmp[q] += h;
155 e_val[p] -= h;
156 e_val[q] += h;
157 A_tmp[XX_MAT(size, p, q)] = 0.0;
158 {
159 xx_word j;
160 for(j = 0; j < p; j++) {
161 g = A_tmp[XX_MAT(size, j, p)];
162 h = A_tmp[XX_MAT(size, j, q)];
163 A_tmp[XX_MAT(size, j, p)] = g - s * (h + g * tau);
164 A_tmp[XX_MAT(size, j, q)] = h + s * (g - h * tau);
165 }
166 for(j = p + 1; j < q; j++) {
167 g = A_tmp[XX_MAT(size, p, j)];
168 h = A_tmp[XX_MAT(size, j, q)];
169 A_tmp[XX_MAT(size, p, j)] = g - s * (h + g * tau);
170 A_tmp[XX_MAT(size, j, q)] = h + s * (g - h * tau);
171 }
172 for(j = q + 1; j < size; j++) {
173 g = A_tmp[XX_MAT(size, p, j)];
174 h = A_tmp[XX_MAT(size, q, j)];
175 A_tmp[XX_MAT(size, p, j)] = g - s * (h + g * tau);
176 A_tmp[XX_MAT(size, q, j)] = h + s * (g - h * tau);
177 }
178 for(j = 0; j < size; j++) {
179 g = e_vec[XX_MAT(size, j, p)];
180 h = e_vec[XX_MAT(size, j, q)];
181 e_vec[XX_MAT(size, j, p)] = g - s * (h + g * tau);
182 e_vec[XX_MAT(size, j, q)] = h + s * (g - h * tau);
183 }
184 }
185 (*n_rot)++;
186 }
187 }
188 }
189 }
190 for(p = 0; p < size; p++) {
191 b_tmp[p] += z_tmp[p];
192 e_val[p] = b_tmp[p];
193 z_tmp[p] = 0.0;
194 }
195 }
196 }
197 }
198 }
199 if(A_tmp && (A_tmp != stack_A_tmp))
200 xx_os_free(A_tmp);
201 if(b_tmp && (b_tmp != stack_b_tmp))
202 xx_os_free(b_tmp);
203 if(z_tmp && (z_tmp != stack_z_tmp))
204 xx_os_free(z_tmp);
205 return ok;
206 }
207
xx_matrix_decompose(xx_float64 * matrix,xx_word size,xx_word * permute,xx_word * parity)208 static xx_word xx_matrix_decompose(xx_float64 * matrix, xx_word size,
209 xx_word * permute, xx_word * parity)
210 {
211
212 xx_boolean ok = XX_TRUE;
213 xx_float64 stack_storage[XX_MATRIX_STACK_STORAGE_MAX];
214 xx_float64 *storage = XX_NULL;
215
216 if(size > XX_MATRIX_STACK_STORAGE_MAX) {
217 storage = (xx_float64 *) xx_os_malloc(xx_sizeof(xx_float64) * size);
218 if(!storage)
219 ok = false;
220 } else {
221 storage = stack_storage;
222 }
223
224 *parity = 1;
225
226 if(ok) {
227 xx_word i, j;
228 for(i = 0; i < size; i++) {
229 xx_float64 max_abs_value = 0.0;
230 for(j = 0; j < size; j++) {
231 xx_float64 test_abs_value = xx_fabs(matrix[XX_MAT(size, i, j)]);
232 if(max_abs_value < test_abs_value)
233 max_abs_value = test_abs_value;
234 }
235 if(max_abs_value == 0.0) {
236 ok = XX_FALSE; /* singular matrix -- no inverse */
237 break;
238 }
239 storage[i] = 1.0 / max_abs_value;
240 }
241 }
242 if(ok) {
243 xx_word i, j, k, i_max = 0;
244
245 for(j = 0; j < size; j++) {
246
247 for(i = 0; i < j; i++) {
248 xx_float64 sum = matrix[XX_MAT(size, i, j)];
249 for(k = 0; k < i; k++) {
250 sum = sum - matrix[XX_MAT(size, i, k)] * matrix[XX_MAT(size, k, j)];
251 }
252 matrix[XX_MAT(size, i, j)] = sum;
253 }
254
255 {
256 xx_float64 max_product = 0.0;
257 for(i = j; i < size; i++) {
258 xx_float64 sum = matrix[XX_MAT(size, i, j)];
259 for(k = 0; k < j; k++) {
260 sum = sum - matrix[XX_MAT(size, i, k)] * matrix[XX_MAT(size, k, j)];
261 }
262 matrix[XX_MAT(size, i, j)] = sum;
263 {
264 xx_float64 test_product = storage[i] * xx_fabs(sum);
265 if(max_product <= test_product) {
266 max_product = test_product;
267 i_max = i;
268 }
269 }
270 }
271 }
272
273 if(j != i_max) {
274 for(k = 0; k < size; k++) {
275 xx_float64 tmp = matrix[XX_MAT(size, i_max, k)];
276 matrix[XX_MAT(size, i_max, k)] = matrix[XX_MAT(size, j, k)];
277 matrix[XX_MAT(size, j, k)] = tmp;
278 }
279 *parity = (0 - *parity);
280 storage[i_max] = storage[j];
281 }
282
283 permute[j] = i_max;
284 if(matrix[XX_MAT(size, j, j)] == 0.0) {
285 /* here we have a choice: */
286 /* or (B): fail outright */
287 ok = false;
288 break;
289 }
290
291 if(j != (size - 1)) {
292 xx_float64 tmp = 1.0 / matrix[XX_MAT(size, j, j)];
293 for(i = j + 1; i < size; i++) {
294 matrix[XX_MAT(size, i, j)] *= tmp;
295 }
296 }
297 }
298 }
299 if(storage && (storage != stack_storage))
300 xx_os_free(storage);
301 return ok;
302 }
303
xx_matrix_back_substitute(xx_float64 * result,xx_float64 * decomp,xx_word size,xx_word * permute)304 static void xx_matrix_back_substitute(xx_float64 * result, xx_float64 * decomp,
305 xx_word size, xx_word * permute)
306 {
307 {
308 xx_word i, ii;
309 ii = -1;
310 for(i = 0; i < size; i++) {
311 xx_word p = permute[i];
312 xx_float64 sum = result[p];
313 result[p] = result[i];
314 if(ii >= 0) {
315 xx_word j;
316 for(j = ii; j < i; j++) {
317 sum = sum - decomp[XX_MAT(size, i, j)] * result[j];
318 }
319 } else if(sum != 0.0) {
320 ii = i;
321 }
322 result[i] = sum;
323 }
324 }
325
326 {
327 xx_word i, j;
328 for(i = size - 1; i >= 0; i--) {
329 xx_float64 sum = result[i];
330 for(j = i + 1; j < size; j++) {
331 sum = sum - decomp[XX_MAT(size, i, j)] * result[j];
332 }
333 result[i] = sum / decomp[XX_MAT(size, i, i)];
334 }
335 }
336 }
337
xx_matrix_invert(xx_float64 * result,const xx_float64 * input,xx_word size)338 xx_boolean xx_matrix_invert(xx_float64 * result, const xx_float64 * input, xx_word size)
339 {
340 xx_float64 stack_mat_tmp[XX_MATRIX_STACK_STORAGE_MAX * XX_MATRIX_STACK_STORAGE_MAX];
341 xx_float64 stack_dbl_tmp[XX_MATRIX_STACK_STORAGE_MAX];
342 xx_word stack_int_tmp[XX_MATRIX_STACK_STORAGE_MAX];
343 xx_float64 *mat_tmp = XX_NULL;
344 xx_float64 *dbl_tmp = XX_NULL;
345 xx_word *int_tmp = XX_NULL;
346 xx_word parity = 0;
347 xx_word ok = XX_TRUE;
348
349 if(size > XX_MATRIX_STACK_STORAGE_MAX) {
350 mat_tmp = (xx_float64 *) xx_os_malloc(xx_sizeof(xx_float64) * size * size);
351 dbl_tmp = (xx_float64 *) xx_os_malloc(xx_sizeof(xx_float64) * size);
352 int_tmp = (xx_word *) xx_os_malloc(xx_sizeof(xx_word) * size);
353 if(!(mat_tmp && dbl_tmp && int_tmp))
354 ok = XX_FALSE;
355 } else {
356 mat_tmp = stack_mat_tmp;
357 dbl_tmp = stack_dbl_tmp;
358 int_tmp = stack_int_tmp;
359 }
360
361 if(ok) {
362 ok = XX_FALSE;
363 memcpy(mat_tmp, input, xx_sizeof(xx_float64) * size * size);
364
365 if(xx_matrix_decompose(mat_tmp, size, int_tmp, &parity)) {
366 xx_word i, j;
367 for(j = 0; j < size; j++) {
368 memset(dbl_tmp, 0, xx_sizeof(xx_float64) * size);
369 dbl_tmp[j] = 1.0;
370 xx_matrix_back_substitute(dbl_tmp, mat_tmp, size, int_tmp);
371 for(i = 0; i < size; i++) {
372 result[XX_MAT(size, i, j)] = dbl_tmp[i];
373 }
374 }
375 ok = XX_TRUE;
376 }
377 }
378 /* clean up */
379 if(mat_tmp && (mat_tmp != stack_mat_tmp))
380 xx_os_free(mat_tmp);
381 if(dbl_tmp && (dbl_tmp != stack_dbl_tmp))
382 xx_os_free(dbl_tmp);
383 if(int_tmp && (int_tmp != stack_int_tmp))
384 xx_os_free(int_tmp);
385 return ok;
386 }
387
388 #undef XX_MAT
389
MatrixInvTransformExtentsR44d3f(const double * matrix,const float * old_min,const float * old_max,float * new_min,float * new_max)390 int MatrixInvTransformExtentsR44d3f(const double *matrix,
391 const float *old_min, const float *old_max,
392 float *new_min, float *new_max)
393 {
394 /* just brute-forcing this for now... */
395 int a;
396 int c;
397
398 double inp_min[3], inp_max[3];
399 double out_min[3], out_max[3];
400 double inp_tst[3], out_tst[3];
401
402 if(!matrix)
403 return 0;
404
405 copy3f3d(old_min, inp_min);
406 copy3f3d(old_max, inp_max);
407
408 for(c = 0; c < 8; c++) {
409 inp_tst[0] = c & 0x1 ? inp_min[0] : inp_max[0];
410 inp_tst[1] = c & 0x2 ? inp_min[1] : inp_max[1];
411 inp_tst[2] = c & 0x4 ? inp_min[2] : inp_max[2];
412
413 inverse_transform44d3d(matrix, inp_tst, out_tst);
414 if(!c) {
415 copy3d(out_tst, out_max);
416 copy3d(out_tst, out_min);
417 } else {
418 for(a = 0; a < 3; a++) {
419 if(out_min[a] > out_tst[a])
420 out_min[a] = out_tst[a];
421 if(out_max[a] < out_tst[a])
422 out_max[a] = out_tst[a];
423 }
424 }
425 }
426 copy3d3f(out_min, new_min);
427 copy3d3f(out_max, new_max);
428 return 1;
429 }
430
MatrixTransformExtentsR44d3f(const double * matrix,const float * old_min,const float * old_max,float * new_min,float * new_max)431 int MatrixTransformExtentsR44d3f(const double *matrix,
432 const float *old_min, const float *old_max,
433 float *new_min, float *new_max)
434 {
435 /* just brute-forcing this for now... */
436 int a;
437 int c;
438
439 double inp_min[3], inp_max[3];
440 double out_min[3], out_max[3];
441 double inp_tst[3], out_tst[3];
442
443 if(!matrix)
444 return 0;
445
446 copy3f3d(old_min, inp_min);
447 copy3f3d(old_max, inp_max);
448
449 for(c = 0; c < 8; c++) {
450 inp_tst[0] = c & 0x1 ? inp_min[0] : inp_max[0];
451 inp_tst[1] = c & 0x2 ? inp_min[1] : inp_max[1];
452 inp_tst[2] = c & 0x4 ? inp_min[2] : inp_max[2];
453
454 transform44d3d(matrix, inp_tst, out_tst);
455 if(!c) {
456 copy3d(out_tst, out_max);
457 copy3d(out_tst, out_min);
458 } else {
459 for(a = 0; a < 3; a++) {
460 if(out_min[a] > out_tst[a])
461 out_min[a] = out_tst[a];
462 if(out_max[a] < out_tst[a])
463 out_max[a] = out_tst[a];
464 }
465 }
466 }
467 copy3d3f(out_min, new_min);
468 copy3d3f(out_max, new_max);
469 return 1;
470 }
471
MatrixInvertC44f(const float * m,float * out)472 int MatrixInvertC44f(const float *m, float *out)
473 {
474
475 /* This routine included in PyMOL under the terms of the
476 * MIT consortium license for Brian Paul's Mesa, from which it was derived. */
477
478 /* MESA comments:
479 * Compute inverse of 4x4 transformation matrix.
480 * Code contributed by Jacques Leroy jle@star.be
481 * Return GL_TRUE for success, GL_FALSE for failure (singular matrix)
482 */
483
484 /* NB. OpenGL Matrices are COLUMN major. */
485 #define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
486 #define MAT(m,r,c) (m)[(c)*4+(r)]
487
488 float wtmp[4][8];
489 float m0, m1, m2, m3, s;
490 float *r0, *r1, *r2, *r3;
491
492 r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
493
494 r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1),
495 r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3),
496 r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0F,
497 r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1),
498 r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3),
499 r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0F,
500 r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1),
501 r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3),
502 r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0F,
503 r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1),
504 r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3), r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0F;
505
506 /* choose pivot - or die */
507 if(fabs(r3[0]) > fabs(r2[0]))
508 SWAP_ROWS(r3, r2);
509 if(fabs(r2[0]) > fabs(r1[0]))
510 SWAP_ROWS(r2, r1);
511 if(fabs(r1[0]) > fabs(r0[0]))
512 SWAP_ROWS(r1, r0);
513 if(0.0F == r0[0])
514 return 0;
515
516 /* eliminate first variable */
517 m1 = r1[0] / r0[0];
518 m2 = r2[0] / r0[0];
519 m3 = r3[0] / r0[0];
520 s = r0[1];
521 r1[1] -= m1 * s;
522 r2[1] -= m2 * s;
523 r3[1] -= m3 * s;
524 s = r0[2];
525 r1[2] -= m1 * s;
526 r2[2] -= m2 * s;
527 r3[2] -= m3 * s;
528 s = r0[3];
529 r1[3] -= m1 * s;
530 r2[3] -= m2 * s;
531 r3[3] -= m3 * s;
532 s = r0[4];
533 if(s != 0.0F) {
534 r1[4] -= m1 * s;
535 r2[4] -= m2 * s;
536 r3[4] -= m3 * s;
537 }
538 s = r0[5];
539 if(s != 0.0F) {
540 r1[5] -= m1 * s;
541 r2[5] -= m2 * s;
542 r3[5] -= m3 * s;
543 }
544 s = r0[6];
545 if(s != 0.0F) {
546 r1[6] -= m1 * s;
547 r2[6] -= m2 * s;
548 r3[6] -= m3 * s;
549 }
550 s = r0[7];
551 if(s != 0.0F) {
552 r1[7] -= m1 * s;
553 r2[7] -= m2 * s;
554 r3[7] -= m3 * s;
555 }
556
557 /* choose pivot - or die */
558 if(fabs(r3[1]) > fabs(r2[1]))
559 SWAP_ROWS(r3, r2);
560 if(fabs(r2[1]) > fabs(r1[1]))
561 SWAP_ROWS(r2, r1);
562 if(0.0F == r1[1])
563 return 0;
564
565 /* eliminate second variable */
566 m2 = r2[1] / r1[1];
567 m3 = r3[1] / r1[1];
568 r2[2] -= m2 * r1[2];
569 r3[2] -= m3 * r1[2];
570 r2[3] -= m2 * r1[3];
571 r3[3] -= m3 * r1[3];
572 s = r1[4];
573 if(0.0F != s) {
574 r2[4] -= m2 * s;
575 r3[4] -= m3 * s;
576 }
577 s = r1[5];
578 if(0.0F != s) {
579 r2[5] -= m2 * s;
580 r3[5] -= m3 * s;
581 }
582 s = r1[6];
583 if(0.0F != s) {
584 r2[6] -= m2 * s;
585 r3[6] -= m3 * s;
586 }
587 s = r1[7];
588 if(0.0F != s) {
589 r2[7] -= m2 * s;
590 r3[7] -= m3 * s;
591 }
592
593 /* choose pivot - or die */
594 if(fabs(r3[2]) > fabs(r2[2]))
595 SWAP_ROWS(r3, r2);
596 if(0.0F == r2[2])
597 return 0;
598
599 /* eliminate third variable */
600 m3 = r3[2] / r2[2];
601 r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
602 r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7];
603
604 /* last check */
605 if(0.0F == r3[3])
606 return 0;
607
608 s = 1.0F / r3[3]; /* now back substitute row 3 */
609 r3[4] *= s;
610 r3[5] *= s;
611 r3[6] *= s;
612 r3[7] *= s;
613
614 m2 = r2[3]; /* now back substitute row 2 */
615 s = 1.0F / r2[2];
616 r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
617 r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
618 m1 = r1[3];
619 r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
620 m0 = r0[3];
621 r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
622
623 m1 = r1[2]; /* now back substitute row 1 */
624 s = 1.0F / r1[1];
625 r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
626 r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
627 m0 = r0[2];
628 r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
629
630 m0 = r0[1]; /* now back substitute row 0 */
631 s = 1.0F / r0[0];
632 r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
633 r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
634
635 MAT(out, 0, 0) = r0[4];
636 MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6];
637 MAT(out, 0, 3) = r0[7], MAT(out, 1, 0) = r1[4];
638 MAT(out, 1, 1) = r1[5], MAT(out, 1, 2) = r1[6];
639 MAT(out, 1, 3) = r1[7], MAT(out, 2, 0) = r2[4];
640 MAT(out, 2, 1) = r2[5], MAT(out, 2, 2) = r2[6];
641 MAT(out, 2, 3) = r2[7], MAT(out, 3, 0) = r3[4];
642 MAT(out, 3, 1) = r3[5], MAT(out, 3, 2) = r3[6];
643 MAT(out, 3, 3) = r3[7];
644
645 return 1;
646
647 #undef MAT
648 #undef SWAP_ROWS
649 }
650
651 /*========================================================================*/
MatrixFilter(float cutoff,int window,int n_pass,int nv,const float * v1,const float * v2)652 int *MatrixFilter(float cutoff, int window, int n_pass, int nv, const float *v1, const float *v2)
653 {
654 int *flag;
655 float center1[3], center2[3];
656 int a, b, c, cc;
657 const float *vv1, *vv2;
658 float *dev, avg_dev;
659 int wc;
660 int start, finish;
661 int cnt;
662
663 flag = pymol::malloc<int>(nv); /* allocate flag matrix */
664 dev = pymol::malloc<float>(nv); /* allocate matrix for storing deviations */
665
666 for(a = 0; a < nv; a++) {
667 flag[a] = true;
668 }
669 for(c = 0; c < n_pass; c++) {
670
671 /* find the geometric center */
672
673 cc = 0;
674 copy3f(v1, center1);
675 copy3f(v2, center2);
676 for(a = 1; a < nv; a++) {
677 if(flag[a]) {
678 add3f(v1, center1, center1);
679 add3f(v2, center2, center2);
680 cc++;
681 }
682 }
683 if(cc) {
684 scale3f(center1, 1.0F / cc, center1);
685 scale3f(center2, 1.0F / cc, center2);
686 }
687
688 /* store average deviation from it */
689 avg_dev = 0.0F;
690 cc = 0;
691 for(a = 0; a < nv; a++) {
692 if(flag[a]) {
693 vv1 = v1 + 3 * a;
694 vv2 = v2 + 3 * a;
695 dev[a] = (float) fabs(diff3f(center1, vv1) - diff3f(center2, vv2));
696 avg_dev += dev[a];
697 cc++;
698 }
699 }
700 if(cc) {
701 avg_dev /= cc;
702
703 if(avg_dev > R_SMALL4) {
704
705 /* eliminate pairs that are greater than cutoff */
706
707 for(a = 0; a < nv; a++) {
708 if((dev[a] / avg_dev) > cutoff)
709 flag[a] = false;
710 dev[a] = 0.0F;
711 }
712
713 /* the grossest outliers have been eliminated -- now for the more subtle ones... */
714
715 /* run a sliding window along the sequence, measuring how deviations of a particular atom compare
716 * to the surrounding atoms. Then eliminate the outliers. */
717
718 for(a = 0; a < nv; a++) {
719 if(flag[a]) {
720
721 /* define a window of active pairs surrounding this residue */
722
723 cnt = window;
724 b = a;
725 start = a;
726 finish = a;
727 while(cnt > (window / 2)) { /* back up to half window size */
728 if(b < 0)
729 break;
730 if(flag[b]) {
731 start = b;
732 cnt--;
733 }
734 b--;
735 }
736 b = a + 1;
737 while(cnt > 0) { /* go forward to complete window */
738 if(b >= nv)
739 break;
740 if(flag[b]) {
741 finish = b;
742 cnt--;
743 }
744 b++;
745 }
746 b = start - 1;
747 while(cnt > 0) { /* back up more if necc. */
748 if(b < 0)
749 break;
750 if(flag[b]) {
751 start = b;
752 cnt--;
753 }
754 b--;
755 }
756
757 if((finish - start) >= window) {
758
759 /* compute geometric center of the window */
760
761 wc = 0;
762 for(b = start; b <= finish; b++)
763 if(flag[b]) {
764 vv1 = v1 + 3 * b;
765 vv2 = v2 + 3 * b;
766 if(!wc) {
767 copy3f(vv1, center1);
768 copy3f(vv2, center2);
769 } else {
770 add3f(v1, center1, center1);
771 add3f(v2, center2, center2);
772 }
773 wc++;
774 }
775 if(wc) {
776 scale3f(center1, 1.0F / wc, center1);
777 scale3f(center2, 1.0F / wc, center2);
778
779 /* compute average deviation over window */
780 avg_dev = 0.0F;
781 wc = 0;
782 for(b = start; b <= finish; b++)
783 if(flag[b]) {
784 vv1 = v1 + 3 * b;
785 vv2 = v2 + 3 * b;
786 avg_dev += (float) fabs(diff3f(center1, vv1) - diff3f(center2, vv2));
787 wc++;
788 }
789 if(wc) { /* store ratio of the actual vs. average deviation */
790 avg_dev /= wc;
791 vv1 = v1 + 3 * a;
792 vv2 = v2 + 3 * a;
793 if(avg_dev > R_SMALL4)
794 dev[a] =
795 (float) fabs(diff3f(center1, vv1) - diff3f(center2, vv2)) / avg_dev;
796 else
797 dev[a] = 0.0F;
798 }
799 }
800 }
801 }
802 }
803 /* now eliminate those above cutoff */
804 for(a = 0; a < nv; a++)
805 if(flag[a])
806 if(dev[a] > cutoff)
807 flag[a] = false;
808 }
809 }
810 }
811 FreeP(dev);
812 return flag;
813 }
814
815
816 /*========================================================================*/
MatrixTransformTTTfN3f(unsigned int n,float * q,const float * m,const float * p)817 void MatrixTransformTTTfN3f(unsigned int n, float *q, const float *m, const float *p)
818 {
819 const float m0 = m[0], m4 = m[4], m8 = m[8], m12 = m[12];
820 const float m1 = m[1], m5 = m[5], m9 = m[9], m13 = m[13];
821 const float m2 = m[2], m6 = m[6], m10 = m[10], m14 = m[14];
822 const float m3 = m[3], m7 = m[7], m11 = m[11];
823 float p0, p1, p2;
824 while(n--) {
825 p0 = *(p++) + m12;
826 p1 = *(p++) + m13;
827 p2 = *(p++) + m14;
828 *(q++) = m0 * p0 + m1 * p1 + m2 * p2 + m3;
829 *(q++) = m4 * p0 + m5 * p1 + m6 * p2 + m7;
830 *(q++) = m8 * p0 + m9 * p1 + m10 * p2 + m11;
831 }
832 }
833
834
835 /*========================================================================*/
MatrixTransformR44fN3f(unsigned int n,float * q,const float * m,const float * p)836 void MatrixTransformR44fN3f(unsigned int n, float *q, const float *m, const float *p)
837 {
838 const float m0 = m[0], m4 = m[4], m8 = m[8];
839 const float m1 = m[1], m5 = m[5], m9 = m[9];
840 const float m2 = m[2], m6 = m[6], m10 = m[10];
841 const float m3 = m[3], m7 = m[7], m11 = m[11];
842 float p0, p1, p2;
843 while(n--) {
844 p0 = *(p++);
845 p1 = *(p++);
846 p2 = *(p++);
847 *(q++) = m0 * p0 + m1 * p1 + m2 * p2 + m3;
848 *(q++) = m4 * p0 + m5 * p1 + m6 * p2 + m7;
849 *(q++) = m8 * p0 + m9 * p1 + m10 * p2 + m11;
850 }
851 }
852
853
854 /*========================================================================*/
MatrixGetRotationC44f(float * m44,float angle,float x,float y,float z)855 void MatrixGetRotationC44f(float *m44, float angle,
856 float x, float y, float z)
857 {
858 float m33[9];
859 rotation_matrix3f(angle, x, y, z, m33);
860 m44[0] = m33[0];
861 m44[1] = m33[3];
862 m44[2] = m33[6];
863 m44[3] = 0.0F;
864 m44[4] = m33[1];
865 m44[5] = m33[4];
866 m44[6] = m33[7];
867 m44[7] = 0.0F;
868 m44[8] = m33[2];
869 m44[9] = m33[5];
870 m44[10] = m33[8];
871 m44[11] = 0.0F;
872 m44[12] = 0.0F;
873 m44[13] = 0.0F;
874 m44[14] = 0.0F;
875 m44[15] = 1.0F;
876 }
877
878
879 /*========================================================================*/
MatrixRotateC44f(float * m,float angle,float x,float y,float z)880 void MatrixRotateC44f(float *m, float angle, float x, float y, float z)
881 {
882 float m33[9];
883 float m44[16];
884 rotation_matrix3f(angle, x, y, z, m33);
885 m44[0] = m33[0];
886 m44[1] = m33[1];
887 m44[2] = m33[2];
888 m44[3] = 0.0F;
889 m44[4] = m33[3];
890 m44[5] = m33[4];
891 m44[6] = m33[5];
892 m44[7] = 0.0F;
893 m44[8] = m33[6];
894 m44[9] = m33[7];
895 m44[10] = m33[8];
896 m44[11] = 0.0F;
897 m44[12] = 0.0F;
898 m44[13] = 0.0F;
899 m44[14] = 0.0F;
900 m44[15] = 1.0F;
901 MatrixMultiplyC44f(m44, m);
902 }
903
904
905 /*========================================================================*/
MatrixTranslateC44f(float * m,float x,float y,float z)906 void MatrixTranslateC44f(float *m, float x, float y, float z)
907 {
908 m[12] = m[0] * x + m[4] * y + m[8] * z + m[12];
909 m[13] = m[1] * x + m[5] * y + m[9] * z + m[13];
910 m[14] = m[2] * x + m[6] * y + m[10] * z + m[14];
911 m[15] = m[3] * x + m[7] * y + m[11] * z + m[15];
912 }
913
914
915 /*========================================================================*/
MatrixMultiplyC44f(const float * b,float * m)916 void MatrixMultiplyC44f(const float *b, float *m)
917 {
918 /* This routine included in PyMOL under the terms of the
919 * MIT consortium license for Brian Paul's Mesa, from which it was derived. */
920 /* original author: Thomas Malik */
921
922 int i;
923
924 #define A(row,col) m[(col<<2)+row]
925 #define B(row,col) b[(col<<2)+row]
926 #define P(row,col) m[(col<<2)+row]
927
928 /* i-te Zeile */
929 for(i = 0; i < 4; i++) {
930 float ai0 = A(i, 0), ai1 = A(i, 1), ai2 = A(i, 2), ai3 = A(i, 3);
931 P(i, 0) = ai0 * B(0, 0) + ai1 * B(1, 0) + ai2 * B(2, 0) + ai3 * B(3, 0);
932 P(i, 1) = ai0 * B(0, 1) + ai1 * B(1, 1) + ai2 * B(2, 1) + ai3 * B(3, 1);
933 P(i, 2) = ai0 * B(0, 2) + ai1 * B(1, 2) + ai2 * B(2, 2) + ai3 * B(3, 2);
934 P(i, 3) = ai0 * B(0, 3) + ai1 * B(1, 3) + ai2 * B(2, 3) + ai3 * B(3, 3);
935 }
936
937 #undef A
938 #undef B
939 #undef P
940 }
941
942
943 /*========================================================================*/
MatrixTransformC44f3f(const float * m,const float * q,float * p)944 void MatrixTransformC44f3f(const float *m, const float *q, float *p)
945 {
946 float q0 = *q, q1 = *(q + 1), q2 = *(q + 2);
947 p[0] = m[0] * q0 + m[4] * q1 + m[8] * q2 + m[12];
948 p[1] = m[1] * q0 + m[5] * q1 + m[9] * q2 + m[13];
949 p[2] = m[2] * q0 + m[6] * q1 + m[10] * q2 + m[14];
950 }
951
952
953 /*========================================================================*/
MatrixTransformC44f4f(const float * m,const float * q,float * p)954 void MatrixTransformC44f4f(const float *m, const float *q, float *p)
955 {
956 float q0 = *q, q1 = *(q + 1), q2 = *(q + 2);
957 p[0] = m[0] * q0 + m[4] * q1 + m[8] * q2 + m[12];
958 p[1] = m[1] * q0 + m[5] * q1 + m[9] * q2 + m[13];
959 p[2] = m[2] * q0 + m[6] * q1 + m[10] * q2 + m[14];
960 p[3] = m[3] * q0 + m[7] * q1 + m[11] * q2 + m[15];
961 }
962
963
964 /*========================================================================*/
965 // same as transform44f3fas33f3f
MatrixInvTransformC44fAs33f3f(const float * m,const float * q,float * p)966 void MatrixInvTransformC44fAs33f3f(const float *m, const float *q, float *p)
967 {
968 /* multiplying a column major rotation matrix as row-major will
969 * give the inverse rotation */
970 float q0 = *q, q1 = *(q + 1), q2 = *(q + 2);
971 p[0] = m[0] * q0 + m[1] * q1 + m[2] * q2;
972 p[1] = m[4] * q0 + m[5] * q1 + m[6] * q2;
973 p[2] = m[8] * q0 + m[9] * q1 + m[10] * q2;
974 }
975
976
977 /*========================================================================*/
MatrixTransformC44fAs33f3f(const float * m,const float * q,float * p)978 void MatrixTransformC44fAs33f3f(const float *m, const float *q, float *p)
979 {
980 float q0 = *q, q1 = *(q + 1), q2 = *(q + 2);
981 p[0] = m[0] * q0 + m[4] * q1 + m[8] * q2;
982 p[1] = m[1] * q0 + m[5] * q1 + m[9] * q2;
983 p[2] = m[2] * q0 + m[6] * q1 + m[10] * q2;
984 }
985
986
987 /*========================================================================*/
MatrixGetRMS(PyMOLGlobals * G,int n,const float * v1,const float * v2,float * wt)988 float MatrixGetRMS(PyMOLGlobals * G, int n, const float *v1, const float *v2, float *wt)
989 {
990 /* Just Compute RMS given current coordinates */
991
992 const float *vv1, *vv2;
993 float err, etmp, tmp;
994 int a, c;
995 float sumwt = 0.0F;
996
997 if(wt) {
998 for(c = 0; c < n; c++)
999 if(wt[c] != 0.0F) {
1000 sumwt = sumwt + wt[c];
1001 }
1002 } else {
1003 for(c = 0; c < n; c++)
1004 sumwt += 1.0F;
1005 }
1006 err = 0.0F;
1007 vv1 = v1;
1008 vv2 = v2;
1009 for(c = 0; c < n; c++) {
1010 etmp = 0.0F;
1011 for(a = 0; a < 3; a++) {
1012 tmp = (vv2[a] - vv1[a]);
1013 etmp += tmp * tmp;
1014 }
1015 if(wt)
1016 err += wt[c] * etmp;
1017 else
1018 err += etmp;
1019 vv1 += 3;
1020 vv2 += 3;
1021 }
1022 err = err / sumwt;
1023 err = (float) sqrt1f(err);
1024
1025 if(fabs(err) < R_SMALL4)
1026 err = 0.0F;
1027
1028 return (err);
1029 }
1030
1031
1032 /*========================================================================*/
MatrixFitRMSTTTf(PyMOLGlobals * G,int n,const float * v1,const float * v2,const float * wt,float * ttt)1033 float MatrixFitRMSTTTf(PyMOLGlobals * G, int n, const float *v1, const float *v2, const float *wt,
1034 float *ttt)
1035 {
1036 /*
1037 Subroutine to do the actual RMS fitting of two sets of vector coordinates
1038 This routine does not rotate the actual coordinates, but instead returns
1039 the RMS fitting value, along with the center-of-mass translation vectors
1040 T1 and T2 and the rotation matrix M, which rotates the translated
1041 coordinates of molecule 2 onto the translated coordinates of molecule 1.
1042 */
1043
1044 double m[3][3], aa[3][3];
1045 double sumwt, tol;
1046 int a, b, c, maxiter;
1047 double t1[3], t2[3];
1048
1049 /* special case: just one atom pair */
1050
1051 if(n == 1) {
1052 if(ttt) {
1053 for(a = 1; a < 11; a++)
1054 ttt[a] = 0.0F;
1055 for(a = 0; a < 12; a += 5)
1056 ttt[a] = 1.0F;
1057 for(a = 0; a < 3; a++)
1058 ttt[a + 12] = v2[a] - v1[a];
1059 }
1060 return 0.0F;
1061 }
1062
1063 /* Initialize arrays. */
1064
1065 for(a = 0; a < 3; a++) {
1066 for(b = 0; b < 3; b++) {
1067 aa[a][b] = 0.0;
1068 }
1069 t1[a] = 0.0;
1070 t2[a] = 0.0;
1071 }
1072
1073 sumwt = 0.0F;
1074 tol = SettingGetGlobal_f(G, cSetting_fit_tolerance);
1075 maxiter = SettingGetGlobal_i(G, cSetting_fit_iterations);
1076
1077 /* Calculate center-of-mass vectors */
1078
1079 {
1080 const float *vv1 = v1, *vv2 = v2;
1081
1082 if(wt) {
1083 for(c = 0; c < n; c++) {
1084 for(a = 0; a < 3; a++) {
1085 t1[a] += wt[c] * vv1[a];
1086 t2[a] += wt[c] * vv2[a];
1087 }
1088 if(wt[c] != 0.0F) {
1089 sumwt = sumwt + wt[c];
1090 } else {
1091 sumwt = sumwt + 1.0F; /* WHAT IS THIS? */
1092 }
1093 vv1 += 3;
1094 vv2 += 3;
1095 }
1096 } else {
1097 for(c = 0; c < n; c++) {
1098 for(a = 0; a < 3; a++) {
1099 t1[a] += vv1[a];
1100 t2[a] += vv2[a];
1101 }
1102 sumwt += 1.0F;
1103 vv1 += 3;
1104 vv2 += 3;
1105 }
1106 }
1107 if(sumwt == 0.0F)
1108 sumwt = 1.0F;
1109 for(a = 0; a < 3; a++) {
1110 t1[a] /= sumwt;
1111 t2[a] /= sumwt;
1112 }
1113 }
1114
1115 {
1116 /* Calculate correlation matrix */
1117 double x[3], xx[3];
1118 const float *vv1 = v1, *vv2 = v2;
1119 for(c = 0; c < n; c++) {
1120 if(wt) {
1121 for(a = 0; a < 3; a++) {
1122 x[a] = wt[c] * (vv1[a] - t1[a]);
1123 xx[a] = wt[c] * (vv2[a] - t2[a]);
1124 }
1125 } else {
1126 for(a = 0; a < 3; a++) {
1127 x[a] = vv1[a] - t1[a];
1128 xx[a] = vv2[a] - t2[a];
1129 }
1130 }
1131 for(a = 0; a < 3; a++)
1132 for(b = 0; b < 3; b++)
1133 aa[a][b] = aa[a][b] + xx[a] * x[b];
1134 vv1 += 3;
1135 vv2 += 3;
1136 }
1137 }
1138
1139 if(n > 1) {
1140 int got_kabsch = false;
1141 int fit_kabsch = SettingGetGlobal_i(G, cSetting_fit_kabsch);
1142 if(fit_kabsch) {
1143
1144 /* WARNING: Kabsch isn't numerically stable */
1145
1146 /* Kabsch as per
1147
1148 http://en.wikipedia.org/wiki/Kabsch_algorithm
1149
1150 minimal RMS matrix is (AtA)^(1/2) * A_inverse, where
1151
1152 Aij = Pki Qkj
1153
1154 assuming Pki and Qkj are centered about the same origin.
1155
1156 */
1157
1158 /* NOTE: This Kabsch implementation only works with 4 or more atoms */
1159
1160 double At[3][3], AtA[3][3];
1161
1162 /* compute At and At * A */
1163
1164 transpose33d33d((double *) (void *) aa, (double *) (void *) At);
1165
1166 multiply33d33d((double *) (void *) At, (double *) (void *) aa,
1167 (double *) (void *) AtA);
1168
1169 /* solve A*At (a real symmetric matrix) */
1170
1171 {
1172 double e_vec[3][3], e_val[3];
1173 xx_word n_rot;
1174
1175 if(xx_matrix_jacobi_solve((xx_float64 *) (void *) e_vec,
1176 (xx_float64 *) (void *) e_val,
1177 &n_rot, (xx_float64 *) (void *) AtA, 3)) {
1178 double V[3][3], Vt[3][3];
1179
1180 /* Kabsch requires non-negative eigenvalues (since sqrt is taken) */
1181
1182 if((e_val[0] >= 0.0) && (e_val[1] >= 0.0) && (e_val[2] >= 0.0)) {
1183 switch (fit_kabsch) {
1184 case 1:
1185 {
1186
1187 /* original Kabsch performs an unnecessary matrix inversion */
1188
1189 /* rot matrix = (AtA)^(1/2) * A^(-1) */
1190
1191 double rootD[3][3], sqrtAtA[3][3], Ai[3][3];
1192
1193 for(a = 0; a < 3; a++)
1194 for(b = 0; b < 3; b++) {
1195 if(a == b) {
1196 rootD[a][b] = sqrt1d(e_val[a]);
1197 } else {
1198 rootD[a][b] = 0.0;
1199 }
1200 V[a][b] = e_vec[a][b];
1201 Vt[a][b] = e_vec[b][a];
1202 }
1203
1204 multiply33d33d((double *) (void *) rootD, (double *) (void *) Vt,
1205 (double *) (void *) sqrtAtA);
1206 multiply33d33d((double *) (void *) V, (double *) (void *) sqrtAtA,
1207 (double *) (void *) sqrtAtA);
1208 /* compute Ai */
1209
1210 if(xx_matrix_invert((xx_float64 *) (void *) Ai,
1211 (xx_float64 *) (void *) aa, 3)) {
1212
1213 /* now compute the rotation matrix = (AtA)^(1/2) * Ai */
1214
1215 multiply33d33d((double *) (void *) sqrtAtA,
1216 (double *) (void *) Ai, (double *) (void *) m);
1217
1218 got_kabsch = true;
1219
1220 { /* is the rotation matrix left-handed? Then swap the
1221 recomposition so as to avoid the reflection */
1222 double cp[3], dp;
1223 cross_product3d((double *) (void *) m, (double *) (void *) m + 3, cp);
1224 dp = dot_product3d((double *) (void *) m + 6, cp);
1225 if((1.0 - fabs(dp)) > R_SMALL4) { /* not orthonormal? */
1226 got_kabsch = false;
1227
1228 PRINTFB(G, FB_Matrix, FB_Warnings)
1229 "Matrix-Warning: Kabsch matrix not orthonormal: falling back on iteration.\n"
1230 ENDFB(G);
1231
1232 } else if(dp < 0.0F) {
1233 multiply33d33d((double *) (void *) rootD, (double *) (void *) V,
1234 (double *) (void *) sqrtAtA);
1235 multiply33d33d((double *) (void *) Vt, (double *) (void *) sqrtAtA,
1236 (double *) (void *) sqrtAtA);
1237 multiply33d33d((double *) (void *) sqrtAtA, (double *) (void *) Ai,
1238 (double *) (void *) m);
1239 }
1240 }
1241 } else {
1242 PRINTFB(G, FB_Matrix, FB_Warnings)
1243 "Matrix-Warning: Kabsch matrix inversion failed: falling back on iteration.\n"
1244 ENDFB(G);
1245 }
1246 }
1247 break;
1248 case 2:
1249 {
1250 /* improved Kabsch skips the matrix inversion */
1251
1252 if((e_val[0] > R_SMALL8) &&
1253 (e_val[1] > R_SMALL8) && (e_val[2] > R_SMALL8)) {
1254
1255 /* inv rot matrix = U Vt = A V D^(-1/2) Vt
1256 * where U from SVD of A = U S Vt
1257 * and where U is known to be = A V D^(-1/2)
1258 * where D are eigenvalues of AtA */
1259
1260 double invRootD[3][3], Mi[3][3];
1261
1262 for(a = 0; a < 3; a++)
1263 for(b = 0; b < 3; b++) {
1264 if(a == b) {
1265 invRootD[a][b] = 1.0 / sqrt1d(e_val[a]);
1266 } else {
1267 invRootD[a][b] = 0.0;
1268 }
1269 V[a][b] = e_vec[a][b];
1270 Vt[a][b] = e_vec[b][a];
1271 }
1272
1273 /* now compute the rotation matrix directly */
1274
1275 multiply33d33d((double *) (void *) invRootD, (double *) (void *) Vt,
1276 (double *) (void *) Mi);
1277 multiply33d33d((double *) (void *) V, (double *) (void *) Mi,
1278 (double *) (void *) Mi);
1279 multiply33d33d((double *) (void *) aa, (double *) (void *) Mi,
1280 (double *) (void *) Mi);
1281
1282 got_kabsch = true;
1283
1284 { /* cis the rotation matrix left-handed? Then swap the
1285 recomposition so as to avoid the reflection */
1286 double cp[3], dp;
1287 cross_product3d((double *) (void *) Mi, (double *) (void *) Mi + 3,
1288 cp);
1289 dp = dot_product3d((double *) (void *) Mi + 6, cp);
1290 if((1.0 - fabs(dp)) > R_SMALL4) { /* not orthonormal? */
1291 got_kabsch = false;
1292
1293 PRINTFB(G, FB_Matrix, FB_Warnings)
1294 "Matrix-Warning: Kabsch matrix not orthonormal: falling back on iteration.\n"
1295 ENDFB(G);
1296
1297 } else if(dp < 0.0F) {
1298 multiply33d33d((double *) (void *) invRootD, (double *) (void *) V,
1299 (double *) (void *) Mi);
1300 multiply33d33d((double *) (void *) Vt, (double *) (void *) Mi,
1301 (double *) (void *) Mi);
1302 multiply33d33d((double *) (void *) aa, (double *) (void *) Mi,
1303 (double *) (void *) Mi);
1304 }
1305 }
1306
1307 if(got_kabsch) { /* transpose to get the inverse */
1308 transpose33d33d((double *) (void *) Mi, (double *) (void *) m);
1309 }
1310
1311 } else {
1312 PRINTFB(G, FB_Matrix, FB_Warnings)
1313 "Matrix-Warning: Kabsch matrix degenerate: falling back on iteration.\n"
1314 ENDFB(G);
1315 }
1316 }
1317 break;
1318 }
1319
1320 /* validate the result */
1321
1322 if(got_kabsch) {
1323
1324 if((fabs(length3d(m[0]) - 1.0) < R_SMALL4) &&
1325 (fabs(length3d(m[1]) - 1.0) < R_SMALL4) &&
1326 (fabs(length3d(m[2]) - 1.0) < R_SMALL4)) {
1327
1328 recondition33d((double *) (void *) m);
1329
1330 } else {
1331 got_kabsch = false;
1332
1333 PRINTFB(G, FB_Matrix, FB_Warnings)
1334 "Matrix-Warning: Kabsch matrix not normal: falling back on iteration.\n"
1335 ENDFB(G);
1336 }
1337 }
1338 } else {
1339 PRINTFB(G, FB_Matrix, FB_Warnings)
1340 "Matrix-Warning: Kabsch eigenvalue(s) negative: falling back on iteration.\n"
1341 ENDFB(G);
1342 }
1343 } else {
1344 PRINTFB(G, FB_Matrix, FB_Warnings)
1345 "Matrix-Warning: Kabsch decomposition failed: falling back on iteration.\n"
1346 ENDFB(G);
1347 }
1348 }
1349 }
1350
1351 if(!got_kabsch) {
1352
1353 /* use PyMOL's original iteration algorithm if Kabsch is
1354 disabled or fails (quite common). */
1355
1356 /* Primary iteration scheme to determine rotation matrix for molecule 2 */
1357 double sg, bb, cc;
1358 int iters, iy, iz, unchanged = 0;
1359 double sig, gam;
1360 double tmp;
1361 double save[3][3];
1362 int perturbed = false;
1363
1364 for(a = 0; a < 3; a++) {
1365 for(b = 0; b < 3; b++) {
1366 m[a][b] = 0.0;
1367 save[a][b] = aa[a][b];
1368 }
1369 m[a][a] = 1.0;
1370 }
1371
1372 iters = 0;
1373 while(1) {
1374
1375 /* IX, IY, and IZ rotate 1-2-3, 2-3-1, 3-1-2, etc. */
1376 iz = (iters + 1) % 3;
1377 iy = (iz + 1) % 3;
1378 sig = aa[iz][iy] - aa[iy][iz];
1379 gam = aa[iy][iy] + aa[iz][iz];
1380
1381 if(iters >= maxiter) {
1382 PRINTFB(G, FB_Matrix, FB_Details)
1383 " Matrix: Warning: no convergence (%1.8f<%1.8f after %d iterations).\n",
1384 (float) tol, (float) gam, iters ENDFB(G);
1385 break;
1386 }
1387
1388 /* Determine size of off-diagonal element. If off-diagonals exceed the
1389 diagonal elements tolerance, perform Jacobi rotation. */
1390 tmp = sig * sig + gam * gam;
1391 sg = sqrt1d(tmp);
1392 if((sg != 0.0F) && (fabs(sig) > (tol * fabs(gam)))) {
1393 unchanged = 0;
1394 sg = 1.0F / sg;
1395 for(a = 0; a < 3; a++) {
1396 bb = gam * aa[iy][a] + sig * aa[iz][a];
1397 cc = gam * aa[iz][a] - sig * aa[iy][a];
1398 aa[iy][a] = bb * sg;
1399 aa[iz][a] = cc * sg;
1400
1401 bb = gam * m[iy][a] + sig * m[iz][a];
1402 cc = gam * m[iz][a] - sig * m[iy][a];
1403 m[iy][a] = bb * sg;
1404 m[iz][a] = cc * sg;
1405 }
1406 } else {
1407 unchanged++;
1408 if(unchanged == 3) {
1409 double residual = 0.0;
1410 for(a = 0; a < 3; a++) {
1411 for(b = 0; b < 3; b++) {
1412 residual += fabs(aa[a][b] - save[a][b]);
1413 }
1414 }
1415 if(residual > R_SMALL4) {
1416 /* matrix has changed significantly, so we assume that
1417 we found the minimum */
1418 break;
1419 } else if(perturbed) {
1420 /* we ended up back where we started even after perturbing */
1421 break;
1422 } else { /* hmm...no change from start... so displace 90
1423 degrees just to make sure we didn't start out
1424 trapped in precisely the opposite direction */
1425 for(a = 0; a < 3; a++) {
1426 bb = aa[iz][a];
1427 cc = -aa[iy][a];
1428 aa[iy][a] = bb;
1429 aa[iz][a] = cc;
1430
1431 bb = m[iz][a];
1432 cc = -m[iy][a];
1433 m[iy][a] = bb;
1434 m[iz][a] = cc;
1435 }
1436 perturbed = true;
1437 unchanged = 0;
1438 }
1439 /* only give up if we've iterated through all three planes */
1440 }
1441 }
1442 iters++;
1443 }
1444 recondition33d((double *) (void *) m);
1445 }
1446 }
1447
1448 /* At this point, we should have a converged rotation matrix (M). Calculate
1449 the weighted RMS error. */
1450 {
1451 const float *vv1 = v1, *vv2 = v2;
1452 double etmp, tmp;
1453 double err = 0.0;
1454 for(c = 0; c < n; c++) {
1455 etmp = 0.0;
1456 for(a = 0; a < 3; a++) {
1457 tmp = m[a][0] * (vv2[0] - t2[0])
1458 + m[a][1] * (vv2[1] - t2[1])
1459 + m[a][2] * (vv2[2] - t2[2]);
1460 tmp = (vv1[a] - t1[a]) - tmp;
1461 etmp += tmp * tmp;
1462 }
1463 if(wt)
1464 err += wt[c] * etmp;
1465 else
1466 err += etmp;
1467 vv1 += 3;
1468 vv2 += 3;
1469 }
1470
1471 err = err / sumwt;
1472 err = sqrt1d(err);
1473
1474 /* NOTE: TTT's are now row-major (to be more like homogenous matrices) */
1475
1476 if(ttt) {
1477 ttt[0] = (float) m[0][0];
1478 ttt[1] = (float) m[1][0];
1479 ttt[2] = (float) m[2][0];
1480 ttt[3] = (float) t2[0];
1481 ttt[4] = (float) m[0][1];
1482 ttt[5] = (float) m[1][1];
1483 ttt[6] = (float) m[2][1];
1484 ttt[7] = (float) t2[1];
1485 ttt[8] = (float) m[0][2];
1486 ttt[9] = (float) m[1][2];
1487 ttt[10] = (float) m[2][2];
1488 ttt[11] = (float) t2[2];
1489 ttt[12] = (float) -t1[0];
1490 ttt[13] = (float) -t1[1];
1491 ttt[14] = (float) -t1[2];
1492 }
1493 /* for compatibility with normal 4x4 matrices */
1494
1495 if(fabs(err) < R_SMALL4)
1496 err = 0.0F;
1497
1498 return ((float) err);
1499 }
1500 }
1501
1502
1503 /*========================================================================*/
1504
1505
1506 /*========================================================================*/
1507
1508
1509 /*========================================================================*/
1510
1511
1512 /*========================================================================*/
1513
1514
1515 /*========================================================================*/
1516
1517
1518 /*========================================================================*/
1519
1520
1521 /*========================================================================*/
1522
1523
1524 /* Only a partial translation of eispack -
1525 * just the real general diagonialization and support routines */
1526
1527
1528 /* eispack.f -- translated by f2c (version 19970805).*/
1529
1530 typedef unsigned int uinteger;
1531 typedef char *address;
1532 typedef short int shortint;
1533 typedef float real;
1534 typedef struct {
1535 real r, i;
1536 } complex;
1537 typedef struct {
1538 doublereal r, i;
1539 } doublecomplex;
1540 typedef int logical;
1541 typedef short int shortlogical;
1542 typedef char logical1;
1543 typedef char integer1;
1544
1545 #define TRUE_ (1)
1546 #define FALSE_ (0)
1547
1548
1549 /* Extern is for use with -E */
1550 #ifndef Extern
1551 #define Extern extern
1552 #endif
1553
1554
1555 /* I/O stuff */
1556
1557 #ifdef f2c_i2
1558
1559
1560 /* for -i2 */
1561 typedef short flag;
1562 typedef short ftnlen;
1563 typedef short ftnint;
1564 #else
1565 typedef long int flag;
1566 typedef long int ftnlen;
1567 typedef long int ftnint;
1568 #endif
1569
1570 #define VOID void
1571
1572 #define abs(x) ((x) >= 0 ? (x) : -(x))
1573 #define dabs(x) (doublereal)abs(x)
1574 #define mymin(a,b) ((a) <= (b) ? (a) : (b))
1575 #define mymax(a,b) ((a) >= (b) ? (a) : (b))
1576 #define dmymin(a,b) (doublereal)mymin(a,b)
1577 #define dmymax(a,b) (doublereal)mymax(a,b)
1578 #define bit_test(a,b) ((a) >> (b) & 1)
1579 #define bit_clear(a,b) ((a) & ~((uinteger)1 << (b)))
1580 #define bit_set(a,b) ((a) | ((uinteger)1 << (b)))
1581
1582
1583 /*========================================================================*/
1584
MatrixEigensolveC33d(PyMOLGlobals * G,const double * a,double * wr,double * wi,double * v)1585 int MatrixEigensolveC33d(PyMOLGlobals * G, const double *a, double *wr, double *wi, double *v)
1586 {
1587 integer n, nm;
1588 integer iv1[3];
1589 integer matz;
1590 double fv1[9];
1591 double at[9];
1592 integer ierr;
1593 int x;
1594
1595 nm = 3;
1596 n = 3;
1597 matz = 1;
1598
1599 for(x = 0; x < 9; x++) /* make a copy -- eispack trashes the matrix */
1600 at[x] = a[x];
1601
1602 pymol_rg_(&nm, &n, at, wr, wi, &matz, v, iv1, fv1, &ierr);
1603
1604 /* NOTE: the returned eigenvectors are stored one per row which is
1605 is actually the inverse of the normal eigenvalue matrix -
1606 ----
1607 IS that because we're actually solving the transpose?
1608 */
1609
1610 if(Feedback(G, FB_Matrix, FB_Blather)) {
1611 printf(" Eigensolve: eigenvectors %8.3f %8.3f %8.3f\n", v[0], v[1], v[2]);
1612 printf(" Eigensolve: %8.3f %8.3f %8.3f\n", v[3], v[4], v[5]);
1613 printf(" Eigensolve: %8.3f %8.3f %8.3f\n", v[6], v[7], v[8]);
1614
1615 printf(" Eigensolve: eigenvalues %8.3f %8.3f %8.3f\n", wr[0], wr[1], wr[2]);
1616 printf(" Eigensolve: %8.3f %8.3f %8.3f\n", wi[0], wi[1], wi[2]);
1617 }
1618 return (ierr);
1619 }
1620
MatrixEigensolveC44d(PyMOLGlobals * G,const double * a,double * wr,double * wi,double * v)1621 int MatrixEigensolveC44d(PyMOLGlobals * G, const double *a, double *wr, double *wi, double *v)
1622 {
1623 integer n, nm;
1624 integer iv1[4];
1625 integer matz;
1626 double fv1[16];
1627 double at[16];
1628 integer ierr;
1629 int x;
1630
1631 nm = 4;
1632 n = 4;
1633 matz = 1;
1634
1635 for(x = 0; x < 16; x++) /* make a copy */
1636 at[x] = a[x];
1637
1638 pymol_rg_(&nm, &n, at, wr, wi, &matz, v, iv1, fv1, &ierr);
1639
1640 /* NOTE: the returned eigenvectors are stored one per row */
1641
1642 if(Feedback(G, FB_Matrix, FB_Blather)) {
1643 printf(" Eigensolve: eigenvectors %8.3f %8.3f %8.3f %8.3f\n", v[0], v[1], v[2], v[3]);
1644 printf(" Eigensolve: %8.3f %8.3f %8.3f %8.3f\n", v[4], v[5], v[6], v[7]);
1645 printf(" Eigensolve: %8.3f %8.3f %8.3f %8.3f\n", v[8], v[9], v[10],
1646 v[11]);
1647 printf(" Eigensolve: %8.3f %8.3f %8.3f %8.3f\n", v[12], v[13], v[14],
1648 v[15]);
1649
1650 printf(" Eigensolve: eigenvalues %8.3f %8.3f %8.3f %8.3f\n", wr[0], wr[1], wr[2],
1651 wr[3]);
1652 printf(" Eigensolve: %8.3f %8.3f %8.3f %8.3f\n", wi[0], wi[1], wi[2],
1653 wi[3]);
1654 }
1655
1656 return (ierr);
1657 }
1658
1659 static double d_sign(doublereal * a, doublereal * b);
1660
1661 static int balanc_(integer * nm, integer * n, doublereal * a, integer * low,
1662 integer * igh, doublereal * scale);
1663
1664 static int balbak_(integer * nm, integer * n, integer * low, integer * igh,
1665 doublereal * scale, integer * m, doublereal * z__);
1666
1667 static int cdiv_(doublereal * ar, doublereal * ai, doublereal * br, doublereal * bi,
1668 doublereal * cr, doublereal * ci);
1669
1670 static int elmhes_(integer * nm, integer * n, integer * low, integer * igh,
1671 doublereal * a, integer * int__);
1672
1673 static int eltran_(integer * nm, integer * n, integer * low, integer * igh,
1674 doublereal * a, integer * int__, doublereal * z__);
1675
1676 static int hqr2_(integer * nm, integer * n, integer * low, integer * igh,
1677 doublereal * h__, doublereal * wr, doublereal * wi, doublereal * z__,
1678 integer * ierr);
1679
1680 static int hqr_(integer * nm, integer * n, integer * low, integer * igh,
1681 doublereal * h__, doublereal * wr, doublereal * wi, integer * ierr);
1682
d_sign(doublereal * a,doublereal * b)1683 double d_sign(doublereal * a, doublereal * b)
1684 {
1685 double x;
1686 x = (*a >= 0 ? *a : -*a);
1687 return (*b >= 0 ? x : -x);
1688 }
1689
1690
1691 /* Table of constant values */
1692
1693 static doublereal c_b126 = 0.;
1694
balanc_(integer * nm,integer * n,doublereal * a,integer * low,integer * igh,doublereal * scale)1695 static int balanc_(integer * nm, integer * n, doublereal * a, integer * low,
1696 integer * igh, doublereal * scale)
1697 {
1698
1699
1700 /* System generated locals */
1701 integer a_dim1, a_offset, i__1, i__2;
1702 doublereal d__1;
1703
1704
1705 /* Local variables */
1706 integer iexc;
1707 doublereal c__, f, g;
1708 integer i__, j, k, l, m;
1709 doublereal r__, s, radix, b2;
1710 integer jj;
1711 logical noconv;
1712
1713
1714 /* this subroutine is a translation of the algol procedure balance, */
1715
1716
1717 /* num. math. 13, 293-304(1969) by parlett and reinsch. */
1718
1719
1720 /* handbook for auto. comp., vol.ii-linear algebra, 315-326(1971). */
1721
1722
1723 /* this subroutine balances a real matrix and isolates */
1724
1725
1726 /* eigenvalues whenever possible. */
1727
1728
1729 /* on input */
1730
1731
1732 /* nm must be set to the row dimension of two-dimensional */
1733
1734
1735 /* array parameters as declared in the calling program */
1736
1737
1738 /* dimension statement. */
1739
1740
1741 /* n is the order of the matrix. */
1742
1743
1744 /* a contains the input matrix to be balanced. */
1745
1746
1747 /* on output */
1748
1749
1750 /* a contains the balanced matrix. */
1751
1752
1753 /* low and igh are two integers such that a(i,j) */
1754
1755
1756 /* is equal to zero if */
1757
1758
1759 /* (1) i is greater than j and */
1760
1761
1762 /* (2) j=1,...,low-1 or i=igh+1,...,n. */
1763
1764
1765 /* scale contains information determining the */
1766
1767
1768 /* permutations and scaling factors used. */
1769
1770
1771 /* suppose that the principal submatrix in rows low through igh */
1772
1773
1774 /* has been balanced, that p(j) denotes the index interchanged */
1775
1776
1777 /* with j during the permutation step, and that the elements */
1778
1779
1780 /* of the diagonal matrix used are denoted by d(i,j). then */
1781
1782
1783 /* scale(j) = p(j), for j = 1,...,low-1 */
1784
1785
1786 /* = d(j,j), j = low,...,igh */
1787
1788
1789 /* = p(j) j = igh+1,...,n. */
1790
1791
1792 /* the order in which the interchanges are made is n to igh+1, */
1793
1794
1795 /* then 1 to low-1. */
1796
1797
1798 /* note that 1 is returned for igh if igh is zero formally. */
1799
1800
1801 /* the algol procedure exc contained in balance appears in */
1802
1803
1804 /* balanc in line. (note that the algol roles of identifiers */
1805
1806
1807 /* k,l have been reversed.) */
1808
1809
1810 /* questions and comments should be directed to burton s. garbow, */
1811
1812
1813 /* mathematics and computer science div, argonne national laboratory
1814 */
1815
1816
1817 /* this version dated august 1983. */
1818
1819
1820 /* ------------------------------------------------------------------
1821 */
1822
1823
1824 /* Parameter adjustments */
1825 --scale;
1826 a_dim1 = *nm;
1827 a_offset = a_dim1 + 1;
1828 a -= a_offset;
1829
1830
1831 /* Function Body */
1832 radix = 16.;
1833
1834 b2 = radix * radix;
1835 k = 1;
1836 l = *n;
1837 goto L100;
1838
1839
1840 /* .......... in-line procedure for row and */
1841
1842
1843 /* column exchange .......... */
1844 L20:
1845 scale[m] = (doublereal) j;
1846 if(j == m) {
1847 goto L50;
1848 }
1849
1850 i__1 = l;
1851 for(i__ = 1; i__ <= i__1; ++i__) {
1852 f = a[i__ + j * a_dim1];
1853 a[i__ + j * a_dim1] = a[i__ + m * a_dim1];
1854 a[i__ + m * a_dim1] = f;
1855
1856
1857 /* L30: */
1858 }
1859
1860 i__1 = *n;
1861 for(i__ = k; i__ <= i__1; ++i__) {
1862 f = a[j + i__ * a_dim1];
1863 a[j + i__ * a_dim1] = a[m + i__ * a_dim1];
1864 a[m + i__ * a_dim1] = f;
1865
1866
1867 /* L40: */
1868 }
1869
1870 L50:
1871 switch ((int) iexc) {
1872 case 1:
1873 goto L80;
1874 case 2:
1875 goto L130;
1876 }
1877
1878
1879 /* .......... search for rows isolating an eigenvalue */
1880
1881
1882 /* and push them down .......... */
1883 L80:
1884 if(l == 1) {
1885 goto L280;
1886 }
1887 --l;
1888
1889
1890 /* .......... for j=l step -1 until 1 do -- .......... */
1891 L100:
1892 i__1 = l;
1893 for(jj = 1; jj <= i__1; ++jj) {
1894 j = l + 1 - jj;
1895
1896 i__2 = l;
1897 for(i__ = 1; i__ <= i__2; ++i__) {
1898 if(i__ == j) {
1899 goto L110;
1900 }
1901 if(a[j + i__ * a_dim1] != 0.) {
1902 goto L120;
1903 }
1904 L110:
1905 ;
1906 }
1907
1908 m = l;
1909 iexc = 1;
1910 goto L20;
1911 L120:
1912 ;
1913 }
1914
1915 goto L140;
1916
1917
1918 /* .......... search for columns isolating an eigenvalue */
1919
1920
1921 /* and push them left .......... */
1922 L130:
1923 ++k;
1924
1925 L140:
1926 i__1 = l;
1927 for(j = k; j <= i__1; ++j) {
1928
1929 i__2 = l;
1930 for(i__ = k; i__ <= i__2; ++i__) {
1931 if(i__ == j) {
1932 goto L150;
1933 }
1934 if(a[i__ + j * a_dim1] != 0.) {
1935 goto L170;
1936 }
1937 L150:
1938 ;
1939 }
1940
1941 m = k;
1942 iexc = 2;
1943 goto L20;
1944 L170:
1945 ;
1946 }
1947
1948
1949 /* .......... now balance the submatrix in rows k to l .......... */
1950 i__1 = l;
1951 for(i__ = k; i__ <= i__1; ++i__) {
1952
1953
1954 /* L180: */
1955 scale[i__] = 1.;
1956 }
1957
1958
1959 /* .......... iterative loop for norm reduction .......... */
1960 L190:
1961 noconv = FALSE_;
1962
1963 i__1 = l;
1964 for(i__ = k; i__ <= i__1; ++i__) {
1965 c__ = 0.;
1966 r__ = 0.;
1967
1968 i__2 = l;
1969 for(j = k; j <= i__2; ++j) {
1970 if(j == i__) {
1971 goto L200;
1972 }
1973 c__ += (d__1 = a[j + i__ * a_dim1], abs(d__1));
1974 r__ += (d__1 = a[i__ + j * a_dim1], abs(d__1));
1975 L200:
1976 ;
1977 }
1978
1979
1980 /* .......... guard against zero c or r due to underflow .........
1981 . */
1982 if(c__ == 0. || r__ == 0.) {
1983 goto L270;
1984 }
1985 g = r__ / radix;
1986 f = 1.;
1987 s = c__ + r__;
1988 L210:
1989 if(c__ >= g) {
1990 goto L220;
1991 }
1992 f *= radix;
1993 c__ *= b2;
1994 goto L210;
1995 L220:
1996 g = r__ * radix;
1997 L230:
1998 if(c__ < g) {
1999 goto L240;
2000 }
2001 f /= radix;
2002 c__ /= b2;
2003 goto L230;
2004
2005
2006 /* .......... now balance .......... */
2007 L240:
2008 if((c__ + r__) / f >= s * .95) {
2009 goto L270;
2010 }
2011 g = 1. / f;
2012 scale[i__] *= f;
2013 noconv = TRUE_;
2014
2015 i__2 = *n;
2016 for(j = k; j <= i__2; ++j) {
2017
2018
2019 /* L250: */
2020 a[i__ + j * a_dim1] *= g;
2021 }
2022
2023 i__2 = l;
2024 for(j = 1; j <= i__2; ++j) {
2025
2026
2027 /* L260: */
2028 a[j + i__ * a_dim1] *= f;
2029 }
2030
2031 L270:
2032 ;
2033 }
2034
2035 if(noconv) {
2036 goto L190;
2037 }
2038
2039 L280:
2040 *low = k;
2041 *igh = l;
2042 return 0;
2043 } /* balanc_ */
2044
balbak_(integer * nm,integer * n,integer * low,integer * igh,doublereal * scale,integer * m,doublereal * z__)2045 static int balbak_(integer * nm, integer * n, integer * low, integer * igh,
2046 doublereal * scale, integer * m, doublereal * z__)
2047 {
2048
2049
2050 /* System generated locals */
2051 integer z_dim1, z_offset, i__1, i__2;
2052
2053
2054 /* Local variables */
2055 integer i__, j, k;
2056 doublereal s;
2057 integer ii;
2058
2059
2060 /* this subroutine is a translation of the algol procedure balbak, */
2061
2062
2063 /* num. math. 13, 293-304(1969) by parlett and reinsch. */
2064
2065
2066 /* handbook for auto. comp., vol.ii-linear algebra, 315-326(1971). */
2067
2068
2069 /* this subroutine forms the eigenvectors of a real general */
2070
2071
2072 /* matrix by back transforming those of the corresponding */
2073
2074
2075 /* balanced matrix determined by balanc. */
2076
2077
2078 /* on input */
2079
2080
2081 /* nm must be set to the row dimension of two-dimensional */
2082
2083
2084 /* array parameters as declared in the calling program */
2085
2086
2087 /* dimension statement. */
2088
2089
2090 /* n is the order of the matrix. */
2091
2092
2093 /* low and igh are integers determined by balanc. */
2094
2095
2096 /* scale contains information determining the permutations */
2097
2098
2099 /* and scaling factors used by balanc. */
2100
2101
2102 /* m is the number of columns of z to be back transformed. */
2103
2104
2105 /* z contains the real and imaginary parts of the eigen- */
2106
2107
2108 /* vectors to be back transformed in its first m columns. */
2109
2110
2111 /* on output */
2112
2113
2114 /* z contains the real and imaginary parts of the */
2115
2116
2117 /* transformed eigenvectors in its first m columns. */
2118
2119
2120 /* questions and comments should be directed to burton s. garbow, */
2121
2122
2123 /* mathematics and computer science div, argonne national laboratory
2124 */
2125
2126
2127 /* this version dated august 1983. */
2128
2129
2130 /* ------------------------------------------------------------------
2131 */
2132
2133
2134 /* Parameter adjustments */
2135 --scale;
2136 z_dim1 = *nm;
2137 z_offset = z_dim1 + 1;
2138 z__ -= z_offset;
2139
2140
2141 /* Function Body */
2142 if(*m == 0) {
2143 goto L200;
2144 }
2145 if(*igh == *low) {
2146 goto L120;
2147 }
2148
2149 i__1 = *igh;
2150 for(i__ = *low; i__ <= i__1; ++i__) {
2151 s = scale[i__];
2152
2153
2154 /* .......... left hand eigenvectors are back transformed */
2155
2156
2157 /* if the foregoing statement is replaced by */
2158
2159
2160 /* s=1.0d0/scale(i). .......... */
2161 i__2 = *m;
2162 for(j = 1; j <= i__2; ++j) {
2163
2164
2165 /* L100: */
2166 z__[i__ + j * z_dim1] *= s;
2167 }
2168
2169
2170 /* L110: */
2171 }
2172
2173
2174 /* ......... for i=low-1 step -1 until 1, */
2175
2176
2177 /* igh+1 step 1 until n do -- .......... */
2178 L120:
2179 i__1 = *n;
2180 for(ii = 1; ii <= i__1; ++ii) {
2181 i__ = ii;
2182 if(i__ >= *low && i__ <= *igh) {
2183 goto L140;
2184 }
2185 if(i__ < *low) {
2186 i__ = *low - ii;
2187 }
2188 k = (integer) scale[i__];
2189 if(k == i__) {
2190 goto L140;
2191 }
2192
2193 i__2 = *m;
2194 for(j = 1; j <= i__2; ++j) {
2195 s = z__[i__ + j * z_dim1];
2196 z__[i__ + j * z_dim1] = z__[k + j * z_dim1];
2197 z__[k + j * z_dim1] = s;
2198
2199
2200 /* L130: */
2201 }
2202
2203 L140:
2204 ;
2205 }
2206
2207 L200:
2208 return 0;
2209 } /* balbak_ */
2210
cdiv_(doublereal * ar,doublereal * ai,doublereal * br,doublereal * bi,doublereal * cr,doublereal * ci)2211 static int cdiv_(doublereal * ar, doublereal * ai, doublereal * br, doublereal * bi,
2212 doublereal * cr, doublereal * ci)
2213 {
2214
2215
2216 /* System generated locals */
2217 doublereal d__1, d__2;
2218
2219
2220 /* Local variables */
2221 doublereal s, ais, bis, ars, brs;
2222
2223
2224 /* complex division, (cr,ci) = (ar,ai)/(br,bi) */
2225
2226 s = abs(*br) + abs(*bi);
2227 ars = *ar / s;
2228 ais = *ai / s;
2229 brs = *br / s;
2230 bis = *bi / s;
2231
2232
2233 /* Computing 2nd power */
2234 d__1 = brs;
2235
2236
2237 /* Computing 2nd power */
2238 d__2 = bis;
2239 s = d__1 * d__1 + d__2 * d__2;
2240 *cr = (ars * brs + ais * bis) / s;
2241 *ci = (ais * brs - ars * bis) / s;
2242 return 0;
2243 } /* cdiv_ */
2244
elmhes_(integer * nm,integer * n,integer * low,integer * igh,doublereal * a,integer * int__)2245 static int elmhes_(integer * nm, integer * n, integer * low, integer * igh,
2246 doublereal * a, integer * int__)
2247 {
2248
2249
2250 /* System generated locals */
2251 integer a_dim1, a_offset, i__1, i__2, i__3;
2252 doublereal d__1;
2253
2254
2255 /* Local variables */
2256 integer i__, j, m;
2257 doublereal x, y;
2258 integer la, mm1, kp1, mp1;
2259
2260
2261 /* this subroutine is a translation of the algol procedure elmhes, */
2262
2263
2264 /* num. math. 12, 349-368(1968) by martin and wilkinson. */
2265
2266
2267 /* handbook for auto. comp., vol.ii-linear algebra, 339-358(1971). */
2268
2269
2270 /* given a real general matrix, this subroutine */
2271
2272
2273 /* reduces a submatrix situated in rows and columns */
2274
2275
2276 /* low through igh to upper hessenberg form by */
2277
2278
2279 /* stabilized elementary similarity transformations. */
2280
2281
2282 /* on input */
2283
2284
2285 /* nm must be set to the row dimension of two-dimensional */
2286
2287
2288 /* array parameters as declared in the calling program */
2289
2290
2291 /* dimension statement. */
2292
2293
2294 /* n is the order of the matrix. */
2295
2296
2297 /* low and igh are integers determined by the balancing */
2298
2299
2300 /* subroutine balanc. if balanc has not been used, */
2301
2302
2303 /* set low=1, igh=n. */
2304
2305
2306 /* a contains the input matrix. */
2307
2308
2309 /* on output */
2310
2311
2312 /* a contains the hessenberg matrix. the multipliers */
2313
2314
2315 /* which were used in the reduction are stored in the */
2316
2317
2318 /* remaining triangle under the hessenberg matrix. */
2319
2320
2321 /* int contains information on the rows and columns */
2322
2323
2324 /* interchanged in the reduction. */
2325
2326
2327 /* only elements low through igh are used. */
2328
2329
2330 /* questions and comments should be directed to burton s. garbow, */
2331
2332
2333 /* mathematics and computer science div, argonne national laboratory
2334 */
2335
2336
2337 /* this version dated august 1983. */
2338
2339
2340 /* ------------------------------------------------------------------
2341 */
2342
2343
2344 /* Parameter adjustments */
2345 a_dim1 = *nm;
2346 a_offset = a_dim1 + 1;
2347 a -= a_offset;
2348 --int__;
2349
2350
2351 /* Function Body */
2352 la = *igh - 1;
2353 kp1 = *low + 1;
2354 if(la < kp1) {
2355 goto L200;
2356 }
2357
2358 i__1 = la;
2359 for(m = kp1; m <= i__1; ++m) {
2360 mm1 = m - 1;
2361 x = 0.;
2362 i__ = m;
2363
2364 i__2 = *igh;
2365 for(j = m; j <= i__2; ++j) {
2366 if((d__1 = a[j + mm1 * a_dim1], abs(d__1)) <= abs(x)) {
2367 goto L100;
2368 }
2369 x = a[j + mm1 * a_dim1];
2370 i__ = j;
2371 L100:
2372 ;
2373 }
2374
2375 int__[m] = i__;
2376 if(i__ == m) {
2377 goto L130;
2378 }
2379
2380
2381 /* .......... interchange rows and columns of a .......... */
2382 i__2 = *n;
2383 for(j = mm1; j <= i__2; ++j) {
2384 y = a[i__ + j * a_dim1];
2385 a[i__ + j * a_dim1] = a[m + j * a_dim1];
2386 a[m + j * a_dim1] = y;
2387
2388
2389 /* L110: */
2390 }
2391
2392 i__2 = *igh;
2393 for(j = 1; j <= i__2; ++j) {
2394 y = a[j + i__ * a_dim1];
2395 a[j + i__ * a_dim1] = a[j + m * a_dim1];
2396 a[j + m * a_dim1] = y;
2397
2398
2399 /* L120: */
2400 }
2401
2402
2403 /* .......... end interchange .......... */
2404 L130:
2405 if(x == 0.) {
2406 goto L180;
2407 }
2408 mp1 = m + 1;
2409
2410 i__2 = *igh;
2411 for(i__ = mp1; i__ <= i__2; ++i__) {
2412 y = a[i__ + mm1 * a_dim1];
2413 if(y == 0.) {
2414 goto L160;
2415 }
2416 y /= x;
2417 a[i__ + mm1 * a_dim1] = y;
2418
2419 i__3 = *n;
2420 for(j = m; j <= i__3; ++j) {
2421
2422
2423 /* L140: */
2424 a[i__ + j * a_dim1] -= y * a[m + j * a_dim1];
2425 }
2426
2427 i__3 = *igh;
2428 for(j = 1; j <= i__3; ++j) {
2429
2430
2431 /* L150: */
2432 a[j + m * a_dim1] += y * a[j + i__ * a_dim1];
2433 }
2434
2435 L160:
2436 ;
2437 }
2438
2439 L180:
2440 ;
2441 }
2442
2443 L200:
2444 return 0;
2445 } /* elmhes_ */
2446
eltran_(integer * nm,integer * n,integer * low,integer * igh,doublereal * a,integer * int__,doublereal * z__)2447 static int eltran_(integer * nm, integer * n, integer * low, integer * igh,
2448 doublereal * a, integer * int__, doublereal * z__)
2449 {
2450 /* System generated locals */
2451 integer a_dim1, a_offset, z_dim1, z_offset, i__1, i__2;
2452
2453 /* Local variables */
2454 integer i__, j, kl, mm, mp, mp1;
2455
2456
2457 /* this subroutine is a translation of the algol procedure elmtrans,
2458 */
2459
2460
2461 /* num. math. 16, 181-204(1970) by peters and wilkinson. */
2462
2463
2464 /* handbook for auto. comp., vol.ii-linear algebra, 372-395(1971). */
2465
2466
2467 /* this subroutine accumulates the stabilized elementary */
2468
2469
2470 /* similarity transformations used in the reduction of a */
2471
2472
2473 /* real general matrix to upper hessenberg form by elmhes. */
2474
2475
2476 /* on input */
2477
2478
2479 /* nm must be set to the row dimension of two-dimensional */
2480
2481
2482 /* array parameters as declared in the calling program */
2483
2484
2485 /* dimension statement. */
2486
2487
2488 /* n is the order of the matrix. */
2489
2490
2491 /* low and igh are integers determined by the balancing */
2492
2493
2494 /* subroutine balanc. if balanc has not been used, */
2495
2496
2497 /* set low=1, igh=n. */
2498
2499
2500 /* a contains the multipliers which were used in the */
2501
2502
2503 /* reduction by elmhes in its lower triangle */
2504
2505
2506 /* below the subdiagonal. */
2507
2508
2509 /* int contains information on the rows and columns */
2510
2511
2512 /* interchanged in the reduction by elmhes. */
2513
2514
2515 /* only elements low through igh are used. */
2516
2517
2518 /* on output */
2519
2520
2521 /* z contains the transformation matrix produced in the */
2522
2523
2524 /* reduction by elmhes. */
2525
2526
2527 /* questions and comments should be directed to burton s. garbow, */
2528
2529
2530 /* mathematics and computer science div, argonne national laboratory
2531 */
2532
2533
2534 /* this version dated august 1983. */
2535
2536
2537 /* ------------------------------------------------------------------
2538 */
2539
2540
2541 /* .......... initialize z to identity matrix .......... */
2542 /* Parameter adjustments */
2543 z_dim1 = *nm;
2544 z_offset = z_dim1 + 1;
2545 z__ -= z_offset;
2546 --int__;
2547 a_dim1 = *nm;
2548 a_offset = a_dim1 + 1;
2549 a -= a_offset;
2550
2551 /* Function Body */
2552 i__1 = *n;
2553 for(j = 1; j <= i__1; ++j) {
2554
2555 i__2 = *n;
2556 for(i__ = 1; i__ <= i__2; ++i__) {
2557
2558
2559 /* L60: */
2560 z__[i__ + j * z_dim1] = 0.;
2561 }
2562
2563 z__[j + j * z_dim1] = 1.;
2564
2565
2566 /* L80: */
2567 }
2568
2569 kl = *igh - *low - 1;
2570 if(kl < 1) {
2571 goto L200;
2572 }
2573
2574
2575 /* .......... for mp=igh-1 step -1 until low+1 do -- .......... */
2576 i__1 = kl;
2577 for(mm = 1; mm <= i__1; ++mm) {
2578 mp = *igh - mm;
2579 mp1 = mp + 1;
2580
2581 i__2 = *igh;
2582 for(i__ = mp1; i__ <= i__2; ++i__) {
2583
2584
2585 /* L100: */
2586 z__[i__ + mp * z_dim1] = a[i__ + (mp - 1) * a_dim1];
2587 }
2588
2589 i__ = int__[mp];
2590 if(i__ == mp) {
2591 goto L140;
2592 }
2593
2594 i__2 = *igh;
2595 for(j = mp; j <= i__2; ++j) {
2596 z__[mp + j * z_dim1] = z__[i__ + j * z_dim1];
2597 z__[i__ + j * z_dim1] = 0.;
2598
2599
2600 /* L130: */
2601 }
2602
2603 z__[i__ + mp * z_dim1] = 1.;
2604 L140:
2605 ;
2606 }
2607
2608 L200:
2609 return 0;
2610 } /* eltran_ */
2611
hqr_(integer * nm,integer * n,integer * low,integer * igh,doublereal * h__,doublereal * wr,doublereal * wi,integer * ierr)2612 static int hqr_(integer * nm, integer * n, integer * low, integer * igh,
2613 doublereal * h__, doublereal * wr, doublereal * wi, integer * ierr)
2614 {
2615 /* System generated locals */
2616 integer h_dim1, h_offset, i__1, i__2, i__3;
2617 doublereal d__1, d__2;
2618
2619 /* Local variables */
2620 doublereal norm;
2621 integer i__, j, k, l = 0, m = 0;
2622 doublereal p = 0.0, q = 0.0, r__ = 0.0, s, t, w, x, y;
2623 integer na, en, ll, mm;
2624 doublereal zz;
2625 logical notlas;
2626 integer mp2, itn, its, enm2;
2627 doublereal tst1, tst2;
2628
2629
2630 /* RESTORED CORRECT INDICES OF LOOPS (200,210,230,240). (9/29/89 BSG) */
2631
2632
2633 /* this subroutine is a translation of the algol procedure hqr, */
2634
2635
2636 /* num. math. 14, 219-231(1970) by martin, peters, and wilkinson. */
2637
2638
2639 /* handbook for auto. comp., vol.ii-linear algebra, 359-371(1971). */
2640
2641
2642 /* this subroutine finds the eigenvalues of a real */
2643
2644
2645 /* upper hessenberg matrix by the qr method. */
2646
2647
2648 /* on input */
2649
2650
2651 /* nm must be set to the row dimension of two-dimensional */
2652
2653
2654 /* array parameters as declared in the calling program */
2655
2656
2657 /* dimension statement. */
2658
2659
2660 /* n is the order of the matrix. */
2661
2662
2663 /* low and igh are integers determined by the balancing */
2664
2665
2666 /* subroutine balanc. if balanc has not been used, */
2667
2668
2669 /* set low=1, igh=n. */
2670
2671
2672 /* h contains the upper hessenberg matrix. information about */
2673
2674
2675 /* the transformations used in the reduction to hessenberg */
2676
2677
2678 /* form by elmhes or orthes, if performed, is stored */
2679
2680
2681 /* in the remaining triangle under the hessenberg matrix. */
2682
2683
2684 /* on output */
2685
2686
2687 /* h has been destroyed. therefore, it must be saved */
2688
2689
2690 /* before calling hqr if subsequent calculation and */
2691
2692
2693 /* back transformation of eigenvectors is to be performed. */
2694
2695
2696 /* wr and wi contain the real and imaginary parts, */
2697
2698
2699 /* respectively, of the eigenvalues. the eigenvalues */
2700
2701
2702 /* are unordered except that complex conjugate pairs */
2703
2704
2705 /* of values appear consecutively with the eigenvalue */
2706
2707
2708 /* having the positive imaginary part first. if an */
2709
2710
2711 /* error exit is made, the eigenvalues should be correct */
2712
2713
2714 /* for indices ierr+1,...,n. */
2715
2716
2717 /* ierr is set to */
2718
2719
2720 /* zero for normal return, */
2721
2722
2723 /* j if the limit of 30*n iterations is exhausted */
2724
2725
2726 /* while the j-th eigenvalue is being sought. */
2727
2728
2729 /* questions and comments should be directed to burton s. garbow, */
2730
2731
2732 /* mathematics and computer science div, argonne national laboratory
2733 */
2734
2735
2736 /* this version dated september 1989. */
2737
2738
2739 /* ------------------------------------------------------------------
2740 */
2741
2742 /* Parameter adjustments */
2743 --wi;
2744 --wr;
2745 h_dim1 = *nm;
2746 h_offset = h_dim1 + 1;
2747 h__ -= h_offset;
2748
2749 /* Function Body */
2750 *ierr = 0;
2751 norm = 0.;
2752 k = 1;
2753
2754
2755 /* .......... store roots isolated by balanc */
2756
2757
2758 /* and compute matrix norm .......... */
2759 i__1 = *n;
2760 for(i__ = 1; i__ <= i__1; ++i__) {
2761
2762 i__2 = *n;
2763 for(j = k; j <= i__2; ++j) {
2764
2765
2766 /* L40: */
2767 norm += (d__1 = h__[i__ + j * h_dim1], abs(d__1));
2768 }
2769
2770 k = i__;
2771 if(i__ >= *low && i__ <= *igh) {
2772 goto L50;
2773 }
2774 wr[i__] = h__[i__ + i__ * h_dim1];
2775 wi[i__] = 0.;
2776 L50:
2777 ;
2778 }
2779
2780 en = *igh;
2781 t = 0.;
2782 itn = *n * 30;
2783
2784
2785 /* .......... search for next eigenvalues .......... */
2786 L60:
2787 if(en < *low) {
2788 goto L1001;
2789 }
2790 its = 0;
2791 na = en - 1;
2792 enm2 = na - 1;
2793
2794
2795 /* .......... look for single small sub-diagonal element */
2796
2797
2798 /* for l=en step -1 until low do -- .......... */
2799 L70:
2800 i__1 = en;
2801 for(ll = *low; ll <= i__1; ++ll) {
2802 l = en + *low - ll;
2803 if(l == *low) {
2804 goto L100;
2805 }
2806 s = (d__1 = h__[l - 1 + (l - 1) * h_dim1], abs(d__1)) + (d__2 = h__[l
2807 + l * h_dim1],
2808 abs(d__2));
2809 if(s == 0.) {
2810 s = norm;
2811 }
2812 tst1 = s;
2813 tst2 = tst1 + (d__1 = h__[l + (l - 1) * h_dim1], abs(d__1));
2814 if(tst2 == tst1) {
2815 goto L100;
2816 }
2817
2818
2819 /* L80: */
2820 }
2821
2822
2823 /* .......... form shift .......... */
2824 L100:
2825 x = h__[en + en * h_dim1];
2826 if(l == en) {
2827 goto L270;
2828 }
2829 y = h__[na + na * h_dim1];
2830 w = h__[en + na * h_dim1] * h__[na + en * h_dim1];
2831 if(l == na) {
2832 goto L280;
2833 }
2834 if(itn == 0) {
2835 goto L1000;
2836 }
2837 if(its != 10 && its != 20) {
2838 goto L130;
2839 }
2840
2841
2842 /* .......... form exceptional shift .......... */
2843 t += x;
2844
2845 i__1 = en;
2846 for(i__ = *low; i__ <= i__1; ++i__) {
2847
2848
2849 /* L120: */
2850 h__[i__ + i__ * h_dim1] -= x;
2851 }
2852
2853 s = (d__1 = h__[en + na * h_dim1], abs(d__1)) + (d__2 = h__[na + enm2 *
2854 h_dim1], abs(d__2));
2855 x = s * .75;
2856 y = x;
2857 w = s * -.4375 * s;
2858 L130:
2859 ++its;
2860 --itn;
2861
2862
2863 /* .......... look for two consecutive small */
2864
2865
2866 /* sub-diagonal elements. */
2867
2868
2869 /* for m=en-2 step -1 until l do -- .......... */
2870 i__1 = enm2;
2871 for(mm = l; mm <= i__1; ++mm) {
2872 m = enm2 + l - mm;
2873 zz = h__[m + m * h_dim1];
2874 r__ = x - zz;
2875 s = y - zz;
2876 p = (r__ * s - w) / h__[m + 1 + m * h_dim1] + h__[m + (m + 1) * h_dim1];
2877 q = h__[m + 1 + (m + 1) * h_dim1] - zz - r__ - s;
2878 r__ = h__[m + 2 + (m + 1) * h_dim1];
2879 s = abs(p) + abs(q) + abs(r__);
2880 p /= s;
2881 q /= s;
2882 r__ /= s;
2883 if(m == l) {
2884 goto L150;
2885 }
2886 tst1 = abs(p) * ((d__1 = h__[m - 1 + (m - 1) * h_dim1], abs(d__1)) +
2887 abs(zz) + (d__2 = h__[m + 1 + (m + 1) * h_dim1], abs(d__2)));
2888 tst2 = tst1 + (d__1 = h__[m + (m - 1) * h_dim1], abs(d__1)) * (abs(q)
2889 + abs(r__));
2890 if(tst2 == tst1) {
2891 goto L150;
2892 }
2893
2894
2895 /* L140: */
2896 }
2897
2898 L150:
2899 mp2 = m + 2;
2900
2901 i__1 = en;
2902 for(i__ = mp2; i__ <= i__1; ++i__) {
2903 h__[i__ + (i__ - 2) * h_dim1] = 0.;
2904 if(i__ == mp2) {
2905 goto L160;
2906 }
2907 h__[i__ + (i__ - 3) * h_dim1] = 0.;
2908 L160:
2909 ;
2910 }
2911
2912
2913 /* .......... double qr step involving rows l to en and */
2914
2915
2916 /* columns m to en .......... */
2917 i__1 = na;
2918 for(k = m; k <= i__1; ++k) {
2919 notlas = k != na;
2920 if(k == m) {
2921 goto L170;
2922 }
2923 p = h__[k + (k - 1) * h_dim1];
2924 q = h__[k + 1 + (k - 1) * h_dim1];
2925 r__ = 0.;
2926 if(notlas) {
2927 r__ = h__[k + 2 + (k - 1) * h_dim1];
2928 }
2929 x = abs(p) + abs(q) + abs(r__);
2930 if(x == 0.) {
2931 goto L260;
2932 }
2933 p /= x;
2934 q /= x;
2935 r__ /= x;
2936 L170:
2937 d__1 = sqrt1d(p * p + q * q + r__ * r__);
2938 s = d_sign(&d__1, &p);
2939 if(k == m) {
2940 goto L180;
2941 }
2942 h__[k + (k - 1) * h_dim1] = -s * x;
2943 goto L190;
2944 L180:
2945 if(l != m) {
2946 h__[k + (k - 1) * h_dim1] = -h__[k + (k - 1) * h_dim1];
2947 }
2948 L190:
2949 p += s;
2950 x = p / s;
2951 y = q / s;
2952 zz = r__ / s;
2953 q /= p;
2954 r__ /= p;
2955 if(notlas) {
2956 goto L225;
2957 }
2958
2959
2960 /* .......... row modification .......... */
2961 i__2 = en;
2962 for(j = k; j <= i__2; ++j) {
2963 p = h__[k + j * h_dim1] + q * h__[k + 1 + j * h_dim1];
2964 h__[k + j * h_dim1] -= p * x;
2965 h__[k + 1 + j * h_dim1] -= p * y;
2966
2967
2968 /* L200: */
2969 }
2970
2971
2972 /* Computing MIN */
2973 i__2 = en, i__3 = k + 3;
2974 j = mymin(i__2, i__3);
2975
2976
2977 /* .......... column modification .......... */
2978 i__2 = j;
2979 for(i__ = l; i__ <= i__2; ++i__) {
2980 p = x * h__[i__ + k * h_dim1] + y * h__[i__ + (k + 1) * h_dim1];
2981 h__[i__ + k * h_dim1] -= p;
2982 h__[i__ + (k + 1) * h_dim1] -= p * q;
2983
2984
2985 /* L210: */
2986 }
2987 goto L255;
2988 L225:
2989
2990
2991 /* .......... row modification .......... */
2992 i__2 = en;
2993 for(j = k; j <= i__2; ++j) {
2994 p =
2995 h__[k + j * h_dim1] + q * h__[k + 1 + j * h_dim1] + r__ * h__[k + 2 + j * h_dim1];
2996 h__[k + j * h_dim1] -= p * x;
2997 h__[k + 1 + j * h_dim1] -= p * y;
2998 h__[k + 2 + j * h_dim1] -= p * zz;
2999
3000
3001 /* L230: */
3002 }
3003
3004
3005 /* Computing MIN */
3006 i__2 = en, i__3 = k + 3;
3007 j = mymin(i__2, i__3);
3008
3009
3010 /* .......... column modification .......... */
3011 i__2 = j;
3012 for(i__ = l; i__ <= i__2; ++i__) {
3013 p = x * h__[i__ + k * h_dim1] + y * h__[i__ + (k + 1) * h_dim1] +
3014 zz * h__[i__ + (k + 2) * h_dim1];
3015 h__[i__ + k * h_dim1] -= p;
3016 h__[i__ + (k + 1) * h_dim1] -= p * q;
3017 h__[i__ + (k + 2) * h_dim1] -= p * r__;
3018
3019
3020 /* L240: */
3021 }
3022 L255:
3023
3024 L260:
3025 ;
3026 }
3027
3028 goto L70;
3029
3030
3031 /* .......... one root found .......... */
3032 L270:
3033 wr[en] = x + t;
3034 wi[en] = 0.;
3035 en = na;
3036 goto L60;
3037
3038
3039 /* .......... two roots found .......... */
3040 L280:
3041 p = (y - x) / 2.;
3042 q = p * p + w;
3043 zz = sqrt1d((abs(q)));
3044 x += t;
3045 if(q < 0.) {
3046 goto L320;
3047 }
3048
3049
3050 /* .......... real pair .......... */
3051 zz = p + d_sign(&zz, &p);
3052 wr[na] = x + zz;
3053 wr[en] = wr[na];
3054 if(zz != 0.) {
3055 wr[en] = x - w / zz;
3056 }
3057 wi[na] = 0.;
3058 wi[en] = 0.;
3059 goto L330;
3060
3061
3062 /* .......... complex pair .......... */
3063 L320:
3064 wr[na] = x + p;
3065 wr[en] = x + p;
3066 wi[na] = zz;
3067 wi[en] = -zz;
3068 L330:
3069 en = enm2;
3070 goto L60;
3071
3072
3073 /* .......... set error -- all eigenvalues have not */
3074
3075
3076 /* converged after 30*n iterations .......... */
3077 L1000:
3078 *ierr = en;
3079 L1001:
3080 return 0;
3081 } /* hqr_ */
3082
hqr2_(integer * nm,integer * n,integer * low,integer * igh,doublereal * h__,doublereal * wr,doublereal * wi,doublereal * z__,integer * ierr)3083 static int hqr2_(integer * nm, integer * n, integer * low, integer * igh,
3084 doublereal * h__, doublereal * wr, doublereal * wi, doublereal * z__,
3085 integer * ierr)
3086 {
3087 /* System generated locals */
3088 integer h_dim1, h_offset, z_dim1, z_offset, i__1, i__2, i__3;
3089 doublereal d__1, d__2, d__3, d__4;
3090
3091 /* Local variables */
3092 doublereal norm;
3093 integer i__, j, k, l = 0, m = 0;
3094 doublereal p = 0.0, q = 0.0, r__ = 0.0, s = 0.0, t, w, x, y;
3095 integer na, ii, en, jj;
3096 doublereal ra, sa;
3097 integer ll, mm, nn;
3098 doublereal vi, vr, zz = 0.0;
3099 logical notlas;
3100 integer mp2, itn, its, enm2;
3101 doublereal tst1, tst2;
3102
3103
3104 /* this subroutine is a translation of the algol procedure hqr2, */
3105
3106
3107 /* num. math. 16, 181-204(1970) by peters and wilkinson. */
3108
3109
3110 /* handbook for auto. comp., vol.ii-linear algebra, 372-395(1971). */
3111
3112
3113 /* this subroutine finds the eigenvalues and eigenvectors */
3114
3115
3116 /* of a real upper hessenberg matrix by the qr method. the */
3117
3118
3119 /* eigenvectors of a real general matrix can also be found */
3120
3121
3122 /* if elmhes and eltran or orthes and ortran have */
3123
3124
3125 /* been used to reduce this general matrix to hessenberg form */
3126
3127
3128 /* and to accumulate the similarity transformations. */
3129
3130
3131 /* on input */
3132
3133
3134 /* nm must be set to the row dimension of two-dimensional */
3135
3136
3137 /* array parameters as declared in the calling program */
3138
3139
3140 /* dimension statement. */
3141
3142
3143 /* n is the order of the matrix. */
3144
3145
3146 /* low and igh are integers determined by the balancing */
3147
3148
3149 /* subroutine balanc. if balanc has not been used, */
3150
3151
3152 /* set low=1, igh=n. */
3153
3154
3155 /* h contains the upper hessenberg matrix. */
3156
3157
3158 /* z contains the transformation matrix produced by eltran */
3159
3160
3161 /* after the reduction by elmhes, or by ortran after the */
3162
3163
3164 /* reduction by orthes, if performed. if the eigenvectors */
3165
3166
3167 /* of the hessenberg matrix are desired, z must contain the */
3168
3169
3170 /* identity matrix. */
3171
3172
3173 /* on output */
3174
3175
3176 /* h has been destroyed. */
3177
3178
3179 /* wr and wi contain the real and imaginary parts, */
3180
3181
3182 /* respectively, of the eigenvalues. the eigenvalues */
3183
3184
3185 /* are unordered except that complex conjugate pairs */
3186
3187
3188 /* of values appear consecutively with the eigenvalue */
3189
3190
3191 /* having the positive imaginary part first. if an */
3192
3193
3194 /* error exit is made, the eigenvalues should be correct */
3195
3196
3197 /* for indices ierr+1,...,n. */
3198
3199
3200 /* z contains the real and imaginary parts of the eigenvectors. */
3201
3202
3203 /* if the i-th eigenvalue is real, the i-th column of z */
3204
3205
3206 /* contains its eigenvector. if the i-th eigenvalue is complex
3207 */
3208
3209
3210 /* with positive imaginary part, the i-th and (i+1)-th */
3211
3212
3213 /* columns of z contain the real and imaginary parts of its */
3214
3215
3216 /* eigenvector. the eigenvectors are unnormalized. if an */
3217
3218
3219 /* error exit is made, none of the eigenvectors has been found.
3220 */
3221
3222
3223 /* ierr is set to */
3224
3225
3226 /* zero for normal return, */
3227
3228
3229 /* j if the limit of 30*n iterations is exhausted */
3230
3231
3232 /* while the j-th eigenvalue is being sought. */
3233
3234
3235 /* calls cdiv for complex division. */
3236
3237
3238 /* questions and comments should be directed to burton s. garbow, */
3239
3240
3241 /* mathematics and computer science div, argonne national laboratory
3242 */
3243
3244
3245 /* this version dated august 1983. */
3246
3247
3248 /* ------------------------------------------------------------------
3249 */
3250
3251 /* Parameter adjustments */
3252 z_dim1 = *nm;
3253 z_offset = z_dim1 + 1;
3254 z__ -= z_offset;
3255 --wi;
3256 --wr;
3257 h_dim1 = *nm;
3258 h_offset = h_dim1 + 1;
3259 h__ -= h_offset;
3260
3261 /* Function Body */
3262 *ierr = 0;
3263 norm = 0.;
3264 k = 1;
3265
3266
3267 /* .......... store roots isolated by balanc */
3268
3269
3270 /* and compute matrix norm .......... */
3271 i__1 = *n;
3272 for(i__ = 1; i__ <= i__1; ++i__) {
3273
3274 i__2 = *n;
3275 for(j = k; j <= i__2; ++j) {
3276
3277
3278 /* L40: */
3279 norm += (d__1 = h__[i__ + j * h_dim1], abs(d__1));
3280 }
3281
3282 k = i__;
3283 if(i__ >= *low && i__ <= *igh) {
3284 goto L50;
3285 }
3286 wr[i__] = h__[i__ + i__ * h_dim1];
3287 wi[i__] = 0.;
3288 L50:
3289 ;
3290 }
3291
3292 en = *igh;
3293 t = 0.;
3294 itn = *n * 30;
3295
3296
3297 /* .......... search for next eigenvalues .......... */
3298 L60:
3299 if(en < *low) {
3300 goto L340;
3301 }
3302 its = 0;
3303 na = en - 1;
3304 enm2 = na - 1;
3305
3306
3307 /* .......... look for single small sub-diagonal element */
3308
3309
3310 /* for l=en step -1 until low do -- .......... */
3311 L70:
3312 i__1 = en;
3313 for(ll = *low; ll <= i__1; ++ll) {
3314 l = en + *low - ll;
3315 if(l == *low) {
3316 goto L100;
3317 }
3318 s = (d__1 = h__[l - 1 + (l - 1) * h_dim1], abs(d__1)) + (d__2 = h__[l
3319 + l * h_dim1],
3320 abs(d__2));
3321 if(s == 0.) {
3322 s = norm;
3323 }
3324 tst1 = s;
3325 tst2 = tst1 + (d__1 = h__[l + (l - 1) * h_dim1], abs(d__1));
3326 if(tst2 == tst1) {
3327 goto L100;
3328 }
3329
3330
3331 /* L80: */
3332 }
3333
3334
3335 /* .......... form shift .......... */
3336 L100:
3337 x = h__[en + en * h_dim1];
3338 if(l == en) {
3339 goto L270;
3340 }
3341 y = h__[na + na * h_dim1];
3342 w = h__[en + na * h_dim1] * h__[na + en * h_dim1];
3343 if(l == na) {
3344 goto L280;
3345 }
3346 if(itn == 0) {
3347 goto L1000;
3348 }
3349 if(its != 10 && its != 20) {
3350 goto L130;
3351 }
3352
3353
3354 /* .......... form exceptional shift .......... */
3355 t += x;
3356
3357 i__1 = en;
3358 for(i__ = *low; i__ <= i__1; ++i__) {
3359
3360
3361 /* L120: */
3362 h__[i__ + i__ * h_dim1] -= x;
3363 }
3364
3365 s = (d__1 = h__[en + na * h_dim1], abs(d__1)) + (d__2 = h__[na + enm2 *
3366 h_dim1], abs(d__2));
3367 x = s * .75;
3368 y = x;
3369 w = s * -.4375 * s;
3370 L130:
3371 ++its;
3372 --itn;
3373
3374
3375 /* .......... look for two consecutive small */
3376
3377
3378 /* sub-diagonal elements. */
3379
3380
3381 /* for m=en-2 step -1 until l do -- .......... */
3382 i__1 = enm2;
3383 for(mm = l; mm <= i__1; ++mm) {
3384 m = enm2 + l - mm;
3385 zz = h__[m + m * h_dim1];
3386 r__ = x - zz;
3387 s = y - zz;
3388 p = (r__ * s - w) / h__[m + 1 + m * h_dim1] + h__[m + (m + 1) * h_dim1];
3389 q = h__[m + 1 + (m + 1) * h_dim1] - zz - r__ - s;
3390 r__ = h__[m + 2 + (m + 1) * h_dim1];
3391 s = abs(p) + abs(q) + abs(r__);
3392 p /= s;
3393 q /= s;
3394 r__ /= s;
3395 if(m == l) {
3396 goto L150;
3397 }
3398 tst1 = abs(p) * ((d__1 = h__[m - 1 + (m - 1) * h_dim1], abs(d__1)) +
3399 abs(zz) + (d__2 = h__[m + 1 + (m + 1) * h_dim1], abs(d__2)));
3400 tst2 = tst1 + (d__1 = h__[m + (m - 1) * h_dim1], abs(d__1)) * (abs(q)
3401 + abs(r__));
3402 if(tst2 == tst1) {
3403 goto L150;
3404 }
3405
3406
3407 /* L140: */
3408 }
3409
3410 L150:
3411 mp2 = m + 2;
3412
3413 i__1 = en;
3414 for(i__ = mp2; i__ <= i__1; ++i__) {
3415 h__[i__ + (i__ - 2) * h_dim1] = 0.;
3416 if(i__ == mp2) {
3417 goto L160;
3418 }
3419 h__[i__ + (i__ - 3) * h_dim1] = 0.;
3420 L160:
3421 ;
3422 }
3423
3424
3425 /* .......... double qr step involving rows l to en and */
3426
3427
3428 /* columns m to en .......... */
3429 i__1 = na;
3430 for(k = m; k <= i__1; ++k) {
3431 notlas = k != na;
3432 if(k == m) {
3433 goto L170;
3434 }
3435 p = h__[k + (k - 1) * h_dim1];
3436 q = h__[k + 1 + (k - 1) * h_dim1];
3437 r__ = 0.;
3438 if(notlas) {
3439 r__ = h__[k + 2 + (k - 1) * h_dim1];
3440 }
3441 x = abs(p) + abs(q) + abs(r__);
3442 if(x == 0.) {
3443 goto L260;
3444 }
3445 p /= x;
3446 q /= x;
3447 r__ /= x;
3448 L170:
3449 d__1 = sqrt1d(p * p + q * q + r__ * r__);
3450 s = d_sign(&d__1, &p);
3451 if(k == m) {
3452 goto L180;
3453 }
3454 h__[k + (k - 1) * h_dim1] = -s * x;
3455 goto L190;
3456 L180:
3457 if(l != m) {
3458 h__[k + (k - 1) * h_dim1] = -h__[k + (k - 1) * h_dim1];
3459 }
3460 L190:
3461 p += s;
3462 x = p / s;
3463 y = q / s;
3464 zz = r__ / s;
3465 q /= p;
3466 r__ /= p;
3467 if(notlas) {
3468 goto L225;
3469 }
3470
3471
3472 /* .......... row modification .......... */
3473 i__2 = *n;
3474 for(j = k; j <= i__2; ++j) {
3475 p = h__[k + j * h_dim1] + q * h__[k + 1 + j * h_dim1];
3476 h__[k + j * h_dim1] -= p * x;
3477 h__[k + 1 + j * h_dim1] -= p * y;
3478
3479
3480 /* L200: */
3481 }
3482
3483
3484 /* Computing MIN */
3485 i__2 = en, i__3 = k + 3;
3486 j = mymin(i__2, i__3);
3487
3488
3489 /* .......... column modification .......... */
3490 i__2 = j;
3491 for(i__ = 1; i__ <= i__2; ++i__) {
3492 p = x * h__[i__ + k * h_dim1] + y * h__[i__ + (k + 1) * h_dim1];
3493 h__[i__ + k * h_dim1] -= p;
3494 h__[i__ + (k + 1) * h_dim1] -= p * q;
3495
3496
3497 /* L210: */
3498 }
3499
3500
3501 /* .......... accumulate transformations .......... */
3502 i__2 = *igh;
3503 for(i__ = *low; i__ <= i__2; ++i__) {
3504 p = x * z__[i__ + k * z_dim1] + y * z__[i__ + (k + 1) * z_dim1];
3505 z__[i__ + k * z_dim1] -= p;
3506 z__[i__ + (k + 1) * z_dim1] -= p * q;
3507
3508
3509 /* L220: */
3510 }
3511 goto L255;
3512 L225:
3513
3514
3515 /* .......... row modification .......... */
3516 i__2 = *n;
3517 for(j = k; j <= i__2; ++j) {
3518 p =
3519 h__[k + j * h_dim1] + q * h__[k + 1 + j * h_dim1] + r__ * h__[k + 2 + j * h_dim1];
3520 h__[k + j * h_dim1] -= p * x;
3521 h__[k + 1 + j * h_dim1] -= p * y;
3522 h__[k + 2 + j * h_dim1] -= p * zz;
3523
3524
3525 /* L230: */
3526 }
3527
3528
3529 /* Computing MIN */
3530 i__2 = en, i__3 = k + 3;
3531 j = mymin(i__2, i__3);
3532
3533
3534 /* .......... column modification .......... */
3535 i__2 = j;
3536 for(i__ = 1; i__ <= i__2; ++i__) {
3537 p = x * h__[i__ + k * h_dim1] + y * h__[i__ + (k + 1) * h_dim1] +
3538 zz * h__[i__ + (k + 2) * h_dim1];
3539 h__[i__ + k * h_dim1] -= p;
3540 h__[i__ + (k + 1) * h_dim1] -= p * q;
3541 h__[i__ + (k + 2) * h_dim1] -= p * r__;
3542
3543
3544 /* L240: */
3545 }
3546
3547
3548 /* .......... accumulate transformations .......... */
3549 i__2 = *igh;
3550 for(i__ = *low; i__ <= i__2; ++i__) {
3551 p = x * z__[i__ + k * z_dim1] + y * z__[i__ + (k + 1) * z_dim1] +
3552 zz * z__[i__ + (k + 2) * z_dim1];
3553 z__[i__ + k * z_dim1] -= p;
3554 z__[i__ + (k + 1) * z_dim1] -= p * q;
3555 z__[i__ + (k + 2) * z_dim1] -= p * r__;
3556
3557
3558 /* L250: */
3559 }
3560 L255:
3561
3562 L260:
3563 ;
3564 }
3565
3566 goto L70;
3567
3568
3569 /* .......... one root found .......... */
3570 L270:
3571 h__[en + en * h_dim1] = x + t;
3572 wr[en] = h__[en + en * h_dim1];
3573 wi[en] = 0.;
3574 en = na;
3575 goto L60;
3576
3577
3578 /* .......... two roots found .......... */
3579 L280:
3580 p = (y - x) / 2.;
3581 q = p * p + w;
3582 zz = sqrt1d((abs(q)));
3583 h__[en + en * h_dim1] = x + t;
3584 x = h__[en + en * h_dim1];
3585 h__[na + na * h_dim1] = y + t;
3586 if(q < 0.) {
3587 goto L320;
3588 }
3589
3590
3591 /* .......... real pair .......... */
3592 zz = p + d_sign(&zz, &p);
3593 wr[na] = x + zz;
3594 wr[en] = wr[na];
3595 if(zz != 0.) {
3596 wr[en] = x - w / zz;
3597 }
3598 wi[na] = 0.;
3599 wi[en] = 0.;
3600 x = h__[en + na * h_dim1];
3601 s = abs(x) + abs(zz);
3602 p = x / s;
3603 q = zz / s;
3604 r__ = sqrt1d(p * p + q * q);
3605 p /= r__;
3606 q /= r__;
3607
3608
3609 /* .......... row modification .......... */
3610 i__1 = *n;
3611 for(j = na; j <= i__1; ++j) {
3612 zz = h__[na + j * h_dim1];
3613 h__[na + j * h_dim1] = q * zz + p * h__[en + j * h_dim1];
3614 h__[en + j * h_dim1] = q * h__[en + j * h_dim1] - p * zz;
3615
3616
3617 /* L290: */
3618 }
3619
3620
3621 /* .......... column modification .......... */
3622 i__1 = en;
3623 for(i__ = 1; i__ <= i__1; ++i__) {
3624 zz = h__[i__ + na * h_dim1];
3625 h__[i__ + na * h_dim1] = q * zz + p * h__[i__ + en * h_dim1];
3626 h__[i__ + en * h_dim1] = q * h__[i__ + en * h_dim1] - p * zz;
3627
3628
3629 /* L300: */
3630 }
3631
3632
3633 /* .......... accumulate transformations .......... */
3634 i__1 = *igh;
3635 for(i__ = *low; i__ <= i__1; ++i__) {
3636 zz = z__[i__ + na * z_dim1];
3637 z__[i__ + na * z_dim1] = q * zz + p * z__[i__ + en * z_dim1];
3638 z__[i__ + en * z_dim1] = q * z__[i__ + en * z_dim1] - p * zz;
3639
3640
3641 /* L310: */
3642 }
3643
3644 goto L330;
3645
3646
3647 /* .......... complex pair .......... */
3648 L320:
3649 wr[na] = x + p;
3650 wr[en] = x + p;
3651 wi[na] = zz;
3652 wi[en] = -zz;
3653 L330:
3654 en = enm2;
3655 goto L60;
3656
3657
3658 /* .......... all roots found. backsubstitute to find */
3659
3660
3661 /* vectors of upper triangular form .......... */
3662 L340:
3663 if(norm == 0.) {
3664 goto L1001;
3665 }
3666
3667
3668 /* .......... for en=n step -1 until 1 do -- .......... */
3669 i__1 = *n;
3670 for(nn = 1; nn <= i__1; ++nn) {
3671 en = *n + 1 - nn;
3672 p = wr[en];
3673 q = wi[en];
3674 na = en - 1;
3675 if(q < 0.) {
3676 goto L710;
3677 } else if(q == 0) {
3678 goto L600;
3679 } else {
3680 goto L800;
3681 }
3682
3683
3684 /* .......... real vector .......... */
3685 L600:
3686 m = en;
3687 h__[en + en * h_dim1] = 1.;
3688 if(na == 0) {
3689 goto L800;
3690 }
3691
3692
3693 /* .......... for i=en-1 step -1 until 1 do -- .......... */
3694 i__2 = na;
3695 for(ii = 1; ii <= i__2; ++ii) {
3696 i__ = en - ii;
3697 w = h__[i__ + i__ * h_dim1] - p;
3698 r__ = 0.;
3699
3700 i__3 = en;
3701 for(j = m; j <= i__3; ++j) {
3702
3703
3704 /* L610: */
3705 r__ += h__[i__ + j * h_dim1] * h__[j + en * h_dim1];
3706 }
3707
3708 if(wi[i__] >= 0.) {
3709 goto L630;
3710 }
3711 zz = w;
3712 s = r__;
3713 goto L700;
3714 L630:
3715 m = i__;
3716 if(wi[i__] != 0.) {
3717 goto L640;
3718 }
3719 t = w;
3720 if(t != 0.) {
3721 goto L635;
3722 }
3723 tst1 = norm;
3724 t = tst1;
3725 L632:
3726 t *= .01;
3727 tst2 = norm + t;
3728 if(tst2 > tst1) {
3729 goto L632;
3730 }
3731 L635:
3732 h__[i__ + en * h_dim1] = -r__ / t;
3733 goto L680;
3734
3735
3736 /* .......... solve real equations .......... */
3737 L640:
3738 x = h__[i__ + (i__ + 1) * h_dim1];
3739 y = h__[i__ + 1 + i__ * h_dim1];
3740 q = (wr[i__] - p) * (wr[i__] - p) + wi[i__] * wi[i__];
3741 t = (x * s - zz * r__) / q;
3742 h__[i__ + en * h_dim1] = t;
3743 if(abs(x) <= abs(zz)) {
3744 goto L650;
3745 }
3746 h__[i__ + 1 + en * h_dim1] = (-r__ - w * t) / x;
3747 goto L680;
3748 L650:
3749 h__[i__ + 1 + en * h_dim1] = (-s - y * t) / zz;
3750
3751
3752 /* .......... overflow control .......... */
3753 L680:
3754 t = (d__1 = h__[i__ + en * h_dim1], abs(d__1));
3755 if(t == 0.) {
3756 goto L700;
3757 }
3758 tst1 = t;
3759 tst2 = tst1 + 1. / tst1;
3760 if(tst2 > tst1) {
3761 goto L700;
3762 }
3763 i__3 = en;
3764 for(j = i__; j <= i__3; ++j) {
3765 h__[j + en * h_dim1] /= t;
3766
3767
3768 /* L690: */
3769 }
3770
3771 L700:
3772 ;
3773 }
3774
3775
3776 /* .......... end real vector .......... */
3777 goto L800;
3778
3779
3780 /* .......... complex vector .......... */
3781 L710:
3782 m = na;
3783
3784
3785 /* .......... last vector component chosen imaginary so that */
3786
3787
3788 /* eigenvector matrix is triangular .......... */
3789 if((d__1 = h__[en + na * h_dim1], abs(d__1)) <= (d__2 = h__[na + en *
3790 h_dim1], abs(d__2))) {
3791 goto L720;
3792 }
3793 h__[na + na * h_dim1] = q / h__[en + na * h_dim1];
3794 h__[na + en * h_dim1] = -(h__[en + en * h_dim1] - p) / h__[en + na * h_dim1];
3795 goto L730;
3796 L720:
3797 d__1 = -h__[na + en * h_dim1];
3798 d__2 = h__[na + na * h_dim1] - p;
3799 cdiv_(&c_b126, &d__1, &d__2, &q, &h__[na + na * h_dim1], &h__[na + en * h_dim1]);
3800 L730:
3801 h__[en + na * h_dim1] = 0.;
3802 h__[en + en * h_dim1] = 1.;
3803 enm2 = na - 1;
3804 if(enm2 == 0) {
3805 goto L800;
3806 }
3807
3808
3809 /* .......... for i=en-2 step -1 until 1 do -- .......... */
3810 i__2 = enm2;
3811 for(ii = 1; ii <= i__2; ++ii) {
3812 i__ = na - ii;
3813 w = h__[i__ + i__ * h_dim1] - p;
3814 ra = 0.;
3815 sa = 0.;
3816
3817 i__3 = en;
3818 for(j = m; j <= i__3; ++j) {
3819 ra += h__[i__ + j * h_dim1] * h__[j + na * h_dim1];
3820 sa += h__[i__ + j * h_dim1] * h__[j + en * h_dim1];
3821
3822
3823 /* L760: */
3824 }
3825
3826 if(wi[i__] >= 0.) {
3827 goto L770;
3828 }
3829 zz = w;
3830 r__ = ra;
3831 s = sa;
3832 goto L795;
3833 L770:
3834 m = i__;
3835 if(wi[i__] != 0.) {
3836 goto L780;
3837 }
3838 d__1 = -ra;
3839 d__2 = -sa;
3840 cdiv_(&d__1, &d__2, &w, &q, &h__[i__ + na * h_dim1], &h__[i__ + en * h_dim1]);
3841 goto L790;
3842
3843
3844 /* .......... solve complex equations .......... */
3845 L780:
3846 x = h__[i__ + (i__ + 1) * h_dim1];
3847 y = h__[i__ + 1 + i__ * h_dim1];
3848 vr = (wr[i__] - p) * (wr[i__] - p) + wi[i__] * wi[i__] - q * q;
3849 vi = (wr[i__] - p) * 2. * q;
3850 if(vr != 0. || vi != 0.) {
3851 goto L784;
3852 }
3853 tst1 = norm * (abs(w) + abs(q) + abs(x) + abs(y) + abs(zz));
3854 vr = tst1;
3855 L783:
3856 vr *= .01;
3857 tst2 = tst1 + vr;
3858 if(tst2 > tst1) {
3859 goto L783;
3860 }
3861 L784:
3862 d__1 = x * r__ - zz * ra + q * sa;
3863 d__2 = x * s - zz * sa - q * ra;
3864 cdiv_(&d__1, &d__2, &vr, &vi, &h__[i__ + na * h_dim1], &h__[i__ + en * h_dim1]);
3865 if(abs(x) <= abs(zz) + abs(q)) {
3866 goto L785;
3867 }
3868 h__[i__ + 1 + na * h_dim1] = (-ra - w * h__[i__ + na * h_dim1] +
3869 q * h__[i__ + en * h_dim1]) / x;
3870 h__[i__ + 1 + en * h_dim1] = (-sa - w * h__[i__ + en * h_dim1] -
3871 q * h__[i__ + na * h_dim1]) / x;
3872 goto L790;
3873 L785:
3874 d__1 = -r__ - y * h__[i__ + na * h_dim1];
3875 d__2 = -s - y * h__[i__ + en * h_dim1];
3876 cdiv_(&d__1, &d__2, &zz, &q, &h__[i__ + 1 + na * h_dim1],
3877 &h__[i__ + 1 + en * h_dim1]);
3878
3879
3880 /* .......... overflow control .......... */
3881 L790:
3882
3883
3884 /* Computing MAX */
3885 d__3 = (d__1 = h__[i__ + na * h_dim1], abs(d__1)), d__4 = (d__2 =
3886 h__[i__ + en * h_dim1],
3887 abs(d__2));
3888 t = mymax(d__3, d__4);
3889 if(t == 0.) {
3890 goto L795;
3891 }
3892 tst1 = t;
3893 tst2 = tst1 + 1. / tst1;
3894 if(tst2 > tst1) {
3895 goto L795;
3896 }
3897 i__3 = en;
3898 for(j = i__; j <= i__3; ++j) {
3899 h__[j + na * h_dim1] /= t;
3900 h__[j + en * h_dim1] /= t;
3901
3902
3903 /* L792: */
3904 }
3905
3906 L795:
3907 ;
3908 }
3909
3910
3911 /* .......... end complex vector .......... */
3912 L800:
3913 ;
3914 }
3915
3916
3917 /* .......... end back substitution. */
3918
3919
3920 /* vectors of isolated roots .......... */
3921 i__1 = *n;
3922 for(i__ = 1; i__ <= i__1; ++i__) {
3923 if(i__ >= *low && i__ <= *igh) {
3924 goto L840;
3925 }
3926
3927 i__2 = *n;
3928 for(j = i__; j <= i__2; ++j) {
3929
3930
3931 /* L820: */
3932 z__[i__ + j * z_dim1] = h__[i__ + j * h_dim1];
3933 }
3934
3935 L840:
3936 ;
3937 }
3938
3939
3940 /* .......... multiply by transformation matrix to give */
3941
3942
3943 /* vectors of original full matrix. */
3944
3945
3946 /* for j=n step -1 until low do -- .......... */
3947 i__1 = *n;
3948 for(jj = *low; jj <= i__1; ++jj) {
3949 j = *n + *low - jj;
3950 m = mymin(j, *igh);
3951
3952 i__2 = *igh;
3953 for(i__ = *low; i__ <= i__2; ++i__) {
3954 zz = 0.;
3955
3956 i__3 = m;
3957 for(k = *low; k <= i__3; ++k) {
3958
3959
3960 /* L860: */
3961 zz += z__[i__ + k * z_dim1] * h__[k + j * h_dim1];
3962 }
3963
3964 z__[i__ + j * z_dim1] = zz;
3965
3966
3967 /* L880: */
3968 }
3969 }
3970
3971 goto L1001;
3972
3973
3974 /* .......... set error -- all eigenvalues have not */
3975
3976
3977 /* converged after 30*n iterations .......... */
3978 L1000:
3979 *ierr = en;
3980 L1001:
3981 return 0;
3982 } /* hqr2_ */
3983
pymol_rg_(integer * nm,integer * n,doublereal * a,doublereal * wr,doublereal * wi,integer * matz,doublereal * z__,integer * iv1,doublereal * fv1,integer * ierr)3984 int pymol_rg_(integer * nm, integer * n, doublereal * a, doublereal * wr,
3985 doublereal * wi, integer * matz, doublereal * z__, integer * iv1,
3986 doublereal * fv1, integer * ierr)
3987 {
3988 /* System generated locals */
3989 integer a_dim1, a_offset, z_dim1, z_offset;
3990
3991 /* Local variables */
3992 integer is1, is2;
3993
3994
3995 /* this subroutine calls the recommended sequence of */
3996
3997
3998 /* subroutines from the eigensystem subroutine package (eispack) */
3999
4000
4001 /* to find the eigenvalues and eigenvectors (if desired) */
4002
4003
4004 /* of a real general matrix. */
4005
4006
4007 /* on input */
4008
4009
4010 /* nm must be set to the row dimension of the two-dimensional */
4011
4012
4013 /* array parameters as declared in the calling program */
4014
4015
4016 /* dimension statement. */
4017
4018
4019 /* n is the order of the matrix a. */
4020
4021
4022 /* a contains the real general matrix. */
4023
4024
4025 /* matz is an integer variable set equal to zero if */
4026
4027
4028 /* only eigenvalues are desired. otherwise it is set to */
4029
4030
4031 /* any non-zero integer for both eigenvalues and eigenvectors. */
4032
4033
4034 /* on output */
4035
4036
4037 /* wr and wi contain the real and imaginary parts, */
4038
4039
4040 /* respectively, of the eigenvalues. complex conjugate */
4041
4042
4043 /* pairs of eigenvalues appear consecutively with the */
4044
4045
4046 /* eigenvalue having the positive imaginary part first. */
4047
4048
4049 /* z contains the real and imaginary parts of the eigenvectors */
4050
4051
4052 /* if matz is not zero. if the j-th eigenvalue is real, the */
4053
4054
4055 /* j-th column of z contains its eigenvector. if the j-th */
4056
4057
4058 /* eigenvalue is complex with positive imaginary part, the */
4059
4060
4061 /* j-th and (j+1)-th columns of z contain the real and */
4062
4063
4064 /* imaginary parts of its eigenvector. the conjugate of this */
4065
4066
4067 /* vector is the eigenvector for the conjugate eigenvalue. */
4068
4069
4070 /* ierr is an integer output variable set equal to an error */
4071
4072
4073 /* completion code described in the documentation for hqr */
4074
4075
4076 /* and hqr2. the normal completion code is zero. */
4077
4078
4079 /* iv1 and fv1 are temporary storage arrays. */
4080
4081
4082 /* questions and comments should be directed to burton s. garbow, */
4083
4084
4085 /* mathematics and computer science div, argonne national laboratory
4086 */
4087
4088
4089 /* this version dated august 1983. */
4090
4091
4092 /* ------------------------------------------------------------------
4093 */
4094
4095 /* Parameter adjustments */
4096 --fv1;
4097 --iv1;
4098 z_dim1 = *nm;
4099 z_offset = z_dim1 + 1;
4100 z__ -= z_offset;
4101 --wi;
4102 --wr;
4103 a_dim1 = *nm;
4104 a_offset = a_dim1 + 1;
4105 a -= a_offset;
4106
4107 /* Function Body */
4108 if(*n <= *nm) {
4109 goto L10;
4110 }
4111 *ierr = *n * 10;
4112 goto L50;
4113
4114 L10:
4115 balanc_(nm, n, &a[a_offset], &is1, &is2, &fv1[1]);
4116 elmhes_(nm, n, &is1, &is2, &a[a_offset], &iv1[1]);
4117 if(*matz != 0) {
4118 goto L20;
4119 }
4120
4121
4122 /* .......... find eigenvalues only .......... */
4123 hqr_(nm, n, &is1, &is2, &a[a_offset], &wr[1], &wi[1], ierr);
4124 goto L50;
4125
4126
4127 /* .......... find both eigenvalues and eigenvectors .......... */
4128 L20:
4129 eltran_(nm, n, &is1, &is2, &a[a_offset], &iv1[1], &z__[z_offset]);
4130 hqr2_(nm, n, &is1, &is2, &a[a_offset], &wr[1], &wi[1], &z__[z_offset], ierr);
4131 if(*ierr != 0) {
4132 goto L50;
4133 }
4134 balbak_(nm, n, &is1, &is2, &fv1[1], n, &z__[z_offset]);
4135 L50:
4136 return 0;
4137 } /* rg_ */
4138