PS_Method_CCS_test/Pseudo_Spectral_Method/Process/Jacb_struct_Re.cpp

/* 
函数功能：
		雅克比矩阵依赖关系重构，根据上一代的雅可比矩阵依赖关系，生成下一代。
输入：
		Jacb_struct_info:雅克比基础结构信息[结构体]
		MeshGrid_nextstep:下一步的网格信息[矩阵]
		length_state：状态量长度
		length_control：控制量长度
		length_path:路径约束维度
		length_event:事件约束维度
		length_integral：积分量维度
输出：
		Jacb_struct1:新网格下的雅克比矩阵依赖关系矩阵[稀疏矩阵]
时间：  matlab版  李兆亭，2020/07/13
时间：  C++版     唐湘佶，2020/10/21

*/

#include <iostream>
#include <armadillo>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include "GPM.h"

int Jacb_struct_Re(Jacb_struct_info0 Jacb_struct_info, mat MeshGrid_nextstep, int length_state, int length_control, 
					int length_path, int length_event, int length_integral, int length_parameter, sp_mat* Jacb_struct1) {
	
	if (MeshGrid_nextstep.is_empty()) {
		(*Jacb_struct1).reset();		// 若为空，表明问题已解决，无需再次赋值
		return 1;
	}

	vec Nk, N_col;
	int Col_total, z_len, interval_num;
	Nk = MeshGrid_nextstep.col(0);			// 每个间隔内的阶数[列向量]
	N_col = Nk + 1;
	Col_total = round(sum(N_col));			// 总配点数
	z_len = (Col_total + 1) * length_state + Col_total * length_control + 2;    // 自变量数目
	interval_num = Nk.n_rows;				// 间隔数目
	
////////////////////////////////////////1、缺陷矩阵对于自变量Z的相关性////////////////////////////////////
	int dD_dS_L = 1;						// 默认为1
	mat dD_dS_R = Jacb_struct_info.dD_dS_R;
	mat dD_dU = Jacb_struct_info.dD_dU;
	urowvec dD_dt = Jacb_struct_info.dD_dt;

	// 1.1缺陷-Z雅可比矩阵初始化
	int rs, cs, re, ce;
	int row_num = 0;
	int column_num = 0;
	int Defect_length = Col_total * length_state;
	sp_mat dD_dZ(Defect_length, z_len);

	// 1.2缺陷矩阵的左侧拟合微分对于状态的微分计算
	int col_num;
	for (int D_disnum = 0; D_disnum < length_state; D_disnum++) {
		for (int int_num = 0; int_num < interval_num; int_num++) {
			col_num = N_col(int_num);
			dD_dZ.rows(row_num, (row_num - 1 + col_num)).cols(column_num, (column_num + col_num)) = ones(col_num, col_num + 1);
			row_num = row_num + col_num;
			column_num = column_num + col_num;
		}
		column_num += 1;
	}

	// 1.3缺陷矩阵的右侧动力学微分对于状态的微分计算
	for (int y_disnum = 0; y_disnum < length_state; y_disnum++) {
		for (int D_disnum = 0; D_disnum < length_state; D_disnum++) {
			if (dD_dS_R(D_disnum, y_disnum) != 0) {
				rs = D_disnum * Col_total;
				cs = y_disnum * (Col_total + 1);                    // LGR插值点比配点多1
				re = (D_disnum + 1) * Col_total - 1;
				ce = (y_disnum + 1) * (Col_total + 1) - 2;          // LGR插值点比配点多1
				dD_dZ.rows(rs, re).cols(cs, ce) = dD_dZ.rows(rs, re).cols(cs, ce) + diagmat(ones(Col_total, 1) * dD_dS_R(D_disnum, y_disnum));
			}
		}
	}

	// 1.4缺陷矩阵对于控制量的微分
	for (int u_disnum = 0; u_disnum < length_control; u_disnum++) {
		for (int D_disnum = 0; D_disnum < length_state; D_disnum++) {
			if (dD_dU(D_disnum, u_disnum) != 0) {
				rs = D_disnum * Col_total;
				cs = column_num + u_disnum * Col_total;              // LGR配点
				re = (D_disnum + 1) * Col_total - 1;
				ce = column_num - 1 + (u_disnum + 1) * Col_total;   // LGR配点
				dD_dZ.rows(rs, re).cols(cs, ce) = diagmat(ones(Col_total, 1) * dD_dU(D_disnum, u_disnum));
			}
		}
	}
	column_num = ce + 1;

	// 1.5缺陷矩阵对于时间的微分
	if (sum(dD_dt) == 0) {
		dD_dZ.cols(column_num + 0, column_num + 1).fill(0);
	}
	else {
		dD_dZ.cols(column_num + 0, column_num + 1).fill(1);
	}

	dD_dZ = spones(dD_dZ);

	//////////////////////////////2、路径约束对于自变量Z的相关性////////////////////////////////////////////
	mat dP_dS = Jacb_struct_info.dP_dS;
	mat dP_dU = Jacb_struct_info.dP_dU;
	int column_num_p;
	int Path_length = Col_total * length_path;
	sp_mat dP_dZ(Path_length, z_len);
	if (length_path == 0) {
		dP_dZ.reset();
	}
	else {
		// 2.1路径约束 - Z雅克比矩阵初始化
		// 2.2路径约束对于状态的微分
		for (int Y_disnum = 0; Y_disnum < length_state; Y_disnum++) {
			for (int C_disnum = 0; C_disnum < length_path; C_disnum++) {
				if (dP_dS(C_disnum, Y_disnum) != 0) {
					rs = C_disnum * Col_total;
					cs = Y_disnum * (Col_total + 1);					// LGR插值点比配点多1
					re = (C_disnum + 1) * Col_total - 1;
					ce = (Y_disnum + 1) * (Col_total + 1) - 2;			// LGR插值点比配点多1
					dP_dZ.rows(rs, re).cols(cs, ce) = diagmat(ones(Col_total, 1) * dP_dS(C_disnum, Y_disnum));
				}
			}
		}
		column_num_p = length_state * (Col_total + 1) - 1;

		// 2.3路径约束对于控制的微分
		for (int U_disnum = 0; U_disnum < length_control; U_disnum++) {
			for (int C_disnum = 0; C_disnum < length_path; C_disnum++) {
				if (dP_dU(C_disnum, U_disnum) != 0) {
					rs = C_disnum * Col_total;
					cs = column_num_p + 1 + U_disnum * Col_total;				// LGR插值点比配点多1
					re = (C_disnum + 1) * Col_total - 1;
					ce = column_num_p + (U_disnum + 1) * Col_total;				// LGR插值点比配点多1
					dP_dZ.rows(rs, re).cols(cs, ce) = diagmat(ones(Col_total, 1) * dP_dU(C_disnum, U_disnum));
				}
			}
		}

	// 2.4路径约束对于端点时间的微分（按照约定，路径约束不显含时间，微分为0）
	}

	//////////////////////////////////////////3、积分约束对于自变量Z的相关性//////////////////////////////////////////////////
	mat dI_dS = Jacb_struct_info.dI_dS;
	mat dI_dU = Jacb_struct_info.dI_dU;
	sp_mat dI_dZ(length_integral, z_len);

	if (length_integral == 0) {
		dI_dZ.reset();
	}
	else {
		// 3.1积分约束 - Z雅克比矩阵初始化
		// 3.2积分约束对于状态量的微分
		int row_repeat0 = 1;
		int col_repeat0 = Col_total + 1;
		dI_dZ.rows(0, length_integral - 1).cols(0, ((Col_total + 1) * length_state - 1)) = repelem(dI_dS, row_repeat0, col_repeat0);
		for (int s_i = 0; s_i < length_state; s_i++) {
			dI_dZ.rows(0, length_integral - 1).col((s_i + 1) * (Col_total + 1) - 1) = zeros(length_integral, 1);
		}

		// 3.3积分约束对于控制量的微分
		col_repeat0 = Col_total;
		int cs1 = (Col_total + 1) * length_state;
		int ce1 = Col_total * length_control + (Col_total + 1) * length_state - 1;
		dI_dZ.rows(0, length_integral - 1).cols(cs1, ce1) = repelem(dI_dU, row_repeat0, col_repeat0);

		// 2.4积分约束对于时间的相关性（由于是积分过程，则必然相关）
		int cs2 = Col_total * length_control + (Col_total + 1) * length_state;
		int ce2 = cs2 + 1;
		dI_dZ.rows(0, length_integral - 1).cols(cs2, ce2).fill(1);
	}

	/////////////////4、事件约束对于自变量Z的相关性////////////////////////////////////
	mat dE_dS = Jacb_struct_info.dE_dS;
	sp_mat dE_dZ(length_event, z_len);

	if (length_event == 0) {
		dE_dZ.reset();
	}
	else {
		// 4.1事件约束 - Z雅克比矩阵初始化
		// 4.2事件约束对于状态量的微分
		for (int i = 0; i < length_state; i++) {
			dE_dZ.col(i * (Col_total + 1)) = dE_dS.col(2 * i);
			dE_dZ.col((i + 1) * (Col_total + 1) - 1) = dE_dS.col(2 * i + 1);
		}
		// 4.3事件约束对于控制量的微分(根据约定，无关)
		// 4.4事件约束对于时间的微分(根据约定，无关)
	}

	/////////////////5、计算约束对于待优化参数的相关性////////////////////////////////////
	int length_F0 = Col_total*length_state + Col_total*length_path + length_integral + length_event;        // NLP约束数目
	sp_mat dF_dp; dF_dp.reset();
	if (length_parameter != 0) {
		umat dD_dp = Jacb_struct_info.dD_dp;
		umat dP_dp = Jacb_struct_info.dP_dp;
		sp_mat dI_dp = Jacb_struct_info.dI_dp;
		mat dE_dp = Jacb_struct_info.dE_dp;
		//dD_dp.print("dD_dp");
		//dP_dp.print("dP_dp");
		//dI_dp.print("dI_dp");
		//dE_dp.print("dE_dp");
		dF_dp = sp_mat(length_F0, length_parameter);
		for (int i = 1; i <= length_state; i++) {
			int ns = 1 + (i - 1)*Col_total;
			int ne = i*Col_total;
			dF_dp.rows(ns - 1, ne - 1) = conv_to<mat>::from(repmat(dD_dp.row(i - 1), Col_total, 1));
		}
		for (int i = 1; i <= length_path; i++) {
			int ns = Col_total*length_state + 1 + (i - 1)*Col_total;
			int ne = Col_total*length_state + i*Col_total;
			dF_dp.rows(ns - 1, ne - 1) = conv_to<mat>::from(repmat(dP_dp.row(i - 1), Col_total, 1));
		}
		if (length_integral != 0) {
			for (int i = 1; i <= length_integral; i++){
				int n = Col_total*length_state + Col_total*length_path + i;
				dF_dp.row(n - 1) = dI_dp.row(i - 1);
			}
		}
		if (length_event != 0) {
			for (int i = 1; i <= length_event; i++) {
				int n = Col_total*length_state + Col_total*length_path + length_integral + i;
				dF_dp.row(n - 1) = dE_dp.row(i - 1);
			}
		}
	}

	*Jacb_struct1 = join_rows(join_cols(dD_dZ, dP_dZ, dI_dZ, dE_dZ), dF_dp);

	return 1;
}