/
AMSCommon.cpp
executable file
·155 lines (145 loc) · 4.29 KB
/
AMSCommon.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
#include <math.h>
#include <complex>
#include "AMSCommon.h"
#include "Libmat.h"
/////////////////////////////////////////////////////////////
void DecomLU_PV_C(int n,CValue *a, int *ip, int &emptyLineNumber)
// ----- LU-decomposition of non-sparse matrix.
// n - dimension of system
// a - square matrix stored by rows (i.e 1st row, 2nd row and so on)
// ip - array of row exchanges
// emptyLineNumber - < 0 for case OK,
// otherwise - number of "bad" line in the system
// Warnings:
// 1. indices of matrix and arrays are zero-based,
// i.e. each index varies in range [0...n-1].
// 2. memory for "a" and "ip" must be allocated beyond this module.
{
CValue zero(0.,0.);
emptyLineNumber = -1;
if (n > 0) ip[n-1] = 1;
if (n > 1)
{ for (int k = 0; k < n - 1; k++) // k - the column number
{ int kp1 = k + 1;
int m = k;
// Find a maximum value in the column
for (int i = kp1; i < n; i ++)
if (CMod(a[i*n+k]) > CMod(a[m*n+k])) m = i;
ip[k] = m;
CValue t = a[m*n+k];
if (m != k) a[m*n+k] = a[k*n+k];
if (t == zero)
{ emptyLineNumber = k;
return;
}
// Put the maximum value in diagonal
a[k*n+k] = t;
t = CValue(1.,0.) / t;
for (int i1 = kp1; i1 < n; i1++)
a[i1*n+k] *= t;
// Exchanging rows
for (int j = kp1; j < n; j++)
{ t = a[m*n+j];
a[m*n+j] = a[k*n+j];
a[k*n+j] = t;
if (t == zero) continue;
for(int i = kp1; i < n; i++)
a[i*n+j] -= (a[i*n+k]*t);
}
} // end of for (k = ...
}
if (a[(n-1)*n+n-1] == zero) emptyLineNumber = (n - 1);
}
void SolveLU_PV_C(int n,CValue *a,CValue *b, int *ip)
// ----- Solving the LU-decomposed non-sparse equation system with complex coeefficients
// n - dimension of system
// a - square matrix stored by rows (i.e 1st row, 2nd row and so on)
// b - at input: vector of unknowns,
// at output: solution of system
// ip - array of row exchanges (already formed in 'decom')
// Warnings:
// 1. indices of matrix and arrays are zero-based,
// i.e. each index varies in range [0...n-1].
// 2. memory for "a", "b" and "ip" must be already
// allocated beyond this module.
{
if (n > 1)
{ for (int k = 0; k < n - 1; k++)
{ int m = ip[k];
CValue t = b[m];
b[m] = b[k];
b[k] = t;
for (int i = k + 1; i < n; i++)
b[i] -= (a[i*n+k] * t);
} // for (k = ...
for (int kb = 0; kb < n - 1; kb++)
{ int km1 = n - kb - 2;
int k = km1 + 1;
b[k] = b[k] / a[k*n+k];
CValue t = -b[k];
for (int i = 0; i < k; i++)
b[i] += (a[i*n+k]*t);
}
}
if (n >= 1) b[0] /= a[0*n+0];
}
void CMatrixMult(CValue *A,CValue *B,CValue *C,int Na,int Nb,int Nab)
{
// C[Na*Nb] = A[Na*Nab] * B[Nab*Nb]
for(int i=0; i < Na; i++)
{
for(int j=0; j < Nb; j++)
{ CValue a(0.,0.);
for(int k=0; k < Nab; k++)
a += A[k + i*Nab]*B[j + k*Nb];
C[j + i*Nb] += a;
}
}
}
void CMatrixDump(const CString name,CValue *A,int Nr,int Nc)
{
printf("\tMATRIX %s[%d*%d]\n",name,Nr,Nc);
for(int i=0; i < Nr; i++)
{ printf("%d.",i);
for(int j=0; j < Nc; j++)
{ int index = j + i*Nc;
printf("\t%d.(%g + j*%g)",index,A[index].real(),A[index].imag());
}
printf("\n");
}
}
double CMod(CValue a)
{
double x = a.real();
double y = a.imag();
return(sqrt(x*x + y*y));
}
double CArg(CValue a_c)
{
double pi_2 = 1.570796326795;
double b = 180/(2*pi_2);
double x = a_c.real();
double y = a_c.imag();
double a;
double s_x = (x != 0.0) ? x/fabs(x) : 1.0;
double s_y = (y != 0.0) ? y/fabs(y) : 1.0;
if(x != 0.0)
{ a = atan(fabs(y)/fabs(x));
if(s_x < 0 && s_y > 0)
a = 2*pi_2 - a;
if(s_x < 0 && s_y < 0)
a = a - 2*pi_2;
if(s_x > 0 && s_y < 0)
a = -a;
return(a*b);
}
else
{ if(y == 0)
return(0.);
if(s_y > 0)
a = pi_2;
else
a = -pi_2;
return(a*b);
}
}