261 lines
6.2 KiB
C++
261 lines
6.2 KiB
C++
/*
|
|
PIP - Platform Independent Primitives
|
|
PIMathSolver
|
|
Ivan Pelipenko peri4ko@yandex.ru
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU Lesser 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 Lesser General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Lesser 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];
|
|
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)
|
|
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) {
|
|
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();
|
|
}
|