Convex_Optimization_VS/CMSCP2/userFunctions.c

#include "userFunctions.h"
#include "freeTrajInfo.h"
void getIniRefTraj(cmscp_trajInfo** refTraj, cmscp_setup* setup)
{
	int i, ij, k, numPhase, iphase, numMesh, maxNumNode, isOutput, isIntegOver, numState, numControl, numParameter, cnt, length, integNumState, integNumControl;
	double outStepSize, integStepSize, newState[8], oldState[8], u[2], * stateK, * controlK, outTime, tempWork[37], path[5], * statePhaseKm1f, * meshNode, * integState0, * integStatef, * integControl0, * integControlf, * time;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_auxdata* auxdata;
	cmscp_scales* scales;
	cmscp_integTraj** integTraj;

	// 问题维数
	dimension = setup->dimension;
	numPhase = setup->numPhase;
	mesh = setup->mesh;
	auxdata = (cmscp_auxdata*)setup->auxdata;
	scales = auxdata->scales;

	// 存储积分轨迹的变量
	integTraj = (cmscp_integTraj**)malloc(numPhase * sizeof(cmscp_integTraj*));

	// 第1段：调姿
	iphase = 0;
	outStepSize = 5.0 / scales->time; // 输出数据的步长，每经过outputStep输出一个数据
	integStepSize = 1 / scales->time; // 积分步长
	// 分配积分轨迹的空间
	maxNumNode = 500;
	numState = 8;
	numControl = 2;
	integTraj[iphase] = (cmscp_integTraj*)malloc(sizeof(cmscp_integTraj));
	integTraj[iphase]->state = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->state->m = numState;
	integTraj[iphase]->state->n = maxNumNode;
	integTraj[iphase]->state->v = (double*)malloc(numState * maxNumNode * sizeof(double));
	integTraj[iphase]->control = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->control->m = numControl;
	integTraj[iphase]->control->n = maxNumNode;
	integTraj[iphase]->control->v = (double*)malloc(numControl * maxNumNode * sizeof(double));
	integTraj[iphase]->time = (double*)malloc(maxNumNode * sizeof(double));
	// 初始状态和控制输入
	copyFloatVec(auxdata->x0, oldState, 8, 0);
	u[0] = 0.0;
	u[1] = 0.0;
	// 保存初始点参数
	cnt = 0;
	stateK = integTraj[iphase]->state->v;
	controlK = integTraj[iphase]->control->v;
	copyFloatVec(oldState, stateK, numState, 0);
	copyFloatVec(u, controlK, numControl, 0);
	integTraj[iphase]->time[cnt] = stateK[numState - 1];
	outTime = oldState[numState - 1] + outStepSize;
	isOutput = 0;

	// 动力学积分
	isIntegOver = 0;
	while (isIntegOver == 0)
	{
		// 控制输入
		u[0] = 0.0;
		u[1] = 0.0;
		// 更新状态
		computeInteg(newState, dynInteg2, oldState, u, NULL, auxdata, 8, 2, 0, 5, tempWork, integStepSize, 3);
		copyFloatVec(newState, oldState, numState, 0);
		dynInteg2(NULL, path, oldState, u, NULL, auxdata);
		// 积分结束标志位
		if (fabs(oldState[numState - 1] - auxdata->x0[7]) > 30 / scales->time || oldState[numState - 1] > 2000 / scales->time)
		{
			isIntegOver = 1;
		}
		// 更新输出标志位
		if ((fabs(outTime - oldState[numState - 1]) <= 0.5 * integStepSize) || isIntegOver)
		{
			isOutput = 1;
			outTime = outTime + outStepSize;
		}
		// 保存状态、控制和约束值
		if (isOutput == 1)
		{
			cnt = cnt + 1;
			stateK = stateK + numState;
			controlK = controlK + numControl;
			copyFloatVec(oldState, stateK, numState, 0);
			copyFloatVec(u, controlK, numControl, 0);
			integTraj[iphase]->time[cnt] = stateK[numState - 1];
			isOutput = 0;
			if (isIntegOver == 1)
			{
				integTraj[iphase]->length = cnt + 1;
			}
		}
	}

	// 第2段：调姿
	iphase = 1;
	outStepSize = 5.0 / scales->time; // 输出数据的步长，每经过outputStep输出一个数据
	integStepSize = 1 / scales->time; // 积分步长
	// 分配积分轨迹的空间
	maxNumNode = 500;
	numState = 8;
	numControl = 2;
	integTraj[iphase] = (cmscp_integTraj*)malloc(sizeof(cmscp_integTraj));
	integTraj[iphase]->state = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->state->m = numState;
	integTraj[iphase]->state->n = maxNumNode;
	integTraj[iphase]->state->v = (double*)malloc(numState * maxNumNode * sizeof(double));
	integTraj[iphase]->control = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->control->m = numControl;
	integTraj[iphase]->control->n = maxNumNode;
	integTraj[iphase]->control->v = (double*)malloc(numControl * maxNumNode * sizeof(double));
	integTraj[iphase]->time = (double*)malloc(maxNumNode * sizeof(double));
	// 初始状态和控制输入
	statePhaseKm1f = integTraj[iphase - 1]->state->v + (integTraj[iphase - 1]->length - 1) * integTraj[iphase - 1]->state->m;
	copyFloatVec(statePhaseKm1f, oldState, numState, 0);
	u[0] = 1.0 * auxdata->phiT0;
	u[1] = 1.0 * auxdata->psiT0;
	// 保存初始点参数
	cnt = 0;
	stateK = integTraj[iphase]->state->v;
	controlK = integTraj[iphase]->control->v;
	copyFloatVec(oldState, stateK, numState, 0);
	copyFloatVec(u, controlK, numControl, 0);
	integTraj[iphase]->time[cnt] = stateK[numState - 1];
	outTime = oldState[numState - 1] + outStepSize;
	isOutput = 0;

	// 动力学积分
	isIntegOver = 0;
	while (isIntegOver == 0)
	{
		// 控制输入
		u[0] = 1.0 * auxdata->phiT0;
		u[1] = 1.0 * auxdata->psiT0;
		// 更新状态
		computeInteg(newState, dynInteg1, oldState, u, NULL, auxdata, 8, 2, 0, 5, tempWork, integStepSize, 3);
		copyFloatVec(newState, oldState, numState, 0);
		dynInteg1(NULL, path, oldState, u, NULL, auxdata);
		// 积分结束标志位
		if (fabs(oldState[3] - statePhaseKm1f[3]) > 360 / scales->speed || path[0] < 100 * 1000 / scales->length || oldState[3] > 7100 / scales->speed || oldState[numState - 1] > 2000 / scales->time)
		{
			isIntegOver = 1;
		}
		// 更新输出标志位
		if ((fabs(outTime - oldState[numState - 1]) <= 0.5 * integStepSize) || isIntegOver)
		{
			isOutput = 1;
			outTime = outTime + outStepSize;
		}
		// 保存状态、控制和约束值
		if (isOutput == 1)
		{
			cnt = cnt + 1;
			stateK = stateK + numState;
			controlK = controlK + numControl;
			copyFloatVec(oldState, stateK, numState, 0);
			copyFloatVec(u, controlK, numControl, 0);
			integTraj[iphase]->time[cnt] = stateK[numState - 1];
			isOutput = 0;
			if (isIntegOver == 1)
			{
				integTraj[iphase]->length = cnt + 1;
			}
		}
	}

	// 第3段：调姿
	iphase = 2;
	outStepSize = 5.0 / scales->time; // 输出数据的步长，每经过outputStep输出一个数据
	integStepSize = 1 / scales->time; // 积分步长
	// 分配积分轨迹的空间
	maxNumNode = 500;
	numState = 8;
	numControl = 2;
	integTraj[iphase] = (cmscp_integTraj*)malloc(sizeof(cmscp_integTraj));
	integTraj[iphase]->state = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->state->m = numState;
	integTraj[iphase]->state->n = maxNumNode;
	integTraj[iphase]->state->v = (double*)malloc(numState * maxNumNode * sizeof(double));
	integTraj[iphase]->control = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->control->m = numControl;
	integTraj[iphase]->control->n = maxNumNode;
	integTraj[iphase]->control->v = (double*)malloc(numControl * maxNumNode * sizeof(double));
	integTraj[iphase]->time = (double*)malloc(maxNumNode * sizeof(double));
	// 初始状态和控制输入
	statePhaseKm1f = integTraj[iphase - 1]->state->v + (integTraj[iphase - 1]->length - 1) * integTraj[iphase - 1]->state->m;
	copyFloatVec(statePhaseKm1f, oldState, numState, 0);
	u[0] = 0.0;
	u[1] = 0.0;
	// 保存初始点参数
	cnt = 0;
	stateK = integTraj[iphase]->state->v;
	controlK = integTraj[iphase]->control->v;
	copyFloatVec(oldState, stateK, numState, 0);
	copyFloatVec(u, controlK, numControl, 0);
	integTraj[iphase]->time[cnt] = stateK[numState - 1];
	outTime = oldState[numState - 1] + outStepSize;
	isOutput = 0;

	// 动力学积分
	isIntegOver = 0;
	while (isIntegOver == 0)
	{
		// 控制输入
		u[0] = 0.0;
		u[1] = 0.0;
		// 更新状态
		computeInteg(newState, dynInteg2, oldState, u, NULL, auxdata, 8, 2, 0, 5, tempWork, integStepSize, 3);
		copyFloatVec(newState, oldState, numState, 0);
		dynInteg2(NULL, path, oldState, u, NULL, auxdata);
		// 积分结束标志位
		if (path[0] < 95 * 1000 / scales->length || oldState[numState - 1] > 2000 / scales->time)
		{
			isIntegOver = 1;
		}
		// 更新输出标志位
		if ((fabs(outTime - oldState[numState - 1]) <= 0.5 * integStepSize) || isIntegOver)
		{
			isOutput = 1;
			outTime = outTime + outStepSize;
		}
		// 保存状态、控制和约束值
		if (isOutput == 1)
		{
			cnt = cnt + 1;
			stateK = stateK + numState;
			controlK = controlK + numControl;
			copyFloatVec(oldState, stateK, numState, 0);
			copyFloatVec(u, controlK, numControl, 0);
			integTraj[iphase]->time[cnt] = stateK[numState - 1];
			isOutput = 0;
			if (isIntegOver == 1)
			{
				integTraj[iphase]->length = cnt + 1;
			}
		}
	}

	// 第4段：正常再入段1
	iphase = 3;
	outStepSize = 4.0 / scales->time; // 输出数据的步长，每经过outputStep输出一个数据
	integStepSize = 1 / scales->time; // 积分步长
	// 分配积分轨迹的空间
	maxNumNode = 500;
	numState = 8;
	numControl = 1;
	integTraj[iphase] = (cmscp_integTraj*)malloc(sizeof(cmscp_integTraj));
	integTraj[iphase]->state = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->state->m = numState;
	integTraj[iphase]->state->n = maxNumNode;
	integTraj[iphase]->state->v = (double*)malloc(numState * maxNumNode * sizeof(double));
	integTraj[iphase]->control = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->control->m = numControl;
	integTraj[iphase]->control->n = maxNumNode;
	integTraj[iphase]->control->v = (double*)malloc(numControl * maxNumNode * sizeof(double));
	integTraj[iphase]->time = (double*)malloc(maxNumNode * sizeof(double));
	// 初始状态和控制输入
	statePhaseKm1f = integTraj[iphase - 1]->state->v + (integTraj[iphase - 1]->length - 1) * integTraj[iphase - 1]->state->m;
	copyFloatVec(statePhaseKm1f, oldState, numState, 0);
	oldState[6] = auxdata->mass2 / scales->mass;
	u[0] = -20.0 * d2r;
	// 保存初始点参数
	cnt = 0;
	stateK = integTraj[iphase]->state->v;
	controlK = integTraj[iphase]->control->v;
	copyFloatVec(oldState, stateK, numState, 0);
	copyFloatVec(u, controlK, numControl, 0);
	integTraj[iphase]->time[cnt] = stateK[numState - 1];
	outTime = oldState[numState - 1] + outStepSize;
	isOutput = 0;

	// 动力学积分
	isIntegOver = 0;
	while (isIntegOver == 0)
	{
		// 控制输入
		u[0] = -20.0 * d2r;
		// 更新状态
		computeInteg(newState, dynInteg3, oldState, u, NULL, auxdata, 8, 2, 0, 5, tempWork, integStepSize, 3);
		copyFloatVec(newState, oldState, numState, 0);
		dynInteg3(NULL, path, oldState, u, NULL, auxdata);
		// 积分结束标志位
		if (oldState[3] < 4 * 1000 / scales->speed || oldState[numState - 1] > 2000 / scales->time)
		{
			isIntegOver = 1;
		}
		// 更新输出标志位
		if ((fabs(outTime - oldState[numState - 1]) <= 0.5 * integStepSize) || isIntegOver)
		{
			isOutput = 1;
			outTime = outTime + outStepSize;
		}
		// 保存状态、控制和约束值
		if (isOutput == 1)
		{
			cnt = cnt + 1;
			stateK = stateK + numState;
			controlK = controlK + numControl;
			copyFloatVec(oldState, stateK, numState, 0);
			copyFloatVec(u, controlK, numControl, 0);
			integTraj[iphase]->time[cnt] = stateK[numState - 1];
			isOutput = 0;
			if (isIntegOver == 1)
			{
				integTraj[iphase]->length = cnt + 1;
			}
		}
	}

	// 第5段：正常再入段2
	iphase = 4;
	outStepSize = 5.0 / scales->time; // 输出数据的步长，每经过outputStep输出一个数据
	integStepSize = 1 / scales->time; // 积分步长
	// 分配积分轨迹的空间
	maxNumNode = 200;
	numState = 8;
	numControl = 1;
	integTraj[iphase] = (cmscp_integTraj*)malloc(sizeof(cmscp_integTraj));
	integTraj[iphase]->state = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->state->m = numState;
	integTraj[iphase]->state->n = maxNumNode;
	integTraj[iphase]->state->v = (double*)malloc(numState * maxNumNode * sizeof(double));
	integTraj[iphase]->control = (demat*)malloc(sizeof(demat));
	integTraj[iphase]->control->m = numControl;
	integTraj[iphase]->control->n = maxNumNode;
	integTraj[iphase]->control->v = (double*)malloc(numControl * maxNumNode * sizeof(double));
	integTraj[iphase]->time = (double*)malloc(maxNumNode * sizeof(double));
	// 初始状态和控制输入
	statePhaseKm1f = integTraj[iphase - 1]->state->v + (integTraj[iphase - 1]->length - 1) * integTraj[iphase - 1]->state->m;
	copyFloatVec(statePhaseKm1f, oldState, numState, 0);
	u[0] = 20.0 * d2r;
	// 保存初始点参数
	cnt = 0;
	stateK = integTraj[iphase]->state->v;
	controlK = integTraj[iphase]->control->v;
	copyFloatVec(oldState, stateK, numState, 0);
	copyFloatVec(u, controlK, numControl, 0);
	integTraj[iphase]->time[cnt] = stateK[numState - 1];
	outTime = oldState[numState - 1] + outStepSize;
	isOutput = 0;

	// 动力学积分
	isIntegOver = 0;
	while (isIntegOver == 0)
	{
		// 控制输入
		u[0] = 20.0 * d2r;
		// 更新状态
		computeInteg(newState, dynInteg3, oldState, u, NULL, auxdata, 8, 2, 0, 5, tempWork, integStepSize, 3);
		copyFloatVec(newState, oldState, numState, 0);
		dynInteg3(NULL, path, oldState, u, NULL, auxdata);
		// 积分结束标志位
		if (path[0] < 8 * 1000 / scales->length || oldState[numState - 1] > 2000 / scales->time)
		{
			isIntegOver = 1;
		}
		// 更新输出标志位
		if ((fabs(outTime - oldState[numState - 1]) <= 0.5 * integStepSize) || isIntegOver)
		{
			isOutput = 1;
			outTime = outTime + outStepSize;
		}
		// 保存状态、控制和约束值
		if (isOutput == 1)
		{
			cnt = cnt + 1;
			stateK = stateK + numState;
			controlK = controlK + numControl;
			copyFloatVec(oldState, stateK, numState, 0);
			copyFloatVec(u, controlK, numControl, 0);
			integTraj[iphase]->time[cnt] = stateK[numState - 1];
			isOutput = 0;
			if (isIntegOver == 1)
			{
				integTraj[iphase]->length = cnt + 1;
			}
		}
	}

	// 插值获取参考轨迹
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		numMesh = mesh[iphase]->num;
		numState = dimension->phase[iphase]->state;
		numControl = dimension->phase[iphase]->control;
		numParameter = dimension->phase[iphase]->parameter;
		meshNode = mesh[iphase]->node;

		length = integTraj[iphase]->length;
		integNumState = integTraj[iphase]->state->m;
		integState0 = integTraj[iphase]->state->v;
		integStatef = integTraj[iphase]->state->v + (length - 1) * integNumState;
		integNumControl = integTraj[iphase]->control->m;
		integControl0 = integTraj[iphase]->control->v;
		integControlf = integTraj[iphase]->control->v + (length - 1) * integNumControl;


		refTraj[iphase] = (cmscp_trajInfo*)malloc(sizeof(cmscp_trajInfo));
		refTraj[iphase]->state = (cmscp_floatDemat*)malloc(sizeof(cmscp_floatDemat));
		refTraj[iphase]->state->m = numState;
		refTraj[iphase]->state->n = numMesh;
		refTraj[iphase]->state->v = (double*)malloc(numState * numMesh * sizeof(double));
		refTraj[iphase]->control = (cmscp_floatDemat*)malloc(sizeof(cmscp_floatDemat));
		refTraj[iphase]->control->m = numControl;
		refTraj[iphase]->control->n = numMesh;
		refTraj[iphase]->control->v = (double*)malloc(numControl * numMesh * sizeof(double));
		refTraj[iphase]->parameter = (double*)malloc(numParameter * sizeof(double));

		refTraj[iphase]->initialtime = integState0[numState - 1];
		refTraj[iphase]->finaltime = integStatef[numState - 1];

		denseScaPlusVec(integTraj[iphase]->time, -refTraj[iphase]->initialtime, integTraj[iphase]->time, length);
		denseScaV(integTraj[iphase]->time, 1.0 / (refTraj[iphase]->finaltime - refTraj[iphase]->initialtime), integTraj[iphase]->time, length);
		ascLinearInterp(integTraj[iphase]->time,
			integTraj[iphase]->state->v,
			length, integNumState,
			meshNode, numMesh, refTraj[iphase]->state->v);
		ascLinearInterp(integTraj[iphase]->time,
			integTraj[iphase]->control->v, length,
			integNumControl,
			meshNode,
			numMesh, refTraj[iphase]->control->v);
		floatVecFillin(refTraj[iphase]->parameter, numParameter, 0.0);

		free(integTraj[iphase]->time);
		free(integTraj[iphase]->control->v);
		free(integTraj[iphase]->control);
		free(integTraj[iphase]->state->v);
		free(integTraj[iphase]->state);
		free(integTraj[iphase]);
	}
	free(integTraj);
	
	// 范围限制
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		// 每段维度
		numMesh = mesh[iphase]->num;
		numState = dimension->phase[iphase]->state;
		numControl = dimension->phase[iphase]->control;
		numParameter = dimension->phase[iphase]->parameter;
		meshNode = mesh[iphase]->node;

		// 初始时间限制
		if (refTraj[iphase]->initialtime > setup->bounds->phase[iphase]->initialtime->upper)
		{
			refTraj[iphase]->initialtime = setup->bounds->phase[iphase]->initialtime->upper;
		}
		else if(refTraj[iphase]->initialtime < setup->bounds->phase[iphase]->initialtime->lower)
		{
			refTraj[iphase]->initialtime = setup->bounds->phase[iphase]->initialtime->lower;
		}

		// 终端时间限制
		if (refTraj[iphase]->finaltime > setup->bounds->phase[iphase]->finaltime->upper)
		{
			refTraj[iphase]->initialtime = setup->bounds->phase[iphase]->finaltime->upper;
		}
		else if (refTraj[iphase]->finaltime < setup->bounds->phase[iphase]->finaltime->lower)
		{
			refTraj[iphase]->finaltime = setup->bounds->phase[iphase]->finaltime->lower;
		}

		// 状态变量限制
		for ( i = 0; i < numState; i++)
		{
			ij = i + numState * 0;
			if (refTraj[iphase]->state->v[ij] > setup->bounds->phase[iphase]->initialstate->upper[i])
			{
				refTraj[iphase]->state->v[ij] = setup->bounds->phase[iphase]->initialstate->upper[i];
			}
			else if (refTraj[iphase]->state->v[ij] < setup->bounds->phase[iphase]->initialstate->lower[i])
			{
				refTraj[iphase]->state->v[ij] = setup->bounds->phase[iphase]->initialstate->lower[i];
			}
			ij = i + numState * (numMesh -1);
			if (refTraj[iphase]->state->v[ij] > setup->bounds->phase[iphase]->finalstate->upper[i])
			{
				refTraj[iphase]->state->v[ij] = setup->bounds->phase[iphase]->finalstate->upper[i];
			}
			else if (refTraj[iphase]->state->v[ij] < setup->bounds->phase[iphase]->finalstate->lower[i])
			{
				refTraj[iphase]->state->v[ij] = setup->bounds->phase[iphase]->finalstate->lower[i];
			}
			for (k = 1; k < numMesh-1; k++)
			{
				ij = i + numState * k;
				if (refTraj[iphase]->state->v[ij] > setup->bounds->phase[iphase]->state->upper[i])
				{
					refTraj[iphase]->state->v[ij] = setup->bounds->phase[iphase]->state->upper[i];
				}
				else if (refTraj[iphase]->state->v[ij] < setup->bounds->phase[iphase]->state->lower[i])
				{
					refTraj[iphase]->state->v[ij] = setup->bounds->phase[iphase]->state->lower[i];
				}
			}
		}

		// 控制变量限制
		for (i = 0; i < numControl; i++)
		{
			for (k = 0; k < numMesh; k++)
			{
				ij = i + numControl * k;
				if (refTraj[iphase]->control->v[ij] > setup->bounds->phase[iphase]->control->upper[i])
				{
					refTraj[iphase]->control->v[ij] = setup->bounds->phase[iphase]->control->upper[i];
				}
				else if (refTraj[iphase]->control->v[ij] < setup->bounds->phase[iphase]->control->lower[i])
				{
					refTraj[iphase]->control->v[ij] = setup->bounds->phase[iphase]->control->lower[i];
				}
			}
		}

		// 静态参数变量限制
		for (i = 0; i < numParameter; i++)
		{
			if (refTraj[iphase]->parameter[i] > setup->bounds->phase[iphase]->parameter->upper[i])
			{
				refTraj[iphase]->parameter[i] = setup->bounds->phase[iphase]->parameter->upper[i];
			}
			else if (refTraj[iphase]->parameter[i] < setup->bounds->phase[iphase]->parameter->lower[i])
			{
				refTraj[iphase]->parameter[i] = setup->bounds->phase[iphase]->parameter->lower[i];
			}
		}
	}
}

