PS_Method_CCS_test/Pseudo_Spectral_Method/Process/NLP_grad/Istruct.cpp

/*
% 函数功能：
%       判断积分矩阵对于自变量Z的依赖关系
% 输入：
%       Z0:自变量
%       Z_num:状态、控制、积分、时间的起止序号
%       Dynamics:动力学方程函数句柄
%       Objective: 性能指标函数句柄
%       Scal：归一化标识
%       Sca_UY：状态控制量归一化系数
%       Sca_t：时间归一化系数
%       deta_t_in_col：配点所对应时间段大小
%       Col_total：总配点数目
%       auxdata：其他参数
%       W_phase：积分权重矩阵
%       length_state：状态长度
%       length_control:控制量长度
%       eps0：求导步长
% 输出：
%       I_Z_struct0:积分依赖性向量
% 编写：
%       李兆亭，2020/07/03
% C++ : 李兆亭，2020/09/23
*/

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

using namespace std;
using namespace arma;

int Istruct(vec Z1, int Col_total, int length_state, int length_control, int length_integral, mat W_phase, auxdata1 auxdata,
			int(*Dynamics_ptr)(mat* StateVar, mat* ControlVar, auxdata1 auxdata, mat* dState, mat* integrand, mat* path),
			vec* I) {

	// 变量Z的离解
	int num0 = size(Z1)(0);
	double tf = Z1(num0 - 1);
	double t0 = Z1(num0 - 2);
	int total_state = (Col_total + 1)*length_state;
	int total_control = Col_total*length_control;
	mat Y_phase_col = Z1.rows(0, total_state - 1);
	mat U_phase_col = Z1.rows(total_state, (total_state + total_control - 1));
	Y_phase_col = reshape(Y_phase_col, Col_total + 1, length_state);		//状态量矩阵
	U_phase_col = reshape(U_phase_col, Col_total, length_control);			//控制量矩阵
	mat Y_phase_col1 = Y_phase_col.rows(0, Col_total - 1);

	// 积分约束矩阵
	mat dState, I_phase, path;
#ifdef LUOYC20220701
	printf("this is Istruct:\n");
#endif
#ifdef LUOYC20220701
	printf("Istruct Y_phase_col1 = :\n");
	for (int j = 0; j < 36; j++)
	{
		printf("%lf ", *(Y_phase_col1.mem + j));
	}
	printf("\n");

	printf("Istruct U_phase_col = :\n");
	for (int j = 0; j < 12; j++)
	{
		printf("%lf ", *(U_phase_col.mem + j));
	}
	printf("\n");
#endif
	(*Dynamics_ptr)(&Y_phase_col1, &U_phase_col, auxdata, &dState, &I_phase, &path);
#ifdef LUOYC20220701
	printf("Istruct Path_phase = :\n");
	for (int j = 0; j < 18; j++)
	{
		printf("%lf ", *(path.mem + j));
	}
	printf("\n");
#endif
	vec Q_integral_phase1 = ((tf - t0)*W_phase*I_phase).t();
	*I = Q_integral_phase1;

	return 1;
}