1 /*
2 * MLIAP_SO3_math.h
3 *
4 * Created on: Apr 12, 2021
5 * Author: macstein
6 */
7
8 #ifndef LMP_MLIAP_SO3_MATH_H
9 #define LMP_MLIAP_SO3_MATH_H
10
11 #include "math_eigen_impl.h"
12
13 namespace SO3Math {
14
15 int jacobin(int n, double const *const *mat, double *eval, double **evec);
16 int invert_matrix(int n, double *A, double *Ainv);
17 int LUPdecompose(int n, double dtol, double *A, int *P);
18 void LUPSolve(int n, double *A, double *B, int *P);
19
20 } // namespace SO3Math
21
22 using namespace MathEigen;
23
24 typedef Jacobi<double, double *, double **, double const *const *> Jacobi_v2;
jacobin(int n,double const * const * mat,double * eval,double ** evec)25 int SO3Math::jacobin(int n, double const *const *mat, double *eval, double **evec)
26 {
27 int *midx = new int[n];
28 double **M = new double *[n];
29 double **mat_cpy = new double *[n];
30
31 for (int i = 0; i < n; i++) {
32 mat_cpy[i] = new double[n];
33 for (int j = 0; j < n; j++) mat_cpy[i][j] = mat[i][j];
34 M[i] = &(mat_cpy[i][0]);
35 }
36
37 Jacobi_v2 ecalcn(n, M, midx);
38 int ierror = ecalcn.Diagonalize(mat, eval, evec, Jacobi_v2::SORT_DECREASING_EVALS);
39
40 for (int i = 0; i < n; i++) {
41 for (int j = i + 1; j < n; j++) std::swap(evec[i][j], evec[j][i]);
42 delete[] mat_cpy[i];
43 }
44 delete[] mat_cpy;
45 delete[] M;
46 delete[] midx;
47
48 return ierror;
49 }
50
invert_matrix(int n,double * A,double * Ainv)51 int SO3Math::invert_matrix(int n, double *A, double *Ainv)
52 {
53
54 int i, j;
55 double dtol = 1.e-30;
56
57 int *P;
58 double *b, *Atemp;
59
60 P = new int[n];
61 b = new double[n];
62 Atemp = new double[n * n];
63
64 for (i = 0; i < n * n; i++) Atemp[i] = A[i];
65
66 int rv = 0;
67 if (LUPdecompose(n, dtol, Atemp, P) == 0) {
68
69 for (i = 0; i < n; i++) {
70 for (j = 0; j < n; j++) b[j] = 0.0;
71
72 b[i] = 1.0;
73 LUPSolve(n, Atemp, b, P);
74
75 for (j = 0; j < n; j++) Ainv[j * n + i] = b[j];
76 }
77 } else {
78 rv = 1;
79 }
80
81 delete[] P;
82 delete[] b;
83 delete[] Atemp;
84
85 return rv;
86 }
87
LUPdecompose(int n,double dtol,double * A,int * P)88 int SO3Math::LUPdecompose(int n, double dtol, double *A, int *P)
89 {
90 int i, j, k, maxi;
91 double maxA, Atemp;
92 double *normi;
93
94 maxi = 0;
95 normi = new double[n];
96
97 for (i = 0; i < n; i++) {
98 maxA = 0.0;
99 for (j = 0; j < n; j++) {
100 Atemp = fabs(A[i * n + j]);
101 if (Atemp > maxA) maxA = Atemp;
102 }
103 if (maxA < dtol) {
104 delete[] normi;
105 return 1;
106 }
107
108 normi[i] = 1.0 / maxA;
109 }
110
111 for (j = 0; j < n; j++) {
112 for (i = 0; i < j; i++)
113 for (k = 0; k < i; k++) A[i * n + j] -= A[i * n + k] * A[k * n + j];
114
115 maxA = 0.0;
116 for (i = j; i < n; i++) {
117 for (k = 0; k < j; k++) A[i * n + j] -= A[i * n + k] * A[k * n + j];
118
119 Atemp = fabs(A[i * n + j]) * normi[i];
120 if (Atemp >= maxA) {
121 maxA = Atemp;
122 maxi = i;
123 }
124 }
125 if (maxi != j) {
126 if ((j == (n - 2)) && (A[j * n + j + 1] == 0.0))
127 maxi = j;
128 else {
129 for (k = 0; k < n; k++) {
130 Atemp = A[j * n + k];
131 A[j * n + k] = A[maxi * n + k];
132 A[maxi * n + k] = Atemp;
133 }
134 normi[maxi] = normi[j];
135 }
136 }
137
138 P[j] = maxi;
139 if (A[j * n + j] == 0.0) A[j * n + j] = dtol;
140
141 if (j != (n - 1)) {
142 Atemp = 1.0 / A[j * n + j];
143 for (i = (j + 1); i < n; i++) A[i * n + j] *= Atemp;
144 }
145 }
146
147 delete[] normi;
148 return 0;
149 }
150
LUPSolve(int n,double * A,double * B,int * P)151 void SO3Math::LUPSolve(int n, double *A, double *B, int *P)
152 {
153 int i, j;
154 double dtemp;
155
156 for (i = 0; i < n; i++) {
157
158 dtemp = B[P[i]];
159 B[P[i]] = B[i];
160 for (j = (i - 1); j >= 0; j--) dtemp -= A[i * n + j] * B[j];
161
162 B[i] = dtemp;
163 }
164
165 for (i = (n - 1); i >= 0; i--) {
166 for (j = (i + 1); j < n; j++) B[i] -= A[i * n + j] * B[j];
167
168 B[i] /= A[i * n + i];
169 }
170 }
171
172 #endif /* LMP_MLIAP_SO3_MATH_H_ */
173