int stopCondition(int iIter, cmscp_trajInfo** refTraj, cmscp_trajInfo** optTraj, cmscp_setup* setup)
{
	int numPhase, iphase, numMesh, numState, j, stopFlag;
	double maxHeightErr, * optStateK, * refStateK;
	cmscp_auxdata* auxdata;
	cmscp_scales* scales;
	numPhase = setup->numPhase;
	auxdata = (cmscp_auxdata*)setup->auxdata;
	scales = auxdata->scales;
	maxHeightErr = 0.0;
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		numMesh = setup->mesh[iphase]->num;
		numState = setup->dimension->phase[iphase]->state;
		for (j = 0; j < numMesh; j++)
		{
			optStateK = optTraj[iphase]->state->v + j * numState;
			refStateK = refTraj[iphase]->state->v + j * numState;
			maxHeightErr = fmax(maxHeightErr, fabs(optStateK[0] - refStateK[0]));
		}
	}
	maxHeightErr = maxHeightErr * scales->length;
	if (maxHeightErr <= 2000)
	{
		stopFlag = 1;
	}
	else if (iIter == setup->scpInfo->maxIter - 1)
	{
		stopFlag = 2;
	}
	else
	{
		stopFlag = 0;
	}
	return stopFlag;
}
void updateMeshInfo(int iIter, cmscp_trajInfo** refTraj, cmscp_trajInfo** optTraj, cmscp_setup* setup)
{
	int numPhase, iphase;
	cmscp_mesh**newMesh, ** oldMesh;
	numPhase = setup->numPhase;
	// 网格更新
	setup->oldMesh = setup->mesh;
	setup->mesh = (cmscp_mesh**)malloc(numPhase * sizeof(cmscp_mesh*));
	newMesh = setup->mesh;
	oldMesh = setup->oldMesh;
	for (iphase = 0;  iphase < numPhase; iphase++)
	{
		newMesh[iphase] = (cmscp_mesh*)malloc(sizeof(cmscp_mesh));
		newMesh[iphase]->num = oldMesh[iphase]->num;
		newMesh[iphase]->node = (double*)malloc(newMesh[iphase]->num * sizeof(double));
		copyFloatVec(oldMesh[iphase]->node, newMesh[iphase]->node, newMesh[iphase]->num, 0);
	}
}

