1+ // ----------------------------------------------------------------------------
2+ // Numerical diagonalization of 3x3 matrcies
3+ // Copyright (C) 2006 Joachim Kopp
4+ // ----------------------------------------------------------------------------
5+ // This library is free software; you can redistribute it and/or
6+ // modify it under the terms of the GNU Lesser General Public
7+ // License as published by the Free Software Foundation; either
8+ // version 2.1 of the License, or (at your option) any later version.
9+ //
10+ // This library is distributed in the hope that it will be useful,
11+ // but WITHOUT ANY WARRANTY; without even the implied warranty of
12+ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13+ // Lesser General Public License for more details.
14+ //
15+ // You should have received a copy of the GNU Lesser General Public
16+ // License along with this library; if not, write to the Free Software
17+ // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18+ //
19+ // Edited for better c++ compatibility: Andriy Kharchenko
20+ //
21+ // ----------------------------------------------------------------------------
22+ #include < stdio.h>
23+ #include < math.h>
24+ #include " dsyevj3.h"
25+
26+ // Macros
27+ #define SQR (x ) ((x)*(x)) // x^2
28+
29+ namespace Nyxus
30+ {
31+
32+ int dsyevj3 (
33+ // IN
34+ const double A_[3 ][3 ],
35+ // OUT
36+ double Q[3 ][3 ], double w[3 ])
37+ // ----------------------------------------------------------------------------
38+ // Calculates the eigenvalues and normalized eigenvectors of a symmetric 3x3
39+ // matrix A using the Jacobi algorithm.
40+ // The upper triangular part of A is destroyed during the calculation,
41+ // the diagonal elements are read but not destroyed, and the lower
42+ // triangular elements are not referenced at all.
43+ // ----------------------------------------------------------------------------
44+ // Parameters:
45+ // A: The symmetric input matrix
46+ // Q: Storage buffer for eigenvectors
47+ // w: Storage buffer for eigenvalues
48+ // ----------------------------------------------------------------------------
49+ // Return value:
50+ // 0: Success
51+ // -1: Error (no convergence)
52+ // ----------------------------------------------------------------------------
53+ {
54+ double A[3 ][3 ] = { {A_[0 ][0 ], A_[0 ][1 ], A_[0 ][2 ]}, {A_[1 ][0 ], A_[1 ][1 ], A_[1 ][2 ]}, {A_[2 ][0 ], A_[2 ][1 ], A_[2 ][2 ]} };
55+ const int n = 3 ;
56+ double sd, so; // Sums of diagonal resp. off-diagonal elements
57+ double s, c, t; // sin(phi), cos(phi), tan(phi) and temporary storage
58+ double g, h, z, theta; // More temporary storage
59+ double thresh;
60+
61+ // Initialize Q to the identitity matrix
62+ #ifndef EVALS_ONLY
63+ for (int i = 0 ; i < n; i++)
64+ {
65+ Q[i][i] = 1.0 ;
66+ for (int j = 0 ; j < i; j++)
67+ Q[i][j] = Q[j][i] = 0.0 ;
68+ }
69+ #endif
70+
71+ // Initialize w to diag(A)
72+ for (int i = 0 ; i < n; i++)
73+ w[i] = A[i][i];
74+
75+ // Calculate SQR(tr(A))
76+ sd = 0.0 ;
77+ for (int i = 0 ; i < n; i++)
78+ sd += fabs (w[i]);
79+ sd = SQR (sd);
80+
81+ // Main iteration loop
82+ for (int nIter = 0 ; nIter < 50 ; nIter++)
83+ {
84+ // Test for convergence
85+ so = 0.0 ;
86+ for (int p = 0 ; p < n; p++)
87+ for (int q = p + 1 ; q < n; q++)
88+ so += fabs (A[p][q]);
89+ if (so == 0.0 )
90+ return 0 ;
91+
92+ if (nIter < 4 )
93+ thresh = 0.2 * so / SQR (n);
94+ else
95+ thresh = 0.0 ;
96+
97+ // Do sweep
98+ for (int p = 0 ; p < n; p++)
99+ for (int q = p + 1 ; q < n; q++)
100+ {
101+ g = 100.0 * fabs (A[p][q]);
102+ if (nIter > 4 && fabs (w[p]) + g == fabs (w[p])
103+ && fabs (w[q]) + g == fabs (w[q]))
104+ {
105+ A[p][q] = 0.0 ;
106+ }
107+ else if (fabs (A[p][q]) > thresh)
108+ {
109+ // Calculate Jacobi transformation
110+ h = w[q] - w[p];
111+ if (fabs (h) + g == fabs (h))
112+ {
113+ t = A[p][q] / h;
114+ }
115+ else
116+ {
117+ theta = 0.5 * h / A[p][q];
118+ if (theta < 0.0 )
119+ t = -1.0 / (sqrt (1.0 + SQR (theta)) - theta);
120+ else
121+ t = 1.0 / (sqrt (1.0 + SQR (theta)) + theta);
122+ }
123+ c = 1.0 / sqrt (1.0 + SQR (t));
124+ s = t * c;
125+ z = t * A[p][q];
126+
127+ // Apply Jacobi transformation
128+ A[p][q] = 0.0 ;
129+ w[p] -= z;
130+ w[q] += z;
131+ for (int r = 0 ; r < p; r++)
132+ {
133+ t = A[r][p];
134+ A[r][p] = c * t - s * A[r][q];
135+ A[r][q] = s * t + c * A[r][q];
136+ }
137+ for (int r = p + 1 ; r < q; r++)
138+ {
139+ t = A[p][r];
140+ A[p][r] = c * t - s * A[r][q];
141+ A[r][q] = s * t + c * A[r][q];
142+ }
143+ for (int r = q + 1 ; r < n; r++)
144+ {
145+ t = A[p][r];
146+ A[p][r] = c * t - s * A[q][r];
147+ A[q][r] = s * t + c * A[q][r];
148+ }
149+
150+ // Update eigenvectors
151+ #ifndef EVALS_ONLY
152+ for (int r = 0 ; r < n; r++)
153+ {
154+ t = Q[r][p];
155+ Q[r][p] = c * t - s * Q[r][q];
156+ Q[r][q] = s * t + c * Q[r][q];
157+ }
158+ #endif
159+ }
160+ }
161+ }
162+
163+ return -1 ;
164+ }
165+
166+ }
0 commit comments