249 lines
6.6 KiB
C++
249 lines
6.6 KiB
C++
/*
|
||
PIP - Platform Independent Primitives
|
||
PIMathSolver
|
||
Copyright (C) 2017 Ivan Pelipenko peri4ko@yandex.ru
|
||
|
||
This program is free software: you can redistribute it and/or modify
|
||
it under the terms of the GNU General Public License as published by
|
||
the Free Software Foundation, either version 3 of the License, or
|
||
(at your option) any later version.
|
||
|
||
This program is distributed in the hope that it will be useful,
|
||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
GNU General Public License for more details.
|
||
|
||
You should have received a copy of the GNU General Public License
|
||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
*/
|
||
|
||
#include "pimathsolver.h"
|
||
|
||
const char PIMathSolver::methods_desc[] = "b{Methods:}\
|
||
\n -1 - Global settings\
|
||
\n 01 - Eyler 1\
|
||
\n 02 - Eyler 2\
|
||
\n 14 - Runge-Kutta 4\
|
||
\n 23 - Adams-Bashfort-Moulton 3\
|
||
\n 24 - Adams-Bashfort-Moulton 4\
|
||
\n 32 - Polynomial Approximation 2\
|
||
\n 33 - Polynomial Approximation 3\
|
||
\n 34 - Polynomial Approximation 4\
|
||
\n 35 - Polynomial Approximation 5";
|
||
|
||
PIMathSolver::Method PIMathSolver::method_global = PIMathSolver::Eyler_2;
|
||
|
||
|
||
void PIMathSolver::solve(double u, double h) {
|
||
switch (method) {
|
||
case Global:
|
||
switch (method_global) {
|
||
case Eyler_1: solveEyler1(u, h); break;
|
||
case Eyler_2: solveEyler2(u, h); break;
|
||
case RungeKutta_4: solveRK4(u, h); break;
|
||
case AdamsBashfortMoulton_2: solveABM2(u, h); break;
|
||
case AdamsBashfortMoulton_3: solveABM3(u, h); break;
|
||
case AdamsBashfortMoulton_4: default: solveABM4(u, h); break;
|
||
case PolynomialApproximation_2: solvePA2(u, h); break;
|
||
case PolynomialApproximation_3: solvePA3(u, h); break;
|
||
case PolynomialApproximation_4: solvePA4(u, h); break;
|
||
case PolynomialApproximation_5: solvePA5(u, h); break;
|
||
}
|
||
break;
|
||
case Eyler_1: solveEyler1(u, h); break;
|
||
case Eyler_2: solveEyler2(u, h); break;
|
||
case RungeKutta_4: solveRK4(u, h); break;
|
||
case AdamsBashfortMoulton_2: solveABM2(u, h); break;
|
||
case AdamsBashfortMoulton_3: solveABM3(u, h); break;
|
||
case AdamsBashfortMoulton_4: default: solveABM4(u, h); break;
|
||
case PolynomialApproximation_2: solvePA2(u, h); break;
|
||
case PolynomialApproximation_3: solvePA3(u, h); break;
|
||
case PolynomialApproximation_4: solvePA4(u, h); break;
|
||
case PolynomialApproximation_5: solvePA5(u, h); break;
|
||
}
|
||
step++;
|
||
}
|
||
|
||
|
||
void PIMathSolver::fromTF(const TransferFunction & TF) {
|
||
if (TF.vector_An.size() >= TF.vector_Bm.size())
|
||
size = TF.vector_An.size()-1;
|
||
else {
|
||
piCout << "PIMathSolver error: {A} should be greater than {B}";
|
||
return;
|
||
}
|
||
if (size == 0) return;
|
||
|
||
step = 0;
|
||
times.fill(0.);
|
||
A.resize(size, size);
|
||
d.resize(size + 1); d.fill(0.);
|
||
a1.resize(size + 1); a1.fill(0.);
|
||
b1.resize(size + 1); b1.fill(0.);
|
||
X.resize(size); X.fill(0.);
|
||
F.resize(5);
|
||
for (uint i = 0; i < 5; ++i)
|
||
F[i].resize(size), F[i].fill(0.);
|
||
k1.resize(size); k1.fill(0.);
|
||
k2.resize(size); k2.fill(0.);
|
||
k3.resize(size); k3.fill(0.);
|
||
k4.resize(size); k4.fill(0.);
|
||
xx.resize(size); xx.fill(0.);
|
||
XX.resize(size); XX.fill(0.);
|
||
for (uint i = 0; i < size + 1; ++i)
|
||
a1[size - i] = TF.vector_An[i];
|
||
for (uint i = 0; i < TF.vector_Bm.size(); ++i)
|
||
b1[size - i] = TF.vector_Bm[i];
|
||
double a0 = a1[0];
|
||
a1 /= a0;
|
||
b1 /= a0;
|
||
|
||
d[0] = b1[0]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> d
|
||
for (uint i = 1; i < size + 1; ++i) {
|
||
sum = 0.;
|
||
for (uint m = 0; m < i; ++m)
|
||
sum += a1[i - m] * d[m];
|
||
d[i] = b1[i] - sum;
|
||
}
|
||
|
||
for (uint i = 0; i < size - 1; ++i) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>
|
||
for (uint j = 0; j < size; ++j)
|
||
A[j][i] = (j == i + 1);
|
||
for (uint i = 0; i < size; ++i)
|
||
A[i][size - 1] = -a1[size - i];
|
||
for (uint i = 0; i < size; ++i)
|
||
d[i] = d[i + 1];
|
||
}
|
||
|
||
|
||
void PIMathSolver::solveEyler1(double u, double h) {
|
||
/*for (uint i = 0; i < size; ++i) {
|
||
* sum = 0.;
|
||
* for (uint j = 0; j < size; ++j)
|
||
* sum += A[j][i] * X[j];
|
||
* xx[i] = sum + d[i] * u;
|
||
}*/
|
||
F[0] = A * X + d * u;
|
||
X += F[0] * h;
|
||
moveF();
|
||
}
|
||
|
||
|
||
void PIMathSolver::solveEyler2(double u, double h) {
|
||
F[0] = A * X + d * u;
|
||
X += (F[0] + F[1]) * h / 2.;
|
||
moveF();
|
||
}
|
||
|
||
|
||
void PIMathSolver::solveRK4(double u, double h) {
|
||
td = X[0] - F[0][0];
|
||
k1 = A * X + d * u; xx = k1 * h / 2.; XX = X + xx;
|
||
k2 = A * (XX + k1 * h / 2.) + d * (u + td/3.); xx = k2 * h / 2.; XX += xx;
|
||
k3 = A * (XX + k2 * h / 2.) + d * (u + td*2./3.); xx = k3 * h; XX += xx;
|
||
k4 = A * (XX + k3 * h) + d * (u + td);
|
||
//cout << k1 << k2 << k3 << k4 << endl;
|
||
X += (k1 + k2 * 2. + k3 * 2. + k4) * h / 6.;
|
||
F[0] = X;
|
||
}
|
||
|
||
|
||
void PIMathSolver::solveABM2(double u, double h) {
|
||
F[0] = A * X + d * u;
|
||
XX = X + (F[0] * 3. - F[1]) * (h / 2.);
|
||
F[1] = A * XX + d * u;
|
||
X += (F[1] + F[0]) * (h / 2.);
|
||
moveF();
|
||
}
|
||
|
||
|
||
void PIMathSolver::solveABM3(double u, double h) {
|
||
F[0] = A * X + d * u;
|
||
XX = X + (F[0] * 23. - F[1] * 16. + F[2] * 5.) * (h / 12.);
|
||
F[2] = A * XX + d * u;
|
||
X += (F[2] * 5. + F[0] * 8. - F[1]) * (h / 12.);
|
||
moveF();
|
||
}
|
||
|
||
|
||
void PIMathSolver::solveABM4(double u, double h) {
|
||
F[0] = A * X + d * u;
|
||
XX = X + (F[0] * 55. - F[1] * 59. + F[2] * 37. - F[3] * 9.) * (h / 24.);
|
||
F[3] = A * XX + d * u;
|
||
X += (F[3] * 9. + F[0] * 19. - F[1] * 5. + F[2]) * (h / 24.);
|
||
moveF();
|
||
}
|
||
|
||
|
||
void PIMathSolver::solvePA(double u, double h, uint deg) {
|
||
F[0] = A * X + d * u;
|
||
M.resize(deg, deg);
|
||
Y.resize(deg);
|
||
pY.resize(deg);
|
||
|
||
for (uint k = 0; k < size; ++k) {
|
||
for (uint i = 0; i < deg; ++i) {
|
||
td = 1.;
|
||
ct = times[i];
|
||
for (uint j = 0; j < deg; ++j) {
|
||
M[j][i] = td;
|
||
td *= ct;
|
||
}
|
||
}
|
||
for (uint i = 0; i < deg; ++i)
|
||
Y[i] = F[i][k];
|
||
/// find polynom
|
||
//if (step == 1) cout << M << endl << Y << endl;
|
||
M.invert(&ok, &Y);
|
||
//if (step == 1) cout << Y << endl;
|
||
if (!ok) {
|
||
solveEyler2(u, h);
|
||
break;
|
||
}
|
||
/// calc last piece
|
||
x0 = 0.;
|
||
td = 1.;
|
||
t = times[0];
|
||
for (uint i = 0; i < deg; ++i) {
|
||
x0 += Y[i] * td / (i + 1.);
|
||
td *= t;
|
||
}
|
||
x0 *= t;
|
||
|
||
x1 = 0.;
|
||
td = 1.;
|
||
t = times[1];
|
||
for (uint i = 0; i < deg; ++i) {
|
||
x1 += Y[i] * td / (i + 1.);
|
||
td *= t;
|
||
}
|
||
x1 *= t;
|
||
lp = x0 - x1;
|
||
|
||
if (deg > 2) {
|
||
/// calc prev piece
|
||
x0 = 0.;
|
||
td = 1.;
|
||
dh = times[1] - times[2];
|
||
if (dh != 0. && step > 1) {
|
||
t = times[2];
|
||
for (uint i = 0; i < deg; ++i) {
|
||
x0 += Y[i] * td / (i + 1.);
|
||
td *= t;
|
||
}
|
||
x0 *= t;
|
||
ct = x1 - x0;
|
||
/// calc correction
|
||
ct -= pY[k];
|
||
}
|
||
/// calc final
|
||
X[k] += lp - ct;
|
||
pY[k] = lp;
|
||
} else {
|
||
X[k] += lp;
|
||
pY[k] = lp;
|
||
}
|
||
}
|
||
moveF();
|
||
}
|