void updateTrustInfo(int iIter, cmscp_trajInfo** refTraj, cmscp_trajInfo** optTraj, cmscp_setup* setup)
{
	int numPhase, iphase;
	double delta;
	numPhase = setup->numPhase;
	if (iIter == 0)
	{
		setup->trust->merit = 1E30;
	}
	if (iIter >= 5)
	{
		if (setup->trust->merit > 1E-4)
		{
			setup->trust->merit = fabs(setup->scpInfo->obj[iIter] - setup->scpInfo->obj[iIter - 2]) / fmax(fabs(setup->scpInfo->obj[iIter]), 2);
		}
		else
		{
			setup->trust->merit = 0;
		}
	}
	if (setup->trust->merit <= 5E-4) {
		for ( iphase = 0; iphase < numPhase; iphase++)
		{
			setup->trust->phase[iphase]->hardRadius = setup->trust->phase[iphase]->hardRadius / 2;
		}
	}
	else
	{
		if(iIter <= 9) {
			for (iphase = 0; iphase < numPhase; iphase++)
			{
				setup->trust->phase[iphase]->hardRadius = 0.5 + (0.1 - 0.5) / 10 * (iIter + 1);
			}
		}
	}
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		if (setup->trust->phase[iphase]->hardRadius <= 0.05)
		{
			setup->trust->phase[iphase]->hardRadius = 0.05;
			if (setup->trust->merit <= 1E-4 && iphase == 0)
			{
				setup->trust->model = "soft";
			}
			
		}
	}
}
void updateRefTraj(cmscp_trajInfo** refTraj, cmscp_trajInfo** optTraj, cmscp_setup* setup)
{
	int iphase, numPhase, oldNumMesh, newNumMesh, numState, numControl, numParameter;
	double* oldMeshNode, *newMeshNode;
	cmscp_mesh** oldMesh, ** newMesh;
	cmscp_dimension* dimension;
	oldMesh = setup->mesh;
	newMesh = setup->mesh;
	numPhase = setup->numPhase;
	dimension = setup->dimension;

	// 插值获取参考轨迹
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		oldNumMesh = oldMesh[iphase]->num;
		oldMeshNode = oldMesh[iphase]->node;
		newNumMesh = newMesh[iphase]->num;
		newMeshNode = newMesh[iphase]->node;
		numState = dimension->phase[iphase]->state;
		numControl = dimension->phase[iphase]->control;
		numParameter = dimension->phase[iphase]->parameter;
		

		// 释放之前的空间
		freeTrajInfoPhase(refTraj, setup, iphase);

		// 重新申请空间
		refTraj[iphase] = (cmscp_trajInfo*)malloc(sizeof(cmscp_trajInfo));
		refTraj[iphase]->state = (cmscp_floatDemat*)malloc(sizeof(cmscp_floatDemat));
		refTraj[iphase]->state->m = numState;
		refTraj[iphase]->state->n = newNumMesh;
		refTraj[iphase]->state->v = (double*)malloc(numState * newNumMesh * sizeof(double));
		refTraj[iphase]->control = (cmscp_floatDemat*)malloc(sizeof(cmscp_floatDemat));
		refTraj[iphase]->control->m = numControl;
		refTraj[iphase]->control->n = newNumMesh;
		refTraj[iphase]->control->v = (double*)malloc(numControl * newNumMesh * sizeof(double));
		refTraj[iphase]->parameter = (double*)malloc(numParameter * sizeof(double));

		ascLinearInterp(oldMeshNode,
			optTraj[iphase]->state->v,
			oldNumMesh, numState,
			newMeshNode, newNumMesh, refTraj[iphase]->state->v);
		ascLinearInterp(oldMeshNode,
			optTraj[iphase]->control->v,
			oldNumMesh, numControl,
			newMeshNode, newNumMesh, refTraj[iphase]->control->v);
		copyFloatVec(optTraj[iphase]->parameter, refTraj[iphase]->parameter, numParameter, 0);
		refTraj[iphase]->initialtime = optTraj[iphase]->initialtime;
		refTraj[iphase]->finaltime = optTraj[iphase]->finaltime;
	}
	// 释放旧网格
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		free(setup->oldMesh[iphase]->node);
		free(setup->oldMesh[iphase]);
	}
	free(setup->oldMesh);
	// 释放最优轨迹
	freeTrajInfo(optTraj, setup, 0);
}

