diff --git a/main.cpp b/main.cpp index 9c897fc..39448b2 100644 --- a/main.cpp +++ b/main.cpp @@ -46,12 +46,14 @@ double E1(double t) { // Якобиан MatrixXd Y = MatrixXd::Zero(MATRIX_DIM, MATRIX_DIM); - // Вектор невязок VectorXd N = VectorXd::Zero(MATRIX_DIM); - // Вектор поправок +VectorXd dX = VectorXd::Zero(MATRIX_DIM); +// Вектор фазовых переменных VectorXd X = VectorXd::Zero(MATRIX_DIM); +// Копия вектора фазовых переменных (для обработки случая неудачного шага) +VectorXd X_backup = VectorXd::Zero(MATRIX_DIM); void Jacobian(double dt) { Y(0, 0) = 1, Y(0, 3) = -1. / dt; @@ -95,10 +97,61 @@ void nevyzka(double dt, double cur_time) { N(11) = E1(cur_time) - X(6); N(12) = E2 - X(9); + + // Перед вектором невязок стоит минус + N = -1 * N; } int main() { - Jacobian(0.1); + double dt = T_Start; // Шаг по времени + double dt_prev = dt; // Предыдущий шаг по времени + double eps_max = ACR; // Максимальное отклонение + double eps_min = 1e-10; // Минимальное отклонение + int counter = 0; // Счетчик шагов по времени + double cur_time = 0; // Текущее время + double eps_cur; + double eps; + double step_c = 0; + + while (dt <= TK) { + // Счётчик итераций + int iteration = 0; + bool flag = true; + while (flag) { + Jacobian(dt); + nevyzka(dt, cur_time); + + #ifdef _DEBUG + std::cout << Y << std::endl << N; + #endif + + dX = Y.fullPivLu().solve(N); + #ifdef _DEBUG + std::cout << '\n' << dX; + #endif + + X += dX; + iteration++; + double maximum = X.maxCoeff(); + if (maximum <= ACR) { + flag = false; + } + else if (iteration == 7 && flag) { + iteration = 0; + dt /= 2.; + if (dt < SMN) { + std::cerr << "Sorry, but i can not solve this" << std::endl; + exit(-1); + } + X = X_backup; + } + } + // TODO: реализовать вычисление уточнённое решения, сравнить его с вычисленным, + // возможно изменить шаг моделирования + + } + // Для примера + /*Jacobian(0.1); nevyzka(0.1, 0); - std::cout << Y << std::endl << N; + std::cout << Y << std::endl << N;*/ } \ No newline at end of file