/****************************************************************************** * Numerical Recipes' Jacobi code to diagonalize a matrix. * * "Numerical Recipes in C", by William H. Press el al., University of * * Cambridge, 1988, ISBN 0521-35465-X. * ******************************************************************************/ #include #include #include "irit_sm.h" #include "misc_lib.h" #include "extra_fn.h" void diag(RealType M[4][4], RealType U[4][4], RealType D[4][4], RealType V[4][4]); static void jacobi(RealType *a[], int n, RealType *d, RealType *v[], int* nrot); #define ROTATE(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); #define VECTOR(n) IritMalloc(sizeof(RealType) * (n + 1)) #define FREE_VECTOR(p) IritFree((p)) /***************************************************************************** * DESCRIPTION: M * Performs a diagonalization of a NxN matrix, as U D V, D diagonal and U and M * V orthogonal matrices U = V^-1 = V^T. M * * * PARAMETERS: M * M: Matrix to diagonalize. M * U: The left orthogonal (=rotation) matrix. M * D: The diagonal matrix. M * V: The eight orthogonal matrix. M * n: Dimension of the matrices as n times n. M * * * RETURN VALUE: M * void M * * * SEE ALSO: M * JacobiMatrixDiag4x4 M * * * KEYWORDS: M * JacobiMatrixDiagNxN M *****************************************************************************/ void JacobiMatrixDiagNxN(RealType *M[], RealType *U[], RealType *D[], RealType *V[], int n) { int i, j, nrot; RealType **MM = VECTOR(n), *DD = VECTOR(n), **VV = VECTOR(n); /* Allocate some local memory. */ for (i = 0; i <= n; i++) { MM[i] = VECTOR(n); VV[i] = VECTOR(n); } for (i = 0; i < n; i++) for (j = 0; j < n; j++) MM[j + 1][j + 1] = M[i][j]; /* Go to the real work. */ jacobi(MM, 4, DD, VV, &nrot); ZAP_MEM(D, n * n * sizeof(RealType)); for (i = 0; i < n; i ++) { D[i][i] = DD[i + 1]; for (j = 0; j < n; j ++) U[i][j] = V[j][i] = VV[i + 1][j + 1]; } /* Free local memory. */ for (i = 0; i <= n; i++) { FREE_VECTOR(MM[i]); FREE_VECTOR(VV[i]); } FREE_VECTOR(MM); FREE_VECTOR(VV); } /***************************************************************************** * DESCRIPTION: M * performs a diagonalization of a 4x4 matrix, as U D V, D diagonal and U and M * V orthogonal matrices U = V^-1 = V^T. M * * * PARAMETERS: M * M: Matrix to diagonalize. M * U: The left orthogonal (=rotation) matrix. M * D: The diagonal matrix. M * V: The eight orthogonal matrix. M * * * RETURN VALUE: M * void M * * * SEE ALSO: M * JacobiMatrixDiagNxN M * * * KEYWORDS: M * JacobiMatrixDiag4x4 M *****************************************************************************/ void JacobiMatrixDiag4x4(RealType M[4][4], RealType U[4][4], RealType D[4][4], RealType V[4][4]) { int i, j, nrot; RealType aa[5][5], vv[5][5], d[5], *a[5], *v[5]; for (i = 0; i < 5; i++) { a[i] = aa[i]; v[i] = vv[i]; } for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) a[i + 1][j + 1] = M[i][j]; jacobi(a, 4, d, v, &nrot); ZAP_MEM(D, 16 * sizeof (RealType)); for (i = 0; i < 4; i ++) { D[i][i] = d[i + 1]; for (j = 0; j < 4; j ++) U[i][j] = V[j][i] = v[i + 1][j + 1]; } } /* From "Numerical Recipes"; Changed function prototype to ease interface. */ static void jacobi(RealType *a[], int n, RealType *d, RealType *v[], int* nrot) { int j, iq, ip, i; RealType tresh, theta, tau, t, sm, s, h, g, c, *b, *z; b = VECTOR(n); z = VECTOR(n); for (ip = 1; ip <= n; ip++) { for (iq = 1; iq <= n; iq++) v[ip][iq] = 0.0; v[ip][ip] = 1.0; } for (ip = 1; ip <= n; ip++) { b[ip] = d[ip] = a[ip][ip]; z[ip] = 0.0; } *nrot = 0; for (i = 1; i <= 50; i++) { sm = 0.0; for (ip = 1; ip <= n - 1; ip++) { for (iq = ip + 1; iq <= n; iq++) sm += FABS(a[ip][iq]); } if (sm == 0.0) { FREE_VECTOR(z); FREE_VECTOR(b); return; } if (i < 4) tresh = 0.2 * sm / (n*n); else tresh = 0.0; for (ip = 1; ip <= n-1; ip++) { for (iq = ip+1; iq <= n; iq++) { g = 100.0 * FABS(a[ip][iq]); if (i > 4 && FABS(d[ip]) + g == FABS(d[ip]) && FABS(d[iq]) + g == FABS(d[iq])) a[ip][iq] = 0.0; else if (FABS(a[ip][iq]) > tresh) { h = d[iq] - d[ip]; if (FABS(h) + g == FABS(h)) t = (a[ip][iq]) / h; else { theta = 0.5 * h / a[ip][iq]; t = 1.0 / (FABS(theta) + sqrt(1.0 + theta * theta)); if (theta < 0.0) t = -t; } c = 1.0 / sqrt(1 + t * t); s = t * c; tau = s / (1.0 + c); h = t * a[ip][iq]; z[ip] -= h; z[iq] += h; d[ip] -= h; d[iq] += h; a[ip][iq] = 0.0; for (j = 1; j <= ip-1; j++) { ROTATE(a, j, ip, j, iq) } for (j = ip+1; j <= iq-1; j++) { ROTATE(a, ip, j, j, iq) } for (j = iq+1; j <= n; j++) { ROTATE(a, ip, j, iq, j) } for (j = 1; j <= n; j++) { ROTATE(v, j, ip, j, iq) } ++(*nrot); } } } for (ip = 1; ip <= n; ip++) { b[ip] += z[ip]; d[ip] = b[ip]; z[ip] = 0.0; } } IRIT_FATAL_ERROR("Too many iterations in Jacobi routine"); } #ifdef DEBUG_DIAG_MATRIX void mul(RealType mat1[4][4], RealType mat2[4][4], RealType mat3[4][4]) { int i, j, k; ZAP_MEM(mat3, 16 * sizeof(RealType)); for (i = 0; i < 4; i ++) for (j = 0; j < 4; j ++) for (k = 0; k < 4; k ++) mat3 [i] [j] += mat1 [i] [k] * mat2 [k] [j]; } void main(void) { int i, j; RealType M[4][4], U[4][4], D[4][4], V[4][4], tmp1[4][4], tmp2[4][4]; printf ("Mat (0,0): "); scanf ("%lf", & M [0] [0]); printf ("Mat (0,1): "); scanf ("%lf", & M [0] [1]); M [1] [0] = M [0] [1]; printf ("Mat (0,2): "); scanf ("%lf", & M [0] [2]); M [2] [0] = M [0] [2]; printf ("Mat (0,3): "); scanf ("%lf", & M [0] [3]); M [3] [0] = M [0] [3]; printf ("Mat (1,1): "); scanf ("%lf", & M [1] [1]); printf ("Mat (1,2): "); scanf ("%lf", & M [1] [2]); M [2] [1] = M [1] [2]; printf ("Mat (1,3): "); scanf ("%lf", & M [1] [3]); M [3] [1] = M [1] [3]; printf ("Mat (2,2): "); scanf ("%lf", & M [2] [2]); printf ("Mat (2,3): "); scanf ("%lf", & M [2] [3]); M [3] [2] = M [2] [3]; printf ("Mat (3,3): "); scanf ("%lf", & M [3] [3]); JacobiMatrixDiag4x4(M, U, D, V); printf ("\nOriginal matrix M:\n 0 1 2\n"); for (i = 0; i < 4; i ++) { printf ("%3d ", i); for (j = 0; j < 4; j ++) printf (" %9.3f", M [i] [j]); printf ("\n"); } printf ("\nMatrix U:\n 0 1 2\n"); for (i = 0; i < 4; i ++) { printf ("%3d ", i); for (j = 0; j < 4; j ++) printf (" %9.3f", U [i] [j]); printf ("\n"); } printf ("\nMatrix D:\n 0 1 2\n"); for (i = 0; i < 4; i ++) { printf ("%3d ", i); for (j = 0; j < 4; j ++) printf (" %9.3f", D [i] [j]); printf ("\n"); } printf ("\nMatrix V:\n 0 1 2\n"); for (i = 0; i < 4; i ++) { printf ("%3d ", i); for (j = 0; j < 4; j ++) printf (" %9.3f", V [i] [j]); printf ("\n"); } mul(U, D, tmp1); mul(tmp1, V, tmp2); printf("\nMatrix UDV:\n 0 1 2\n"); for (i = 0; i < 4; i ++) { printf ("%3d ", i); for (j = 0; j < 4; j ++) printf (" %9.3f", tmp2 [i] [j]); printf ("\n"); } } #endif /* DEBUG_DIAG_MATRIX */