void getObjEventDepend(intDemat* dependMatrix)
{
	// 函数功能：获取事先准备目标函数和事件函数的相关性矩阵
	// 输入：
	// dependMatrix：没有存储相关性矩阵的地址
	// 输出：
	// dependMatrix：存储相关性矩阵的地址
	// 谢磊：2022 / 6 / 25编写
	int i, j, numRow, numCol, * dependMatrixJ;
	// 第1段
	numRow = 1;
	numCol = 100;
	dependMatrix->m = numRow;
	dependMatrix->n = numCol;
	dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
	dependMatrixJ = dependMatrix->v;
	for (j = 0; j < numCol; j++)
	{
		for (i = 0; i < numRow; i++)
		{
			if ((i == 0 && j == 98) || (i == 0 && j == 99))
			{
				dependMatrixJ[i] = 1;
			}
			else
			{
				dependMatrixJ[i] = 0;
			}

		}
		dependMatrixJ = dependMatrixJ + numRow;
	}
}

void getContDepend(intDemat* dependMatrix, int iphase, cmscp_setup* setup)
{
	// 函数功能：获取事先准备时间连续函数的相关性矩阵
	// 输入：
	// dependMatrix：没有存储相关性矩阵的地址
	// iphase：需要的是第几段的相关性矩阵
	// setup：问题的设置参数
	// 输出：
	// dependMatrix：存储相关性矩阵的地址
	// 谢磊：2022 / 6 / 25编写
	int a1[11][12] = {
		{0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0},
		{1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0},
		{1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 0},
		{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0},
		{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1},
	};
	int a2[11][12] = {
		{0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0},
		{1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1},
	};
	int a3[11][10] = {
		{0, 0, 0, 1, 1, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 0, 0, 0, 0},
		{1, 0, 0, 1, 1, 1, 0, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 1, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 1, 0, 1, 0},
		{1, 0, 1, 1, 1, 1, 0, 0, 1, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 0, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 1},
	};
	int a4[11][12] = {
		{0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0},
		{1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0},
		{1, 0, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{1, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0},
	};

	int i, j, numRow, numCol, numPhase, * dependMatrixJ, numState;
	numPhase = 5;
	if (iphase == 0)
	{
		// 第1段
		numRow = 11;
		numCol = 12;
		numState = setup->dimension->phase[iphase]->state;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				if (i < numState && i == j)
				{
					dependMatrixJ[i] = 1;
				}
				else
				{
					dependMatrixJ[i] = a2[i][j];
				}
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 1)
	{
		// 第2段
		numRow = 11;
		numCol = 12;
		numState = setup->dimension->phase[iphase]->state;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				if (i < numState && i == j)
				{
					dependMatrixJ[i] = 1;
				}
				else
				{
					dependMatrixJ[i] = a1[i][j];
				}
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 2)
	{
		// 第3段
		numRow = 11;
		numCol = 12;
		numState = setup->dimension->phase[iphase]->state;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				if (i < numState && i == j)
				{
					dependMatrixJ[i] = 1;
				}
				else
				{
					dependMatrixJ[i] = a2[i][j];
				}
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 3)
	{
		// 第4段
		numRow = 11;
		numCol = 10;
		numState = setup->dimension->phase[iphase]->state;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				if (i < numState && i == j)
				{
					dependMatrixJ[i] = 1;
				}
				else
				{
					dependMatrixJ[i] = a3[i][j];
				}
			};
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 4)
	{
		// 第5段
		numRow = 11;
		numCol = 12;
		numState = setup->dimension->phase[iphase]->state;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				if (i < numState && i == j)
				{
					dependMatrixJ[i] = 1;
				}
				else
				{
					dependMatrixJ[i] = a4[i][j];
				}
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
}

void getEndpDepend(intDemat* dependMatrix, int iphase, cmscp_setup* setup)
{

	int a1[3][20] = {
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0},
	};

	int a2[5][20] = {
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
	};

	int a3[4][20] = {
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0},
	};


	int a4[4][19] = {
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0},
		{0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0}
	};

	int a5[8][21] = {
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
		{0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	};
	int i, j, numRow, numCol, numPhase, * dependMatrixJ;
	numPhase = 5;
	if (iphase == 0)
	{
		// 第1段
		numRow = 3;
		numCol = 20;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				dependMatrixJ[i] = a1[i][j];
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 1)
	{
		// 第2段
		numRow = 5;
		numCol = 20;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				dependMatrixJ[i] = a2[i][j];
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 2)
	{
		// 第3段
		numRow = 4;
		numCol = 20;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				dependMatrixJ[i] = a3[i][j];
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 3)
	{
		// 第4段
		numRow = 4;
		numCol = 19;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				dependMatrixJ[i] = a4[i][j];
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 4)
	{
		// 第5段
		numRow = 8;
		numCol = 21;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				dependMatrixJ[i] = a5[i][j];
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
}


void getLinkDepend(intDemat* dependMatrix, int iphase, cmscp_setup* setup)
{
	int a1[8][22] = {
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0},
	};

	int a2[8][22] = {
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0},
	};

	int a3[7][21] = {
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0},
	};

	int a4[8][22] = {
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0},
	};
	int i, j, numRow, numCol, numPhase, * dependMatrixJ;
	numPhase = 5;
	if (iphase == 0)
	{
		// 第1段
		numRow = 8;
		numCol = 22;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				dependMatrixJ[i] = a1[i][j];
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 1)
	{
		// 第2段
		numRow = 8;
		numCol = 22;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				dependMatrixJ[i] = a2[i][j];
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 2)
	{
		// 第3段
		numRow = 7;
		numCol = 21;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				dependMatrixJ[i] = a3[i][j];
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
	else if (iphase == 3)
	{
		// 第4段
		numRow = 8;
		numCol = 22;
		dependMatrix->m = numRow;
		dependMatrix->n = numCol;
		dependMatrix->v = (int*)malloc(numRow * numCol * sizeof(int));
		dependMatrixJ = dependMatrix->v;
		for (j = 0; j < numCol; j++)
		{
			for (i = 0; i < numRow; i++)
			{
				dependMatrixJ[i] = a4[i][j];
			}
			dependMatrixJ = dependMatrixJ + numRow;
		}
	}
}