1 /*************************************************************************\
2 
3   Copyright 1995 The University of North Carolina at Chapel Hill.
4   All Rights Reserved.
5 
6   Permission to use, copy, modify and distribute this software and its
7   documentation for educational, research and non-profit purposes, without
8   fee, and without a written agreement is hereby granted, provided that the
9   above copyright notice and the following three paragraphs appear in all
10   copies.
11 
12   IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
13   LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
14   CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
15   USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
16   OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
17   DAMAGES.
18 
19 
20   Permission to use, copy, modify and distribute this software and its
21   documentation for educational, research and non-profit purposes, without
22   fee, and without a written agreement is hereby granted, provided that the
23   above copyright notice and the following three paragraphs appear in all
24   copies.
25 
26   THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
27   WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
28   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
29   PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
30   NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
31   UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
32 
33   The authors may be contacted via:
34 
35   US Mail:             S. Gottschalk
36                        Department of Computer Science
37                        Sitterson Hall, CB #3175
38                        University of N. Carolina
39                        Chapel Hill, NC 27599-3175
40 
41   Phone:               (919)962-1749
42 
43   EMail:              {gottscha}@cs.unc.edu
44 
45 
46 \**************************************************************************/
47 
48 
49 
50 #ifndef MATVEC_H
51 #define MATVEC_H
52 
53 #include <math.h>
54 #include <stdio.h>
55 
56 #ifdef gnu
57 #include "zzzz.h"
58 
59 #ifdef hppa
60 #define myfabs(x) \
61  ({double __value, __arg = (x); \
62    asm("fabs,dbl %1, %0": "=f" (__value): "f" (__arg)); \
63    __value; \
64 });
65 #endif
66 
67 #ifdef mips
68 #define myfabs(x) \
69  ({double __value, __arg = (x); \
70    asm("abs.d %0, %1": "=f" (__value): "f" (__arg)); \
71    __value; \
72 });
73 #endif
74 
75 #else
76 
77 #define myfabs(x) ((x < 0) ? -x : x)
78 
79 #endif
80 
81 
82 inline
83 void
Mprintg(double M[3][3])84 Mprintg(double M[3][3])
85 {
86   printf("%g %g %g\n%g %g %g\n%g %g %g\n",
87 	 M[0][0], M[0][1], M[0][2],
88 	 M[1][0], M[1][1], M[1][2],
89 	 M[2][0], M[2][1], M[2][2]);
90 }
91 
92 
93 inline
94 void
Mfprint(FILE * f,double M[3][3])95 Mfprint(FILE *f, double M[3][3])
96 {
97   fprintf(f, "%g %g %g\n%g %g %g\n%g %g %g\n",
98 	 M[0][0], M[0][1], M[0][2],
99 	 M[1][0], M[1][1], M[1][2],
100 	 M[2][0], M[2][1], M[2][2]);
101 }
102 
103 inline
104 void
Mprint(double M[3][3])105 Mprint(double M[3][3])
106 {
107   printf("%g %g %g\n%g %g %g\n%g %g %g\n",
108 	 M[0][0], M[0][1], M[0][2],
109 	 M[1][0], M[1][1], M[1][2],
110 	 M[2][0], M[2][1], M[2][2]);
111 }
112 
113 inline
114 void
Vprintg(double V[3])115 Vprintg(double V[3])
116 {
117   printf("%g %g %g\n", V[0], V[1], V[2]);
118 }
119 
120 inline
121 void
Vfprint(FILE * f,double V[3])122 Vfprint(FILE *f, double V[3])
123 {
124   fprintf(f, "%g %g %g\n", V[0], V[1], V[2]);
125 }
126 
127 inline
128 void
Vprint(double V[3])129 Vprint(double V[3])
130 {
131   printf("%g %g %g\n", V[0], V[1], V[2]);
132 }
133 
134 inline
135 void
Midentity(double M[3][3])136 Midentity(double M[3][3])
137 {
138   M[0][0] = M[1][1] = M[2][2] = 1.0;
139   M[0][1] = M[1][2] = M[2][0] = 0.0;
140   M[0][2] = M[1][0] = M[2][1] = 0.0;
141 }
142 
143 inline
144 void
McM(double Mr[3][3],double M[3][3])145 McM(double Mr[3][3], double M[3][3])
146 {
147   Mr[0][0] = M[0][0];  Mr[0][1] = M[0][1];  Mr[0][2] = M[0][2];
148   Mr[1][0] = M[1][0];  Mr[1][1] = M[1][1];  Mr[1][2] = M[1][2];
149   Mr[2][0] = M[2][0];  Mr[2][1] = M[2][1];  Mr[2][2] = M[2][2];
150 }
151 
152 inline
153 void
VcV(double Vr[3],double V[3])154 VcV(double Vr[3], double V[3])
155 {
156   Vr[0] = V[0];  Vr[1] = V[1];  Vr[2] = V[2];
157 }
158 
159 inline
160 void
McolcV(double Vr[3],double M[3][3],int c)161 McolcV(double Vr[3], double M[3][3], int c)
162 {
163   Vr[0] = M[0][c];
164   Vr[1] = M[1][c];
165   Vr[2] = M[2][c];
166 }
167 
168 inline
169 void
McolcMcol(double Mr[3][3],int cr,double M[3][3],int c)170 McolcMcol(double Mr[3][3], int cr, double M[3][3], int c)
171 {
172   Mr[0][cr] = M[0][c];
173   Mr[1][cr] = M[1][c];
174   Mr[2][cr] = M[2][c];
175 }
176 
177 inline
178 void
MxMpV(double Mr[3][3],double M1[3][3],double M2[3][3],double T[3])179 MxMpV(double Mr[3][3], double M1[3][3], double M2[3][3], double T[3])
180 {
181   Mr[0][0] = (M1[0][0] * M2[0][0] +
182 	      M1[0][1] * M2[1][0] +
183 	      M1[0][2] * M2[2][0] +
184 	      T[0]);
185   Mr[1][0] = (M1[1][0] * M2[0][0] +
186 	      M1[1][1] * M2[1][0] +
187 	      M1[1][2] * M2[2][0] +
188 	      T[1]);
189   Mr[2][0] = (M1[2][0] * M2[0][0] +
190 	      M1[2][1] * M2[1][0] +
191 	      M1[2][2] * M2[2][0] +
192 	      T[2]);
193   Mr[0][1] = (M1[0][0] * M2[0][1] +
194 	      M1[0][1] * M2[1][1] +
195 	      M1[0][2] * M2[2][1] +
196 	      T[0]);
197   Mr[1][1] = (M1[1][0] * M2[0][1] +
198 	      M1[1][1] * M2[1][1] +
199  	      M1[1][2] * M2[2][1] +
200 	      T[1]);
201   Mr[2][1] = (M1[2][0] * M2[0][1] +
202 	      M1[2][1] * M2[1][1] +
203 	      M1[2][2] * M2[2][1] +
204 	      T[2]);
205   Mr[0][2] = (M1[0][0] * M2[0][2] +
206 	      M1[0][1] * M2[1][2] +
207 	      M1[0][2] * M2[2][2] +
208 	      T[0]);
209   Mr[1][2] = (M1[1][0] * M2[0][2] +
210 	      M1[1][1] * M2[1][2] +
211 	      M1[1][2] * M2[2][2] +
212 	      T[1]);
213   Mr[2][2] = (M1[2][0] * M2[0][2] +
214 	      M1[2][1] * M2[1][2] +
215 	      M1[2][2] * M2[2][2] +
216 	      T[2]);
217 }
218 
219 inline
220 void
MxM(double Mr[3][3],double M1[3][3],double M2[3][3])221 MxM(double Mr[3][3], double M1[3][3], double M2[3][3])
222 {
223   Mr[0][0] = (M1[0][0] * M2[0][0] +
224 	      M1[0][1] * M2[1][0] +
225 	      M1[0][2] * M2[2][0]);
226   Mr[1][0] = (M1[1][0] * M2[0][0] +
227 	      M1[1][1] * M2[1][0] +
228 	      M1[1][2] * M2[2][0]);
229   Mr[2][0] = (M1[2][0] * M2[0][0] +
230 	      M1[2][1] * M2[1][0] +
231 	      M1[2][2] * M2[2][0]);
232   Mr[0][1] = (M1[0][0] * M2[0][1] +
233 	      M1[0][1] * M2[1][1] +
234 	      M1[0][2] * M2[2][1]);
235   Mr[1][1] = (M1[1][0] * M2[0][1] +
236 	      M1[1][1] * M2[1][1] +
237  	      M1[1][2] * M2[2][1]);
238   Mr[2][1] = (M1[2][0] * M2[0][1] +
239 	      M1[2][1] * M2[1][1] +
240 	      M1[2][2] * M2[2][1]);
241   Mr[0][2] = (M1[0][0] * M2[0][2] +
242 	      M1[0][1] * M2[1][2] +
243 	      M1[0][2] * M2[2][2]);
244   Mr[1][2] = (M1[1][0] * M2[0][2] +
245 	      M1[1][1] * M2[1][2] +
246 	      M1[1][2] * M2[2][2]);
247   Mr[2][2] = (M1[2][0] * M2[0][2] +
248 	      M1[2][1] * M2[1][2] +
249 	      M1[2][2] * M2[2][2]);
250 }
251 
252 
253 inline
254 void
MxMT(double Mr[3][3],double M1[3][3],double M2[3][3])255 MxMT(double Mr[3][3], double M1[3][3], double M2[3][3])
256 {
257   Mr[0][0] = (M1[0][0] * M2[0][0] +
258 	      M1[0][1] * M2[0][1] +
259 	      M1[0][2] * M2[0][2]);
260   Mr[1][0] = (M1[1][0] * M2[0][0] +
261 	      M1[1][1] * M2[0][1] +
262 	      M1[1][2] * M2[0][2]);
263   Mr[2][0] = (M1[2][0] * M2[0][0] +
264 	      M1[2][1] * M2[0][1] +
265 	      M1[2][2] * M2[0][2]);
266   Mr[0][1] = (M1[0][0] * M2[1][0] +
267 	      M1[0][1] * M2[1][1] +
268 	      M1[0][2] * M2[1][2]);
269   Mr[1][1] = (M1[1][0] * M2[1][0] +
270 	      M1[1][1] * M2[1][1] +
271  	      M1[1][2] * M2[1][2]);
272   Mr[2][1] = (M1[2][0] * M2[1][0] +
273 	      M1[2][1] * M2[1][1] +
274 	      M1[2][2] * M2[1][2]);
275   Mr[0][2] = (M1[0][0] * M2[2][0] +
276 	      M1[0][1] * M2[2][1] +
277 	      M1[0][2] * M2[2][2]);
278   Mr[1][2] = (M1[1][0] * M2[2][0] +
279 	      M1[1][1] * M2[2][1] +
280 	      M1[1][2] * M2[2][2]);
281   Mr[2][2] = (M1[2][0] * M2[2][0] +
282 	      M1[2][1] * M2[2][1] +
283 	      M1[2][2] * M2[2][2]);
284 }
285 
286 inline
287 void
MTxM(double Mr[3][3],double M1[3][3],double M2[3][3])288 MTxM(double Mr[3][3], double M1[3][3], double M2[3][3])
289 {
290   Mr[0][0] = (M1[0][0] * M2[0][0] +
291 	      M1[1][0] * M2[1][0] +
292 	      M1[2][0] * M2[2][0]);
293   Mr[1][0] = (M1[0][1] * M2[0][0] +
294 	      M1[1][1] * M2[1][0] +
295 	      M1[2][1] * M2[2][0]);
296   Mr[2][0] = (M1[0][2] * M2[0][0] +
297 	      M1[1][2] * M2[1][0] +
298 	      M1[2][2] * M2[2][0]);
299   Mr[0][1] = (M1[0][0] * M2[0][1] +
300 	      M1[1][0] * M2[1][1] +
301 	      M1[2][0] * M2[2][1]);
302   Mr[1][1] = (M1[0][1] * M2[0][1] +
303 	      M1[1][1] * M2[1][1] +
304  	      M1[2][1] * M2[2][1]);
305   Mr[2][1] = (M1[0][2] * M2[0][1] +
306 	      M1[1][2] * M2[1][1] +
307 	      M1[2][2] * M2[2][1]);
308   Mr[0][2] = (M1[0][0] * M2[0][2] +
309 	      M1[1][0] * M2[1][2] +
310 	      M1[2][0] * M2[2][2]);
311   Mr[1][2] = (M1[0][1] * M2[0][2] +
312 	      M1[1][1] * M2[1][2] +
313 	      M1[2][1] * M2[2][2]);
314   Mr[2][2] = (M1[0][2] * M2[0][2] +
315 	      M1[1][2] * M2[1][2] +
316 	      M1[2][2] * M2[2][2]);
317 }
318 
319 inline
320 void
MxV(double Vr[3],double M1[3][3],double V1[3])321 MxV(double Vr[3], double M1[3][3], double V1[3])
322 {
323   Vr[0] = (M1[0][0] * V1[0] +
324 	   M1[0][1] * V1[1] +
325 	   M1[0][2] * V1[2]);
326   Vr[1] = (M1[1][0] * V1[0] +
327 	   M1[1][1] * V1[1] +
328 	   M1[1][2] * V1[2]);
329   Vr[2] = (M1[2][0] * V1[0] +
330 	   M1[2][1] * V1[1] +
331 	   M1[2][2] * V1[2]);
332 }
333 
334 
335 inline
336 void
MxVpV(double Vr[3],double M1[3][3],double V1[3],double V2[3])337 MxVpV(double Vr[3], double M1[3][3], double V1[3], double V2[3])
338 {
339   Vr[0] = (M1[0][0] * V1[0] +
340 	   M1[0][1] * V1[1] +
341 	   M1[0][2] * V1[2] +
342 	   V2[0]);
343   Vr[1] = (M1[1][0] * V1[0] +
344 	   M1[1][1] * V1[1] +
345 	   M1[1][2] * V1[2] +
346 	   V2[1]);
347   Vr[2] = (M1[2][0] * V1[0] +
348 	   M1[2][1] * V1[1] +
349 	   M1[2][2] * V1[2] +
350 	   V2[2]);
351 }
352 
353 inline
354 void
sMxVpV(double Vr[3],double s1,double M1[3][3],double V1[3],double V2[3])355 sMxVpV(double Vr[3], double s1, double M1[3][3], double V1[3], double V2[3])
356 {
357   Vr[0] = s1 * (M1[0][0] * V1[0] +
358 		M1[0][1] * V1[1] +
359 		M1[0][2] * V1[2]) +
360 		V2[0];
361   Vr[1] = s1 * (M1[1][0] * V1[0] +
362 		M1[1][1] * V1[1] +
363 		M1[1][2] * V1[2]) +
364 		V2[1];
365   Vr[2] = s1 * (M1[2][0] * V1[0] +
366 		M1[2][1] * V1[1] +
367 		M1[2][2] * V1[2]) +
368 		V2[2];
369 }
370 
371 inline
372 void
MTxV(double Vr[3],double M1[3][3],double V1[3])373 MTxV(double Vr[3], double M1[3][3], double V1[3])
374 {
375   Vr[0] = (M1[0][0] * V1[0] +
376 	   M1[1][0] * V1[1] +
377 	   M1[2][0] * V1[2]);
378   Vr[1] = (M1[0][1] * V1[0] +
379 	   M1[1][1] * V1[1] +
380 	   M1[2][1] * V1[2]);
381   Vr[2] = (M1[0][2] * V1[0] +
382 	   M1[1][2] * V1[1] +
383 	   M1[2][2] * V1[2]);
384 }
385 
386 inline
387 void
sMTxV(double Vr[3],double s1,double M1[3][3],double V1[3])388 sMTxV(double Vr[3], double s1, double M1[3][3], double V1[3])
389 {
390   Vr[0] = s1*(M1[0][0] * V1[0] +
391 	      M1[1][0] * V1[1] +
392 	      M1[2][0] * V1[2]);
393   Vr[1] = s1*(M1[0][1] * V1[0] +
394 	      M1[1][1] * V1[1] +
395 	      M1[2][1] * V1[2]);
396   Vr[2] = s1*(M1[0][2] * V1[0] +
397 	      M1[1][2] * V1[1] +
398 	      M1[2][2] * V1[2]);
399 }
400 
401 
402 inline
403 void
VmV(double Vr[3],const double V1[3],const double V2[3])404 VmV(double Vr[3], const double V1[3], const double V2[3])
405 {
406   Vr[0] = V1[0] - V2[0];
407   Vr[1] = V1[1] - V2[1];
408   Vr[2] = V1[2] - V2[2];
409 }
410 
411 inline
412 void
VpV(double Vr[3],double V1[3],double V2[3])413 VpV(double Vr[3], double V1[3], double V2[3])
414 {
415   Vr[0] = V1[0] + V2[0];
416   Vr[1] = V1[1] + V2[1];
417   Vr[2] = V1[2] + V2[2];
418 }
419 
420 inline
421 void
VpVxS(double Vr[3],double V1[3],double V2[3],double s)422 VpVxS(double Vr[3], double V1[3], double V2[3], double s)
423 {
424   Vr[0] = V1[0] + V2[0] * s;
425   Vr[1] = V1[1] + V2[1] * s;
426   Vr[2] = V1[2] + V2[2] * s;
427 }
428 
429 inline
430 void
MskewV(double M[3][3],const double v[3])431 MskewV(double M[3][3], const double v[3])
432 {
433   M[0][0] = M[1][1] = M[2][2] = 0.0;
434   M[1][0] = v[2];
435   M[0][1] = -v[2];
436   M[0][2] = v[1];
437   M[2][0] = -v[1];
438   M[1][2] = -v[0];
439   M[2][1] = v[0];
440 }
441 
442 
443 inline
444 void
VcrossV(double Vr[3],const double V1[3],const double V2[3])445 VcrossV(double Vr[3], const double V1[3], const double V2[3])
446 {
447   Vr[0] = V1[1]*V2[2] - V1[2]*V2[1];
448   Vr[1] = V1[2]*V2[0] - V1[0]*V2[2];
449   Vr[2] = V1[0]*V2[1] - V1[1]*V2[0];
450 }
451 
452 
453 inline
454 double
Vlength(double V[3])455 Vlength(double V[3])
456 {
457   return sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
458 }
459 
460 inline
461 void
Vnormalize(double V[3])462 Vnormalize(double V[3])
463 {
464   double d = 1.0 / sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
465   V[0] *= d;
466   V[1] *= d;
467   V[2] *= d;
468 }
469 
470 
471 inline
472 double
VdotV(double V1[3],double V2[3])473 VdotV(double V1[3], double V2[3])
474 {
475   return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]);
476 }
477 
478 inline
479 void
VxS(double Vr[3],double V[3],double s)480 VxS(double Vr[3], double V[3], double s)
481 {
482   Vr[0] = V[0] * s;
483   Vr[1] = V[1] * s;
484   Vr[2] = V[2] * s;
485 }
486 
487 inline
488 void
Mqinverse(double Mr[3][3],double m[3][3])489 Mqinverse(double Mr[3][3], double m[3][3])
490 {
491   int i,j;
492 
493   for(i=0; i<3; i++)
494     for(j=0; j<3; j++)
495       {
496 	int i1 = (i+1)%3;
497 	int i2 = (i+2)%3;
498 	int j1 = (j+1)%3;
499 	int j2 = (j+2)%3;
500 	Mr[i][j] = (m[j1][i1]*m[j2][i2] - m[j1][i2]*m[j2][i1]);
501       }
502 }
503 
504 
505 #define rfabs(x) ((x < 0) ? -x : x)
506 
507 #define ROT(a,i,j,k,l) g=a[i][j]; h=a[k][l]; a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau);
508 
509 int
510 inline
Meigen(double vout[3][3],double dout[3],double a[3][3])511 Meigen(double vout[3][3], double dout[3], double a[3][3])
512 {
513   int i;
514   double tresh,theta,tau,t,sm,s,h,g,c;
515   int nrot;
516   double b[3];
517   double z[3];
518   double v[3][3];
519   double d[3];
520 
521   v[0][0] = v[1][1] = v[2][2] = 1.0;
522   v[0][1] = v[1][2] = v[2][0] = 0.0;
523   v[0][2] = v[1][0] = v[2][1] = 0.0;
524 
525   b[0] = a[0][0]; d[0] = a[0][0]; z[0] = 0.0;
526   b[1] = a[1][1]; d[1] = a[1][1]; z[1] = 0.0;
527   b[2] = a[2][2]; d[2] = a[2][2]; z[2] = 0.0;
528 
529   nrot = 0;
530 
531   for(i=0; i<50; i++)
532     {
533 
534       sm=0.0; sm+=fabs(a[0][1]); sm+=fabs(a[0][2]); sm+=fabs(a[1][2]);
535       if (sm == 0.0) { McM(vout,v); VcV(dout,d); return i; }
536 
537       if (i < 3) tresh=0.2*sm/(3*3); else tresh=0.0;
538 
539       {
540 	g = 100.0*rfabs(a[0][1]);
541 	if (i>3 && rfabs(d[0])+g==rfabs(d[0]) && rfabs(d[1])+g==rfabs(d[1]))
542 	  a[0][1]=0.0;
543 	else if (rfabs(a[0][1])>tresh)
544 	  {
545 	    h = d[1]-d[0];
546 	    if (rfabs(h)+g == rfabs(h)) t=(a[0][1])/h;
547 	    else
548 	      {
549 		theta=0.5*h/(a[0][1]);
550 		t=1.0/(rfabs(theta)+sqrt(1.0+theta*theta));
551 		if (theta < 0.0) t = -t;
552 	      }
553 	    c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[0][1];
554 	    z[0] -= h; z[1] += h; d[0] -= h; d[1] += h;
555 	    a[0][1]=0.0;
556 	    ROT(a,0,2,1,2); ROT(v,0,0,0,1); ROT(v,1,0,1,1); ROT(v,2,0,2,1);
557 	    nrot++;
558 	  }
559       }
560 
561       {
562 	g = 100.0*rfabs(a[0][2]);
563 	if (i>3 && rfabs(d[0])+g==rfabs(d[0]) && rfabs(d[2])+g==rfabs(d[2]))
564 	  a[0][2]=0.0;
565 	else if (rfabs(a[0][2])>tresh)
566 	  {
567 	    h = d[2]-d[0];
568 	    if (rfabs(h)+g == rfabs(h)) t=(a[0][2])/h;
569 	    else
570 	      {
571 		theta=0.5*h/(a[0][2]);
572 		t=1.0/(rfabs(theta)+sqrt(1.0+theta*theta));
573 		if (theta < 0.0) t = -t;
574 	      }
575 	    c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[0][2];
576 	    z[0] -= h; z[2] += h; d[0] -= h; d[2] += h;
577 	    a[0][2]=0.0;
578 	    ROT(a,0,1,1,2); ROT(v,0,0,0,2); ROT(v,1,0,1,2); ROT(v,2,0,2,2);
579 	    nrot++;
580 	  }
581       }
582 
583 
584       {
585 	g = 100.0*rfabs(a[1][2]);
586 	if (i>3 && rfabs(d[1])+g==rfabs(d[1]) && rfabs(d[2])+g==rfabs(d[2]))
587 	  a[1][2]=0.0;
588 	else if (rfabs(a[1][2])>tresh)
589 	  {
590 	    h = d[2]-d[1];
591 	    if (rfabs(h)+g == rfabs(h)) t=(a[1][2])/h;
592 	    else
593 	      {
594 		theta=0.5*h/(a[1][2]);
595 		t=1.0/(rfabs(theta)+sqrt(1.0+theta*theta));
596 		if (theta < 0.0) t = -t;
597 	      }
598 	    c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[1][2];
599 	    z[1] -= h; z[2] += h; d[1] -= h; d[2] += h;
600 	    a[1][2]=0.0;
601 	    ROT(a,0,1,0,2); ROT(v,0,1,0,2); ROT(v,1,1,1,2); ROT(v,2,1,2,2);
602 	    nrot++;
603 	  }
604       }
605 
606       b[0] += z[0]; d[0] = b[0]; z[0] = 0.0;
607       b[1] += z[1]; d[1] = b[1]; z[1] = 0.0;
608       b[2] += z[2]; d[2] = b[2]; z[2] = 0.0;
609 
610     }
611 
612   fprintf(stderr, "eigen: too many iterations in Jacobi transform (%d).\n", i);
613 
614   return i;
615 }
616 
617 
618 #endif
619