Реализация метода Ньютона для вычисления вектора смещения

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;*/
 }