tree changes
This commit is contained in:
242
libs/main/math/pimathsolver.cpp
Normal file
242
libs/main/math/pimathsolver.cpp
Normal file
@@ -0,0 +1,242 @@
|
||||
/*
|
||||
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();
|
||||
}
|
||||
Reference in New Issue
Block a user