PS_Method_CCS_test/Pseudo_Spectral_Method/Process/NLP_grad/IZstruct.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 IZstruct(vec Z0, int Col_total, int length_state, int length_control, int length_integral, mat W_phase, auxdata1 auxdata, sp_mat*I_Z_struct0,
			int(*Dynamics_ptr)(mat* StateVar, mat* ControlVar, auxdata1 auxdata, mat* dState, mat* integrand, mat* path)) {

	// 初始化
	int DIM_all = length_state + length_control + 2;
	mat I_Z_struct01 = zeros(length_integral, DIM_all);

	// 计算依赖性矩阵
	int num;
	vec Z1, I1;
	for (int i = 1; i <= DIM_all; i++) {
		if (i <= length_state) {
			num = 1 + (i - 1)*(Col_total + 1) - 1;
		}		
		else if (i > length_state && i <= length_state + length_control) {
			num = length_state*(Col_total + 1) + 1 + (i - 1 - length_state)*Col_total - 1;
		}	
		else if (i > length_state + length_control) {
			num = length_state*(Col_total + 1) + length_control*Col_total + (i - length_control - length_state) - 1;
		}
		Z1 = Z0;
		Z1(num) = datum::nan;
		Istruct(Z1, Col_total, length_state, length_control, length_integral, W_phase, auxdata, Dynamics_ptr, &I1);
		I_Z_struct01.col(i - 1) = I1;
	}
			
	I_Z_struct01.elem(find_finite(I_Z_struct01)).zeros();		// 不相关设为0
	I_Z_struct01.elem(find_nonfinite(I_Z_struct01)).ones();		// 相关设为1

	(*I_Z_struct0) = sp_mat(I_Z_struct01);

	return 1;
}