Convex_Optimization_VS/CMSCP2/cmscp_socpInfo.c

#include "cmscp_socpInfo.h"
#include "freeSocpInfo.h"


void socpInfoSparseDim(cmscp_socpInfo** socpInfo0, cmscp_consTypeInfo* consTypeInfo, cmscp_sparseInfo* sparseInfo, cmscp_setup* setup)
{
	// 函数功能：获取二阶锥规划问题的参数的维数以及分配相应的空间
	// 输入：
	// socpInfo0：无维数和未分配二阶锥规划问题参数信息
	// consTypeInfo：约束类型信息
	// sparseInfo：稀疏（非零元位置）信息
	// setup：问题设置参数
	// 输出：
	// socpInfo0：有维数和已分配二阶锥规划问题参数信息
	// 假设函数的自变量按照[x, u, p]排序

	// 假设优化变量的排列顺序为
	// [{x1}, { u1 }, t01, tf1, p1, Tr1, .., { xi }, { ui }, t0i, tfi, pi, Tri, ..., { xN }, { uN }, t0N, tfN, pN, TrN];
	// 其中{ xi } = { xi_1,...,xi_j,...,xi_n }, xi_j表示第i段的第j个状态向量
	// { ui } = { ui_1,...,ui_j,...,ui_n }, xi_j表示第i段的第j个控制输入
	// t0i, tfi表示第i段初始和终端时刻
	// pi表示第i段参数变量
	// Tri表示软信赖域边界
	// 谢磊：2022 / 6 / 27编写

	// 变量声明
	int numPhase, numOptVar, * numPhaseOptVar, iphase, numMesh, numState, numControl, numParameter, numRowAEqua, numNzAEqua, numRowAIneq, numNzAIneq, numRowASoc, numNzASoc, numSubASoc, numNlPath, numSubSocPath, * dimSocPath, * contStateNzPtrRow, * contControlNzPtrRow, * contParameterNzPtrRow, numNzPhaseAEqua, numRowPhaseAIneq, numRowPhaseAEqua, numNzPhaseAIneq, iType, numType, * idxType, cnt, idxRow, numPhaseSubASoc, numRowPointASoc, numRowPhaseASoc, idxStartRowSoc, idxEndRowSoc, numNzPhaseASoc, numNlEndp, numSubSocEndp, * dimSocEndp, * endpIniStateNzPtrRow, * endpFinStateNzPtrRow, * endpIniTimeNzPtrRow, * endpFinTimeNzPtrRow, * endpParameterNzPtrRow, numNlLink, numSubSocLink, * dimSocLink, * linkLeftStatefNzPtrRow, * linkLeftTfNzPtrRow, * linkLeftParameterNzPtrRow, * linkRightState0NzPtrRow, * linkRightT0NzPtrRow, * linkRightParameterNzPtrRow, * objEventIniStateNzPos, * objEventFinStateNzPos, * objEventIniTimeNzPos, * objEventFinTimeNzPos, * objEventParameterNzPos;
	double* c;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_nzPos_cont** contNzPos;
	cmscp_nzPos_endp** objEventNzPos;
	cmscp_nzPos_endp** endpNzPos;
	cmscp_nzPos_link** linkNzPos;
	cmscp_consType** endpConsType;
	cmscp_consType** pathConsType;
	cmscp_consType_bound* boundConsType;
	cmscp_consType** linkConsType;
	cmscp_consType* eventConsType;
	cmscp_consType** boundIniTimeType;
	cmscp_consType** boundFinTimeType;
	cmscp_consType** boundIniStateType;
	cmscp_consType** boundFinStateType;
	cmscp_consType** boundStateType;
	cmscp_consType** boundControlType;
	cmscp_consType** boundParameterType;

	cmscp_socpInfo* socpInfo;
	//  1. 基本参数
	dimension = setup->dimension;	// 问题维数信息
	mesh = setup->mesh;	// 网格信息
	numPhase = setup->numPhase;	// 阶段数

	// 2. 优化变量的个数
	numOptVar = 0;
	numPhaseOptVar = (int*)malloc(numPhase * sizeof(int));
	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;
		numPhaseOptVar[iphase] = numMesh * (numState + numControl) + numParameter + 3; // 保留软信赖域边
		numOptVar = numOptVar + numPhaseOptVar[iphase];
	}

	// 3. 目标函数系数
	c = (double*)malloc(numOptVar * sizeof(double));

	// 4. 统计三类矩阵的非零元个数和行数：不等式约束矩阵AIneq、等式约束矩阵AEqua和二阶锥约束矩阵ASoc
	numRowAEqua = 0;// 等式约束矩阵的行数
	numNzAEqua = 0;// 等式约束矩阵的非零个数
	numRowAIneq = 0;// 不等式约束矩阵的行数
	numNzAIneq = 0;// 不等式约束矩阵的非零个数
	numRowASoc = 0;// 二阶锥矩阵的行数
	numNzASoc = 0; // 二阶锥约束矩阵的非零元
	numSubASoc = 0;// 二阶锥矩阵中子二阶锥的个数
	contNzPos = sparseInfo->contNzPos;// 连续函数雅可比稀疏信息
	endpNzPos = sparseInfo->endpNzPos;// 端点函数雅可比稀疏信息
	objEventNzPos = sparseInfo->objEventNzPos;// 目标函数雅可比稀疏信息
	linkNzPos = sparseInfo->linkNzPos;// 衔接函数雅可比稀疏信息
	// 约束类型
	endpConsType = consTypeInfo->endp;
	linkConsType = consTypeInfo->link;
	pathConsType = consTypeInfo->path;
	boundConsType = consTypeInfo->bound;
	boundIniTimeType = boundConsType->iniTime;
	boundFinTimeType = boundConsType->finTime;
	boundIniStateType = boundConsType->iniState;
	boundFinStateType = boundConsType->finState;
	boundStateType = boundConsType->state;
	boundControlType = boundConsType->control;
	boundParameterType = boundConsType->parameter;
	eventConsType = consTypeInfo->event;

	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;

		// 连续函数部分
		// 约束维度信息
		numNlPath = dimension->phase[iphase]->path->nl;
		numSubSocPath = dimension->phase[iphase]->path->soc->n;
		dimSocPath = dimension->phase[iphase]->path->soc->q;

		// 连续函数雅可比矩阵稀疏信息
		contStateNzPtrRow = contNzPos[iphase]->state->ptrRow;
		contControlNzPtrRow = contNzPos[iphase]->control->ptrRow;
		contParameterNzPtrRow = contNzPos[iphase]->parameter->ptrRow;

		// 动力学方程部分
		numNzPhaseAEqua = 0;
		numNzPhaseAEqua = numNzPhaseAEqua + 2 * contStateNzPtrRow[numState];// 状态变量部分
		numNzPhaseAEqua = numNzPhaseAEqua + 2 * contControlNzPtrRow[numState];// 控制变量部分
		numNzPhaseAEqua = numNzPhaseAEqua + contParameterNzPtrRow[numState];// 静态参数部分
		numNzPhaseAEqua = numNzPhaseAEqua + numState * 2;// 初始和终端时刻部分
		numNzPhaseAEqua = (numMesh - 1) * numNzPhaseAEqua;
		numNzAEqua = numNzAEqua + numNzPhaseAEqua;
		numRowAEqua = numRowAEqua + (numMesh - 1) * numState;

		// 路径约束部分
		// 不等式和等式部分
		numRowPhaseAIneq = 0;// 各段不等式约束的行数
		numRowPhaseAEqua = 0;// 各段等式约束的行数
		numNzPhaseAIneq = 0;// 各段不等式约束的非零元
		numNzPhaseAEqua = 0;// 各段等式约束的非零元

		for (iType = 0; iType < 4; iType++)
		{
			numType = pathConsType[iphase]->type[iType].num;
			idxType = pathConsType[iphase]->type[iType].idx;
			if (numType)
			{
				if (iType == 0)
				{
					numRowPhaseAIneq = numRowPhaseAIneq + 2 * numType;
					for (cnt = 0; cnt < numType; cnt++)
					{
						idxRow = idxType[cnt] + numState; // 在连续函数雅可比矩阵中行位置
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (contStateNzPtrRow[idxRow + 1] - contStateNzPtrRow[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (contControlNzPtrRow[idxRow + 1] - contControlNzPtrRow[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (contParameterNzPtrRow[idxRow + 1] - contParameterNzPtrRow[idxRow]);
					}
				}
				else if (iType == 1)
				{
					numRowPhaseAEqua = numRowPhaseAEqua + numType;
					for (cnt = 0; cnt < numType; cnt++)
					{
						idxRow = idxType[cnt] + numState;
						numNzPhaseAEqua = numNzPhaseAEqua + contStateNzPtrRow[idxRow + 1] - contStateNzPtrRow[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + contControlNzPtrRow[idxRow + 1] - contControlNzPtrRow[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + contParameterNzPtrRow[idxRow + 1] - contParameterNzPtrRow[idxRow];
					}
				}
				else
				{
					numRowPhaseAIneq = numRowPhaseAIneq + numType;
					for (cnt = 0; cnt < numType; cnt++)
					{
						idxRow = idxType[cnt] + numState;
						numNzPhaseAIneq = numNzPhaseAIneq + contStateNzPtrRow[idxRow + 1] - contStateNzPtrRow[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + contControlNzPtrRow[idxRow + 1] - contControlNzPtrRow[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + contParameterNzPtrRow[idxRow + 1] - contParameterNzPtrRow[idxRow];
					}
				}
			}
		}
		numRowPhaseAIneq = numMesh * numRowPhaseAIneq;
		numRowPhaseAEqua = numMesh * numRowPhaseAEqua;
		numRowAIneq = numRowAIneq + numRowPhaseAIneq;
		numRowAEqua = numRowAEqua + numRowPhaseAEqua;
		numNzPhaseAIneq = numMesh * numNzPhaseAIneq;
		numNzPhaseAEqua = numMesh * numNzPhaseAEqua;
		numNzAIneq = numNzAIneq + numNzPhaseAIneq;
		numNzAEqua = numNzAEqua + numNzPhaseAEqua;
		if (numSubSocPath)
		{
			// 二阶锥个数
			numPhaseSubASoc = numMesh * numSubSocPath;// 各段二阶锥矩阵个数
			numSubASoc = numSubASoc + numPhaseSubASoc;// 总二阶锥个数
			// 二阶锥矩阵行数
			numRowPointASoc = cmscp_sum(dimSocPath, numSubSocPath);// 单个离散点处的二阶锥矩阵行数
			numRowPhaseASoc = numMesh * numRowPointASoc;// 各段二阶锥矩阵行数
			numRowASoc = numRowASoc + numMesh * numRowPhaseASoc;// 总二阶锥矩阵行数
			// 单段二阶锥矩阵的非零元个数
			idxStartRowSoc = numState + numNlPath;// 二阶锥矩阵在连续函数雅可比矩阵中的起始行
			idxEndRowSoc = numState + numNlPath + numRowPointASoc;// 二阶锥矩阵在连续函数雅可比矩阵中的结束行
			numNzPhaseASoc = numNzPhaseASoc + contStateNzPtrRow[idxEndRowSoc + 1] - contStateNzPtrRow[idxStartRowSoc + 1];
			numNzPhaseASoc = numNzPhaseASoc + contControlNzPtrRow[idxEndRowSoc + 1] - contControlNzPtrRow[idxStartRowSoc + 1];
			numNzPhaseASoc = numNzPhaseASoc + contParameterNzPtrRow[idxEndRowSoc + 1] - contParameterNzPtrRow[idxStartRowSoc + 1];
			numNzPhaseASoc = numMesh * numNzPhaseASoc;
			// 总二阶锥矩阵非零元个数
			numNzASoc = numNzASoc + numNzPhaseASoc;
		}

		//// 端点函数部分
		// 约束维度信息
		numNlEndp = dimension->phase[iphase]->endpoint->nl;
		numSubSocEndp = dimension->phase[iphase]->endpoint->soc->n;
		dimSocEndp = dimension->phase[iphase]->endpoint->soc->q;
		// 端点函数雅可比矩阵稀疏信息
		endpIniStateNzPtrRow = endpNzPos[iphase]->iniState->ptrRow;
		endpFinStateNzPtrRow = endpNzPos[iphase]->finState->ptrRow;
		endpIniTimeNzPtrRow = endpNzPos[iphase]->iniTime->ptrRow;
		endpFinTimeNzPtrRow = endpNzPos[iphase]->finTime->ptrRow;
		endpParameterNzPtrRow = endpNzPos[iphase]->parameter->ptrRow;
		// 不等式和等式部分
		numRowPhaseAIneq = 0;// 各段不等式约束的行数
		numRowPhaseAEqua = 0;// 各段等式约束的行数
		numNzPhaseAIneq = 0;// 各段不等式约束的非零元
		numNzPhaseAEqua = 0;// 各段等式约束的非零元
		for (iType = 0; iType < 4; iType++)
		{
			numType = endpConsType[iphase]->type[iType].num;
			idxType = endpConsType[iphase]->type[iType].idx;
			if (numType)
			{
				if (iType == 0)
				{
					numRowPhaseAIneq = numRowPhaseAIneq + 2 * numType;
					for (cnt = 0; cnt < numType; cnt++)
					{
						idxRow = idxType[cnt];
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (endpIniStateNzPtrRow[idxRow + 1] - endpIniStateNzPtrRow[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (endpFinStateNzPtrRow[idxRow + 1] - endpFinStateNzPtrRow[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (endpIniTimeNzPtrRow[idxRow + 1] - endpIniTimeNzPtrRow[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (endpFinTimeNzPtrRow[idxRow + 1] - endpFinTimeNzPtrRow[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (endpParameterNzPtrRow[idxRow + 1] - endpParameterNzPtrRow[idxRow]);
					}
				}
				else if (iType == 1)
				{
					numRowPhaseAEqua = numRowPhaseAEqua + numType;
					for (cnt = 0; cnt < numType; cnt++)
					{
						idxRow = idxType[cnt];
						numNzPhaseAEqua = numNzPhaseAEqua + endpIniStateNzPtrRow[idxRow + 1] - endpIniStateNzPtrRow[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + endpFinStateNzPtrRow[idxRow + 1] - endpFinStateNzPtrRow[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + endpIniTimeNzPtrRow[idxRow + 1] - endpIniTimeNzPtrRow[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + endpFinTimeNzPtrRow[idxRow + 1] - endpFinTimeNzPtrRow[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + endpParameterNzPtrRow[idxRow + 1] - endpParameterNzPtrRow[idxRow];
					}
				}
				else
				{
					numRowPhaseAIneq = numRowPhaseAIneq + numType;
					for (cnt = 0; cnt < numType; cnt++)
					{
						idxRow = idxType[cnt];
						numNzPhaseAIneq = numNzPhaseAIneq + endpIniStateNzPtrRow[idxRow + 1] - endpIniStateNzPtrRow[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + endpFinStateNzPtrRow[idxRow + 1] - endpFinStateNzPtrRow[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + endpIniTimeNzPtrRow[idxRow + 1] - endpIniTimeNzPtrRow[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + endpFinTimeNzPtrRow[idxRow + 1] - endpFinTimeNzPtrRow[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + endpParameterNzPtrRow[idxRow + 1] - endpParameterNzPtrRow[idxRow];
					}
				}
			}
		}
		numRowAIneq = numRowAIneq + numRowPhaseAIneq;
		numRowAEqua = numRowAEqua + numRowPhaseAEqua;
		numNzAIneq = numNzAIneq + numNzPhaseAIneq;
		numNzAEqua = numNzAEqua + numNzPhaseAEqua;
		// 二阶锥约束部分
		if (numSubSocEndp)
		{
			// 二阶锥个数
			numSubASoc = numSubASoc + numSubSocEndp; // 总二阶锥个数, 每段就一个端点约束函数
			// 二阶锥矩阵行数
			numRowPhaseASoc = cmscp_sum(dimSocEndp, numSubSocEndp); // 各段二阶锥矩阵行数
			numRowASoc = numRowASoc + numRowPhaseASoc; // 总二阶锥矩阵行数
			// 单段矩阵的非零元个数
			idxStartRowSoc = numNlEndp; // 二阶锥矩阵在连续函数雅可比矩阵中的起始行
			idxEndRowSoc = numNlEndp + numRowPhaseASoc; // 二阶锥矩阵在连续函数雅可比矩阵中的结束行
			numNzPhaseASoc = 0;
			numNzPhaseASoc = numNzPhaseASoc + endpIniStateNzPtrRow[idxEndRowSoc] - endpIniStateNzPtrRow[idxStartRowSoc];
			numNzPhaseASoc = numNzPhaseASoc + endpFinStateNzPtrRow[idxEndRowSoc] - endpFinStateNzPtrRow[idxStartRowSoc];
			numNzPhaseASoc = numNzPhaseASoc + endpIniTimeNzPtrRow[idxEndRowSoc] - endpIniTimeNzPtrRow[idxStartRowSoc];
			numNzPhaseASoc = numNzPhaseASoc + endpFinTimeNzPtrRow[idxEndRowSoc] - endpFinTimeNzPtrRow[idxStartRowSoc];
			numNzPhaseASoc = numNzPhaseASoc + endpParameterNzPtrRow[idxEndRowSoc] - endpParameterNzPtrRow[idxStartRowSoc];
			// 总二阶锥矩阵非零元个数
			numNzASoc = numNzASoc + numNzPhaseASoc;
		}

		if (iphase < numPhase - 1)
		{
			// 约束维数信息
			numNlLink = dimension->phase[iphase]->linkage->nl;// 非线性部分维数
			numSubSocLink = dimension->phase[iphase]->linkage->soc->n;
			dimSocLink = dimension->phase[iphase]->linkage->soc->q;
			// 稀疏信息
			linkLeftStatefNzPtrRow = linkNzPos[iphase]->leftStatef->ptrRow;
			linkLeftTfNzPtrRow = linkNzPos[iphase]->leftTf->ptrRow;
			linkLeftParameterNzPtrRow = linkNzPos[iphase]->leftParameter->ptrRow;
			linkRightState0NzPtrRow = linkNzPos[iphase]->rightState0->ptrRow;
			linkRightT0NzPtrRow = linkNzPos[iphase]->rightT0->ptrRow;
			linkRightParameterNzPtrRow = linkNzPos[iphase]->rightParameter->ptrRow;
			// 统计各类非线性约束矩阵的行数
			numRowPhaseAIneq = 0;// 各段不等式约束的行数
			numRowPhaseAEqua = 0;// 各段等式约束的行数
			numNzPhaseAIneq = 0;// 各段不等式约束的非零元
			numNzPhaseAEqua = 0;// 各段等式约束的非零元
			for (iType = 0; iType < 4; iType++)
			{
				numType = linkConsType[iphase]->type[iType].num;
				idxType = linkConsType[iphase]->type[iType].idx;
				if (numType)
				{
					if (iType == 0)
					{
						numRowPhaseAIneq = numRowPhaseAIneq + 2 * numType;
						for (cnt = 0; cnt < numType; cnt++)
						{
							idxRow = idxType[cnt];
							numNzPhaseAIneq = numNzPhaseAIneq + 2 * (linkLeftStatefNzPtrRow[idxRow + 1] - linkLeftStatefNzPtrRow[idxRow]);
							numNzPhaseAIneq = numNzPhaseAIneq + 2 * (linkLeftTfNzPtrRow[idxRow + 1] - linkLeftTfNzPtrRow[idxRow]);
							numNzPhaseAIneq = numNzPhaseAIneq + 2 * (linkLeftParameterNzPtrRow[idxRow + 1] - linkLeftParameterNzPtrRow[idxRow]);
							numNzPhaseAIneq = numNzPhaseAIneq + 2 * (linkRightState0NzPtrRow[idxRow + 1] - linkRightState0NzPtrRow[idxRow]);
							numNzPhaseAIneq = numNzPhaseAIneq + 2 * (linkRightT0NzPtrRow[idxRow + 1] - linkRightT0NzPtrRow[idxRow]);
							numNzPhaseAIneq = numNzPhaseAIneq + 2 * (linkRightParameterNzPtrRow[idxRow + 1] - linkRightParameterNzPtrRow[idxRow]);
						}
					}
					else if (iType == 1)
					{
						numRowPhaseAEqua = numRowPhaseAEqua + numType;
						for (cnt = 0; cnt < numType; cnt++)
						{
							idxRow = idxType[cnt];
							numNzPhaseAEqua = numNzPhaseAEqua + linkLeftStatefNzPtrRow[idxRow + 1] - linkLeftStatefNzPtrRow[idxRow];
							numNzPhaseAEqua = numNzPhaseAEqua + linkLeftTfNzPtrRow[idxRow + 1] - linkLeftTfNzPtrRow[idxRow];
							numNzPhaseAEqua = numNzPhaseAEqua + linkLeftParameterNzPtrRow[idxRow + 1] - linkLeftParameterNzPtrRow[idxRow];
							numNzPhaseAEqua = numNzPhaseAEqua + linkRightState0NzPtrRow[idxRow + 1] - linkRightState0NzPtrRow[idxRow];
							numNzPhaseAEqua = numNzPhaseAEqua + linkRightT0NzPtrRow[idxRow + 1] - linkRightT0NzPtrRow[idxRow];
							numNzPhaseAEqua = numNzPhaseAEqua + linkRightParameterNzPtrRow[idxRow + 1] - linkRightParameterNzPtrRow[idxRow];
						}
					}
					else
					{
						numRowPhaseAIneq = numRowPhaseAIneq + numType;
						for (cnt = 0; cnt < numType; cnt++)
						{
							idxRow = idxType[cnt];
							numNzPhaseAIneq = numNzPhaseAIneq + linkLeftStatefNzPtrRow[idxRow + 1] - linkLeftStatefNzPtrRow[idxRow];
							numNzPhaseAIneq = numNzPhaseAIneq + linkLeftTfNzPtrRow[idxRow + 1] - linkLeftTfNzPtrRow[idxRow];
							numNzPhaseAIneq = numNzPhaseAIneq + linkLeftParameterNzPtrRow[idxRow + 1] - linkLeftParameterNzPtrRow[idxRow];
							numNzPhaseAIneq = numNzPhaseAIneq + linkRightState0NzPtrRow[idxRow + 1] - linkRightState0NzPtrRow[idxRow];
							numNzPhaseAIneq = numNzPhaseAIneq + linkRightT0NzPtrRow[idxRow + 1] - linkRightT0NzPtrRow[idxRow];
							numNzPhaseAIneq = numNzPhaseAIneq + linkRightParameterNzPtrRow[idxRow + 1] - linkRightParameterNzPtrRow[idxRow];
						}
					}
				}
			}
			numRowAIneq = numRowAIneq + numRowPhaseAIneq;
			numRowAEqua = numRowAEqua + numRowPhaseAEqua;
			numNzAIneq = numNzAIneq + numNzPhaseAIneq;
			numNzAEqua = numNzAEqua + numNzPhaseAEqua;
			if (numSubSocLink)
			{
				// 二阶锥个数
				numSubASoc = numSubASoc + numSubSocLink;// 总二阶锥个数, 每段就一个端点约束函数
				// 二阶锥矩阵行数
				numRowPhaseASoc = cmscp_sum(dimSocLink, numSubSocLink);// 各段二阶锥矩阵行数
				numRowASoc = numRowASoc + numRowPhaseASoc;// 总二阶锥矩阵行数
				// 单段矩阵的非零元个数
				idxStartRowSoc = numNlLink;// 二阶锥矩阵在连续函数雅可比矩阵中的起始行
				idxEndRowSoc = numNlLink + numRowPhaseASoc;// 二阶锥矩阵在连续函数雅可比矩阵中的结束行
				numNzPhaseASoc = 0;
				numNzPhaseASoc = numNzPhaseASoc + linkLeftStatefNzPtrRow[idxEndRowSoc] - linkLeftStatefNzPtrRow[idxStartRowSoc];
				numNzPhaseASoc = numNzPhaseASoc + linkLeftTfNzPtrRow[idxEndRowSoc] - linkLeftTfNzPtrRow[idxStartRowSoc];
				numNzPhaseASoc = numNzPhaseASoc + linkLeftParameterNzPtrRow[idxEndRowSoc] - linkLeftParameterNzPtrRow[idxStartRowSoc];
				numNzPhaseASoc = numNzPhaseASoc + linkRightState0NzPtrRow[idxEndRowSoc] - linkRightState0NzPtrRow[idxStartRowSoc];
				numNzPhaseASoc = numNzPhaseASoc + linkRightT0NzPtrRow[idxEndRowSoc] - linkRightT0NzPtrRow[idxStartRowSoc];
				numNzPhaseASoc = numNzPhaseASoc + linkRightParameterNzPtrRow[idxEndRowSoc] - linkRightParameterNzPtrRow[idxStartRowSoc];
				// 总二阶锥矩阵非零元个数
				numNzASoc = numNzASoc + numNzPhaseASoc;
			}
		}

		//// 上下边界约束
		// 各段不等式和等式约束的行数也等于非零元个数
		numRowPhaseAEqua = 0;
		numRowPhaseAIneq = 0;
		// 统计行数
		for (iType = 0; iType < 4; iType++)
		{
			// 初始时刻变量
			numType = boundIniTimeType[iphase]->type[iType].num;
			if (numType)
			{
				if (iType == 0)
				{
					numRowPhaseAIneq = numRowPhaseAIneq + 2 * numType;
				}
				else if (iType == 1)
				{
					numRowPhaseAEqua = numRowPhaseAEqua + numType;
				}
				else
				{
					numRowPhaseAIneq = numRowPhaseAIneq + numType;
				}
			}
			// 终端时刻变量
			numType = boundFinTimeType[iphase]->type[iType].num;
			if (numType)
			{
				if (iType == 0)
				{
					numRowPhaseAIneq = numRowPhaseAIneq + 2 * numType;
				}
				else if (iType == 1)
				{
					numRowPhaseAEqua = numRowPhaseAEqua + numType;
				}
				else
				{
					numRowPhaseAIneq = numRowPhaseAIneq + numType;
				}
			}
			// 初始状态变量
			numType = boundIniStateType[iphase]->type[iType].num;
			if (numType)
			{
				if (iType == 0)
				{
					numRowPhaseAIneq = numRowPhaseAIneq + 2 * numType;
				}
				else if (iType == 1)
				{
					numRowPhaseAEqua = numRowPhaseAEqua + numType;
				}
				else
				{
					numRowPhaseAIneq = numRowPhaseAIneq + numType;
				}
			}
			// 终端状态变量
			numType = boundFinStateType[iphase]->type[iType].num;
			if (numType)
			{
				if (iType == 0)
				{
					numRowPhaseAIneq = numRowPhaseAIneq + 2 * numType;
				}
				else if (iType == 1)
				{
					numRowPhaseAEqua = numRowPhaseAEqua + numType;
				}
				else
				{
					numRowPhaseAIneq = numRowPhaseAIneq + numType;
				}
			}
			// 过程状态变量
			numType = boundStateType[iphase]->type[iType].num;
			if (numType)
			{
				if (iType == 0)
				{
					numRowPhaseAIneq = numRowPhaseAIneq + 2 * (numMesh - 2) * numType;
				}
				else if (iType == 1)
				{
					numRowPhaseAEqua = numRowPhaseAEqua + (numMesh - 2) * numType;
				}
				else
				{
					numRowPhaseAIneq = numRowPhaseAIneq + (numMesh - 2) * numType;
				}
			}
			// 控制变量
			numType = boundControlType[iphase]->type[iType].num;
			if (numType)
			{
				if (iType == 0)
				{
					numRowPhaseAIneq = numRowPhaseAIneq + 2 * numMesh * numType;
				}
				else if (iType == 1)
				{
					numRowPhaseAEqua = numRowPhaseAEqua + numMesh * numType;
				}
				else
				{
					numRowPhaseAIneq = numRowPhaseAIneq + numMesh * numType;
				}
			}
			// 静态参数变量
			numType = boundParameterType[iphase]->type[iType].num;
			if (numType)
			{
				if (iType == 0)
				{
					numRowPhaseAIneq = numRowPhaseAIneq + 2 * numType;
				}
				else if (iType == 1)
				{
					numRowPhaseAEqua = numRowPhaseAEqua + numType;
				}
				else
				{
					numRowPhaseAIneq = numRowPhaseAIneq + numType;
				}
			}
		}
		numRowAIneq = numRowAIneq + numRowPhaseAIneq;
		numRowAEqua = numRowAEqua + numRowPhaseAEqua;
		numNzAIneq = numNzAIneq + numRowPhaseAIneq;
		numNzAEqua = numNzAEqua + numRowPhaseAEqua;

		//// 信赖域约束
		// 二阶锥约束个数
		// 状态：numMesh、控制：numMesh、初始时刻：1、终端时刻：1、静态参数：1
		numSubASoc = numSubASoc + numMesh * 2 + 3;
		// 二阶锥约束矩阵行数和非零元个数相等
		// 每个锥的维数比对应变量多1
		numRowPhaseASoc = numMesh * (numState + numControl + 2) + numParameter + 5;
		numRowASoc = numRowASoc + numRowPhaseASoc;
		numNzASoc = numNzASoc + numRowPhaseASoc;
	}

	// 事件约束
	for (iType = 0; iType < 4; iType++)
	{
		numType = eventConsType->type[iType].num;
		idxType = eventConsType->type[iType].idx;
		if (numType)
		{
			if (iType == 0)
			{
				for (cnt = 0; cnt < numType; cnt++)
				{
					// 统计非零元素个数
					// 第idxType(cnt)个event约束被分为numPhase段
					idxRow = idxType[cnt] + 1; // 加偏置1，因为稀疏信息是objEvent的，第一行是目标函数obj的
					for (iphase = 0; iphase < numPhase; iphase++)
					{
						// 稀疏信息
						objEventIniStateNzPos = objEventNzPos[iphase]->iniState->ptrRow;
						objEventFinStateNzPos = objEventNzPos[iphase]->finState->ptrRow;
						objEventIniTimeNzPos = objEventNzPos[iphase]->iniTime->ptrRow;
						objEventFinTimeNzPos = objEventNzPos[iphase]->finTime->ptrRow;
						objEventParameterNzPos = objEventNzPos[iphase]->parameter->ptrRow;
						// 每段非零元个数
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (objEventIniStateNzPos[idxRow + 1] - objEventIniStateNzPos[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (objEventFinStateNzPos[idxRow + 1] - objEventFinStateNzPos[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (objEventIniTimeNzPos[idxRow + 1] - objEventIniTimeNzPos[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (objEventFinTimeNzPos[idxRow + 1] - objEventFinTimeNzPos[idxRow]);
						numNzPhaseAIneq = numNzPhaseAIneq + 2 * (objEventParameterNzPos[idxRow + 1] - objEventParameterNzPos[idxRow]);
					}
					numRowAIneq = numRowAIneq + 1;
				}

			}
			else if (iType == 1)
			{
				for (cnt = 0; cnt < numType; cnt++)
				{
					// 统计非零元素个数
					// 第idxType(cnt)个event约束被分为numPhase段
					idxRow = idxType[cnt] + 1; // 加偏置1，因为稀疏信息是objEvent的，第一行是目标函数obj的
					for (iphase = 0; iphase < numPhase; iphase++)
					{
						// 稀疏信息
						objEventIniStateNzPos = objEventNzPos[iphase]->iniState->ptrRow;
						objEventFinStateNzPos = objEventNzPos[iphase]->finState->ptrRow;
						objEventIniTimeNzPos = objEventNzPos[iphase]->iniTime->ptrRow;
						objEventFinTimeNzPos = objEventNzPos[iphase]->finTime->ptrRow;
						objEventParameterNzPos = objEventNzPos[iphase]->parameter->ptrRow;
						// 每段非零元个数
						numNzPhaseAEqua = numNzPhaseAEqua + objEventIniStateNzPos[idxRow + 1] - objEventIniStateNzPos[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + objEventFinStateNzPos[idxRow + 1] - objEventFinStateNzPos[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + objEventIniTimeNzPos[idxRow + 1] - objEventIniTimeNzPos[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + objEventFinTimeNzPos[idxRow + 1] - objEventFinTimeNzPos[idxRow];
						numNzPhaseAEqua = numNzPhaseAEqua + objEventParameterNzPos[idxRow + 1] - objEventParameterNzPos[idxRow];
					}
					numRowAEqua = numRowAEqua + 1;
				}
			}
			else
			{
				for (cnt = 0; cnt < numType; cnt++)
				{
					// 统计非零元素个数
					// 第idxType(cnt)个event约束被分为numPhase段
					idxRow = idxType[cnt] + 1; // 加偏置1，因为稀疏信息是objEvent的，第一行是目标函数obj的
					for (iphase = 0; iphase < numPhase; iphase++)
					{
						// 稀疏信息
						objEventIniStateNzPos = objEventNzPos[iphase]->iniState->ptrRow;
						objEventFinStateNzPos = objEventNzPos[iphase]->finState->ptrRow;
						objEventIniTimeNzPos = objEventNzPos[iphase]->iniTime->ptrRow;
						objEventFinTimeNzPos = objEventNzPos[iphase]->finTime->ptrRow;
						objEventParameterNzPos = objEventNzPos[iphase]->parameter->ptrRow;
						// 每段非零元个数
						numNzPhaseAIneq = numNzPhaseAIneq + objEventIniStateNzPos[idxRow + 1] - objEventIniStateNzPos[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + objEventFinStateNzPos[idxRow + 1] - objEventFinStateNzPos[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + objEventIniTimeNzPos[idxRow + 1] - objEventIniTimeNzPos[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + objEventFinTimeNzPos[idxRow + 1] - objEventFinTimeNzPos[idxRow];
						numNzPhaseAIneq = numNzPhaseAIneq + objEventParameterNzPos[idxRow + 1] - objEventParameterNzPos[idxRow];
					}
					numRowAIneq = numRowAIneq + 1;
				}
			}
		}
	}

	// 分配约束相关空间
	socpInfo = (cmscp_socpInfo*)malloc(sizeof(cmscp_socpInfo));
	socpInfo->bEqua = (double*)malloc(numRowAEqua * sizeof(double));
	socpInfo->bCone = (double*)malloc((numRowAIneq + numRowASoc) * sizeof(double));
	socpInfo->AEqua = (cmscp_crsSpmat*)malloc(sizeof(cmscp_crsSpmat));
	socpInfo->ACone = (cmscp_crsSpmat*)malloc(sizeof(cmscp_crsSpmat));
	socpInfo->AEqua->ptrRow = (int*)malloc((numRowAEqua + 1) * sizeof(int));
	socpInfo->AEqua->ptrRow[numRowAEqua] = numNzAEqua;
	socpInfo->AEqua->idxCol = (int*)malloc(numNzAEqua * sizeof(int));
	socpInfo->AEqua->v = (double*)malloc(numNzAEqua * sizeof(double));
	socpInfo->AEqua->m = numRowAEqua;
	socpInfo->AEqua->n = numOptVar;
	socpInfo->ACone->ptrRow = (int*)malloc((numRowAIneq + numRowASoc + 1) * sizeof(int));
	socpInfo->ACone->ptrRow[numRowAIneq + numRowASoc] = numNzAIneq + numNzASoc;
	socpInfo->ACone->idxCol = (int*)malloc((numNzAIneq + numNzASoc) * sizeof(int));
	socpInfo->ACone->v = (double*)malloc((numNzAIneq + numNzASoc) * sizeof(double));
	socpInfo->ACone->m = numRowAIneq + numRowASoc;
	socpInfo->ACone->n = numOptVar;

	socpInfo->AEqua1 = (cmscp_ccsSpmat*)malloc(sizeof(cmscp_ccsSpmat));
	socpInfo->ACone1 = (cmscp_ccsSpmat*)malloc(sizeof(cmscp_ccsSpmat));
	socpInfo->AEqua1->idxRow = (int*)malloc(numNzAEqua * sizeof(int));
	socpInfo->AEqua1->ptrCol = (int*)malloc((numOptVar+1) * sizeof(int));
	socpInfo->AEqua1->ptrCol[numOptVar] = numNzAEqua;
	socpInfo->AEqua1->v = (double*)malloc(numNzAEqua * sizeof(double));
	socpInfo->AEqua1->m = numRowAEqua;
	socpInfo->AEqua1->n = numOptVar;
	socpInfo->ACone1->idxRow = (int*)malloc((numNzAIneq + numNzASoc) * sizeof(int));
	socpInfo->ACone1->ptrCol = (int*)malloc( (numOptVar + 1) * sizeof(int));
	socpInfo->ACone1->ptrCol[numOptVar] = numNzAIneq + numNzASoc;
	socpInfo->ACone1->v = (double*)malloc((numNzAIneq + numNzASoc) * sizeof(double));
	socpInfo->ACone1->m = numRowAIneq + numRowASoc;
	socpInfo->ACone1->n = numOptVar;

	socpInfo->dimCone = (cmscp_dimCone*)malloc(sizeof(cmscp_dimCone));
	socpInfo->dimCone->l = numRowAIneq;
	socpInfo->dimCone->nsoc = numSubASoc;
	socpInfo->dimCone->q = (int*)malloc(numSubASoc * sizeof(int));

	socpInfo->numOptVar = numOptVar;
	socpInfo->numPhaseOptVar = numPhaseOptVar;

	socpInfo->c = c;

	socpInfo->cntNzAEqua = 0;
	socpInfo->cntRowAEqua = 0;
	socpInfo->cntNzAIneq = 0;
	socpInfo->cntRowAIneq = 0;
	socpInfo->cntNzASoc = numNzAIneq;
	socpInfo->cntRowASoc = numRowAIneq;
	socpInfo->cntSubASoc = 0;

	// 输出
	*socpInfo0 = socpInfo;
}



void socpInfoSparseObjEvent(cmscp_socpInfo** socpInfo0, cmscp_trajInfo** refTraj, cmscp_sparseInfo* sparseInfo, cmscp_consTypeInfo* consTypeInfo, cmscp_setup* setup)
{

	// 函数功能：获取与目标函数和事件约束相关的二阶锥问题参数
	// 输入：
	// socpInfo0：无获目标函数和事件约束相关的二阶锥问题参数
	// refTraj：参考轨迹
	// consTypeInfo：约束类型信息
	// sparseInfo：稀疏（非零元位置）信息
	// setup：问题设置参数
	// 输出：
	// socpInfo0：有目标函数和事件约束相关的二阶锥问题参数
	// 假设函数的自变量按照[x, u, p]排序

	// 假设优化变量的排列顺序为
	// [{x1}, { u1 }, t01, tf1, p1, Tr1, .., { xi }, { ui }, t0i, tfi, pi, Tri, ..., { xN }, { uN }, t0N, tfN, pN, TrN];
	// 其中{ xi } = { xi_1,...,xi_j,...,xi_n }, xi_j表示第i段的第j个状态向量
	// { ui } = { ui_1,...,ui_j,...,ui_n }, xi_j表示第i段的第j个控制输入
	// t0i, tfi表示第i段初始和终端时刻
	// pi表示第i段参数变量
	// Tri表示软信赖域边界
	// 谢磊：2022 / 6 / 27编写

	// 变量声明
	int numPhase, numOptVar, * numPhaseOptVar, iphase, numMesh, numState, numControl, numParameter, numRowAEqua, numNzAEqua, numRowAIneq, numNzAIneq, numRowASoc, numNzASoc, numSubASoc, numNlPath, numSubSocPath, * dimSocPath, * contStateNzPtrRow, * contControlNzPtrRow, * contParameterNzPtrRow, numNzPhaseAEqua, numRowPhaseAIneq, numRowPhaseAEqua, numNzPhaseAIneq, iType, numType, * idxType, cnt, idxRow, numPhaseSubASoc, numRowPointASoc, numRowPhaseASoc, idxStartRowSoc, idxEndRowSoc, numNzPhaseASoc, numNlEndp, numSubSocEndp, * dimSocEndp, * endpIniStateNzPtrRow, * endpFinStateNzPtrRow, * endpIniTimeNzPtrRow, * endpFinTimeNzPtrRow, * endpParameterNzPtrRow, numNlLink, numSubSocLink, * dimSocLink, * linkLeftStatefNzPtrRow, * linkLeftTfNzPtrRow, * linkLeftParameterNzPtrRow, * linkRightState0NzPtrRow, * linkRightT0NzPtrRow, * linkRightParameterNzPtrRow, numNlEvent, numSubSocEvent, * dimSocEvent, idxStartRowSocEvent, * objEventIniStateNzPos, * objEventFinStateNzPos, * objEventIniTimeNzPos, * objEventFinTimeNzPos, * ptrRowAEqua, * idxColAEqua, * ptrRowACone, * idxColACone, cntNzAEqua, cntRowAEqua, cntNzAIneq, cntRowAIneq, cntNzASoc, cntRowASoc, cntSubASoc, idxOptVarPhaseStart, cntRowGrd, * iniStateNzPtrRow, * finStateNzPtrRow, * iniTimeNzPtrRow, * finTimeNzPtrRow, * parameterNzPtrRow, * iniStateNzIdxCol, * finStateNzIdxCol, * iniTimeNzIdxCol, * finTimeNzIdxCol, * parameterNzIdxCol, idxOptVarState0, idxOptVarStatef, idxOptVarT0, idxOptVarTf, idxOptVarParameter, idxOptVarTrust, j, nnzType1, i, i0, cntRowAIneqLower, numType1, idxGrdCol, p, numSocEvent, numRowEvent, numRowSocEvent;
	double* c, * valueAEqua, * bEqua, * valueACone, * bCone, w,	* eventNlLower, * eventNlUpper, * refState0, * refStatef, refT0, refTf, * refParameter, temp, * eventGrd, * eventRef, *eventSocUpper;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_bound* bounds;
	cmscp_trust* trust;
	cmscp_consType* consType;
	cmscp_nzPos_endp** objEventNzPos;
	cmscp_derInfo* objDerInfo;
	cmscp_derInfo* eventDerInfo;
	cmscp_socpInfo* socpInfo;
	cmscp_dimCone* dimCone;
	//  1. 基本参数
	dimension = setup->dimension;// 问题维数信息
	mesh = setup->mesh;// 网格信息
	numPhase = setup->numPhase;// 阶段数
	bounds = setup->bounds;// 边界信息
	trust = setup->trust;// 信赖域信息
	socpInfo = (*socpInfo0);
	numPhaseOptVar = socpInfo->numPhaseOptVar;// 每段优化变量个数
	objEventNzPos = sparseInfo->objEventNzPos;// 稀疏信息
	consType = consTypeInfo->event;// 约束类型信息
	// 二阶锥规划参数
	numOptVar = socpInfo->numOptVar;
	c = socpInfo->c;
	ptrRowAEqua = socpInfo->AEqua->ptrRow;
	idxColAEqua = socpInfo->AEqua->idxCol;
	valueAEqua = socpInfo->AEqua->v;
	bEqua = socpInfo->bEqua;
	ptrRowACone = socpInfo->ACone->ptrRow;
	idxColACone = socpInfo->ACone->idxCol;
	valueACone = socpInfo->ACone->v;
	bCone = socpInfo->bCone;
	dimCone = socpInfo->dimCone;
	// 计数变量
	cntNzAEqua = socpInfo->cntNzAEqua;
	cntRowAEqua = socpInfo->cntRowAEqua;
	cntNzAIneq = socpInfo->cntNzAIneq;
	cntRowAIneq = socpInfo->cntRowAIneq;
	cntNzASoc = socpInfo->cntNzASoc;
	cntRowASoc = socpInfo->cntRowASoc;
	cntSubASoc = socpInfo->cntSubASoc;
	 //1.1. 梯度信息
	gradInfoSparseObjEvent(&objDerInfo, &eventDerInfo, sparseInfo, refTraj, setup);
	eventGrd = eventDerInfo->der;
	eventRef = eventDerInfo->ref;
	//// 2->目标函数
	floatVecFillin(c, numOptVar, 0);
	// 索引
	idxOptVarPhaseStart = 0; // 各段起始索引
	cntRowGrd = 0; // 目标函数梯度计数
	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;
		if (strcmp(setup->trust->model, "hard") == 0)
		{
			w = 0;
		}
		else if ((strcmp(setup->trust->model, "soft") == 0))
		{
			w = trust->phase[iphase]->softPenaWeight;
		}
		// 稀疏信息
		iniStateNzPtrRow = objEventNzPos[iphase]->iniState->ptrRow;
		finStateNzPtrRow = objEventNzPos[iphase]->finState->ptrRow;
		iniTimeNzPtrRow = objEventNzPos[iphase]->iniTime->ptrRow;
		finTimeNzPtrRow = objEventNzPos[iphase]->finTime->ptrRow;
		parameterNzPtrRow = objEventNzPos[iphase]->parameter->ptrRow;
		iniStateNzIdxCol = objEventNzPos[iphase]->iniState->idxCol;
		finStateNzIdxCol = objEventNzPos[iphase]->finState->idxCol;
		iniTimeNzIdxCol = objEventNzPos[iphase]->iniTime->idxCol;
		finTimeNzIdxCol = objEventNzPos[iphase]->finTime->idxCol;
		parameterNzIdxCol = objEventNzPos[iphase]->parameter->idxCol;
		// 每段各种所需部分的起始索引
		idxOptVarState0 = idxOptVarPhaseStart;
		idxOptVarStatef = idxOptVarPhaseStart + (numMesh - 1) * numState;
		idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);
		idxOptVarTf = idxOptVarT0 + 1;
		idxOptVarParameter = idxOptVarPhaseStart + numMesh * (numState + numControl) + 2;
		idxOptVarTrust = idxOptVarParameter + numParameter;
		// x0 前面系数
		for (cnt = iniStateNzPtrRow[0]; cnt < iniStateNzPtrRow[1]; cnt++)
		{
			j = iniStateNzIdxCol[cnt];
			c[idxOptVarState0 + j] = objDerInfo->der[cntRowGrd + j];
		}
		cntRowGrd = cntRowGrd + numState;
		// xf 前面系数
		for (cnt = finStateNzPtrRow[0]; cnt < finStateNzPtrRow[1]; cnt++)
		{
			j = finStateNzIdxCol[cnt];
			c[idxOptVarStatef + j] = objDerInfo->der[cntRowGrd + j];
		}
		cntRowGrd = cntRowGrd + numState;
		// t0 前面系数
		for (cnt = iniTimeNzPtrRow[0]; cnt < iniTimeNzPtrRow[1]; cnt++)
		{
			j = iniTimeNzIdxCol[cnt];
			c[idxOptVarT0] = objDerInfo->der[cntRowGrd + j];
		}
		cntRowGrd = cntRowGrd + 1;
		// tf 前面系数
		for (cnt = finTimeNzPtrRow[0]; cnt < finTimeNzPtrRow[1]; cnt++)
		{
			j = finTimeNzIdxCol[cnt];
			c[idxOptVarTf] = objDerInfo->der[cntRowGrd + j];
		}
		cntRowGrd = cntRowGrd + 1;
		// p 前面系数
		for (cnt = parameterNzPtrRow[0]; cnt < parameterNzPtrRow[1]; cnt++)
		{
			j = parameterNzIdxCol[cnt];
			c[idxOptVarParameter + j] = objDerInfo->der[cntRowGrd + j];
		}
		cntRowGrd = cntRowGrd + numParameter;
		// Tr 前面系数
		c[idxOptVarTrust] = w;
		// 更新索引
		idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
	}
	//// 3-> 事件函数
	// 3->1 类型1：双不等式约束项
	// 上界：  A * x0 + B * xf + C * t0 + D * tf + F * p <= b
	// 下界： - A * x0 - B * xf - C * t0 - D * tf - F * p <= b
	iType = 1;
	numType = consType->type[iType].num;
	idxType = consType->type[iType].idx;

	// 维数信息
	numNlEvent = dimension->event->nl;
	numSocEvent = dimension->event->soc->n;
	dimSocEvent = dimension->event->soc->q;
	if (numSocEvent)
	{
		numRowSocEvent = cmscp_sum(dimSocEvent, numSocEvent);
		numRowEvent = numNlEvent + numRowSocEvent;
	}
	else
	{
		numRowEvent = numNlEvent;
	}
	// 约束边界
	eventNlLower = bounds->event->nl->lower;
	eventNlUpper = bounds->event->nl->upper;
	eventSocUpper = bounds->event->soc;
	if (numType)
	{
		nnzType1 = getType1EventNnz(consType->type[iType], objEventNzPos, numPhase); // 第1类约束的一半非零元个数
		for (cnt = 0; cnt < numType; cnt++)
		{
			i = idxType[cnt]; // 在Event矩阵的行
			i0 = i + 1; // 在objEvent矩阵的行（Event第一行前加obj）
			cntRowAIneq = cntRowAIneq + 1;
			cntRowAIneqLower = cntRowAIneq + numType; // 下界部分的行位置
			idxOptVarPhaseStart = 0; // 各段起始索引
			idxGrdCol = 0; // 雅可比矩阵的列的计数器
			// 负左端常数项
			temp = -eventRef[i];
			// 每行首元素位置
			ptrRowACone[cntRowAIneq] = cntNzAIneq;
			ptrRowACone[cntRowAIneq+ numType] = cntNzAIneq+ nnzType1;
			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;
				// 稀疏信息
				iniStateNzPtrRow = objEventNzPos[iphase]->iniState->ptrRow;
				finStateNzPtrRow = objEventNzPos[iphase]->finState->ptrRow;
				iniTimeNzPtrRow = objEventNzPos[iphase]->iniTime->ptrRow;
				finTimeNzPtrRow = objEventNzPos[iphase]->finTime->ptrRow;
				parameterNzPtrRow = objEventNzPos[iphase]->parameter->ptrRow;
				iniStateNzIdxCol = objEventNzPos[iphase]->iniState->idxCol;
				finStateNzIdxCol = objEventNzPos[iphase]->finState->idxCol;
				parameterNzIdxCol = objEventNzPos[iphase]->parameter->idxCol;
				// 每段各种所需部分的起始索引
				idxOptVarState0 = idxOptVarPhaseStart;
				idxOptVarStatef = idxOptVarPhaseStart + (numMesh - 1) * numState;
				idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);
				idxOptVarTf = idxOptVarT0 + 1;
				idxOptVarParameter = idxOptVarPhaseStart + numMesh * (numState + numControl) + 2;
				// 参考轨迹
				refState0 = refTraj[iphase]->state->v;
				refStatef = refTraj[iphase]->state->v + (numMesh - 1) * numState;
				refT0 = refTraj[iphase]->initialtime;
				refTf = refTraj[iphase]->finaltime;
				refParameter = refTraj[iphase]->parameter;
				//  x0 前面系数
				for (p = iniStateNzPtrRow[i0]; p < iniStateNzPtrRow[i0 + 1]; p++)
				{
					j = iniStateNzIdxCol[p]; // 列坐标
					idxColACone[cntNzAIneq] = idxOptVarState0 + j;
					valueACone[cntNzAIneq] = eventGrd[i + (idxGrdCol + j) * numRowEvent];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refState0[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + numState;
				// xf 前面系数
				for (p = finStateNzPtrRow[i0]; p < finStateNzPtrRow[i0 + 1]; p++)
				{
					j = finStateNzIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarStatef + j;
					valueACone[cntNzAIneq] = eventGrd[i + (idxGrdCol + j) * numRowEvent];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refStatef[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + numState;
				// t0 前面系数
				if (iniTimeNzPtrRow[i0] + 1 == iniTimeNzPtrRow[i0 + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarT0;
					valueACone[cntNzAIneq] = eventGrd[i + idxGrdCol * numRowEvent];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refT0;
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + 1;
				// tf 前面系数
				if (finTimeNzPtrRow[i0] + 1 == finTimeNzPtrRow[i0 + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarTf;
					valueACone[cntNzAIneq] = eventGrd[i + idxGrdCol * numRowEvent];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refTf;
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + 1;
				// p 前面系数
				for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
				{
					j = parameterNzIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarParameter + j;
					valueACone[cntNzAIneq] = eventGrd[i + (idxGrdCol + j) * numRowEvent];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refParameter[j];
					cntNzAIneq = cntNzAIneq + 1;

				}
				idxGrdCol = idxGrdCol + numParameter;
				// 更新索引
				idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
			}
			// 右端项
			bCone[cntRowAIneq] = temp + eventNlUpper[i]; // 上界
			bCone[numType + cntRowAIneq] = -temp - eventNlLower[i]; // 下界
			cntRowAIneq = cntRowAIneq + 1;
		}
		cntNzAIneq = cntNzAIneq + nnzType1;
		cntRowAIneq = cntRowAIneq + numType;
	}

	// 3->2 类型2：等式约束项
// A * x0 + B * xf + C * t0 + D * tf + F * p == b
	iType = 1;
	numType = consType->type[iType].num;
	idxType = consType->type[iType].idx;
	if (numType)
	{
		for (cnt = 0; cnt < numType; cnt++)
		{
			i = idxType[cnt];
			i0 = i + 1;
			idxOptVarPhaseStart = 0;
			idxGrdCol = 0; // 雅可比矩阵的列的计数器
			// 负左端常数项
			temp = -eventRef[i];
			// 每行首元素位置
			ptrRowACone[cntRowAEqua] = cntNzAEqua;
			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;
				// 稀疏信息
				iniStateNzPtrRow = objEventNzPos[iphase]->iniState->ptrRow;
				finStateNzPtrRow = objEventNzPos[iphase]->finState->ptrRow;
				iniTimeNzPtrRow = objEventNzPos[iphase]->iniTime->ptrRow;
				finTimeNzPtrRow = objEventNzPos[iphase]->finTime->ptrRow;
				parameterNzPtrRow = objEventNzPos[iphase]->parameter->ptrRow;
				iniStateNzIdxCol = objEventNzPos[iphase]->iniState->idxCol;
				finStateNzIdxCol = objEventNzPos[iphase]->finState->idxCol;
				parameterNzIdxCol = objEventNzPos[iphase]->parameter->idxCol;
				// 每段各种所需部分的起始索引
				idxOptVarState0 = idxOptVarPhaseStart;
				idxOptVarStatef = idxOptVarPhaseStart + (numMesh - 1) * numState;
				idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);
				idxOptVarTf = idxOptVarT0 + 1;
				idxOptVarParameter = idxOptVarPhaseStart + numMesh * (numState + numControl) + 2;
				// 参考轨迹
				refState0 = refTraj[iphase]->state->v;
				refStatef = refTraj[iphase]->state->v + (numMesh - 1) * numState;
				refT0 = refTraj[iphase]->initialtime;
				refTf = refTraj[iphase]->finaltime;
				refParameter = refTraj[iphase]->parameter;
				// x0 前面系数
				for (p = iniStateNzPtrRow[i0]; p < iniStateNzPtrRow[i0 + 1]; p++)
				{
					j = iniStateNzIdxCol[p]; // 列坐标
					cntNzAEqua = cntNzAEqua + 1;
					idxColAEqua[cntNzAEqua] = idxOptVarState0 + j;
					valueACone[cntNzAEqua] = eventGrd[i + (idxGrdCol + j) * numRowEvent];
					temp = temp + valueACone[cntNzAEqua] * refState0[j];
				}
				idxGrdCol = idxGrdCol + numState;
				// xf 前面系数
				for (p = finStateNzPtrRow[i0]; p < finStateNzPtrRow[i0 + 1]; p++)
				{
					j = finStateNzIdxCol[p]; // 列
					idxColAEqua[cntNzAEqua] = idxOptVarStatef + j;
					valueAEqua[cntNzAEqua] = eventGrd[i + (idxGrdCol + j) * numRowEvent];
					temp = temp + valueAEqua[cntNzAEqua] * refStatef[j];
					cntNzAEqua = cntNzAEqua + 1;
				}
				idxGrdCol = idxGrdCol + numState;
				// t0 前面系数
				if (iniTimeNzPtrRow[i0] + 1 == iniTimeNzPtrRow[i0 + 1])
				{
					idxColAEqua[cntNzAEqua] = idxOptVarT0;
					valueAEqua[cntNzAEqua] = eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueAEqua[cntNzAEqua] * refT0;
					cntNzAEqua = cntNzAEqua + 1;
				}
				idxGrdCol = idxGrdCol + 1;
				// tf 前面系数
				if (finTimeNzPtrRow[i0] + 1 == finTimeNzPtrRow[i0 + 1])
				{
					idxColAEqua[cntNzAEqua] = idxOptVarTf;
					valueAEqua[cntNzAEqua] = eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueAEqua[cntNzAEqua] * refTf;
					cntNzAEqua = cntNzAEqua + 1;
				}
				idxGrdCol = idxGrdCol + 1;

				// p 前面系数
				for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
				{
					j = parameterNzIdxCol[p]; // 列
					idxColAEqua[cntNzAEqua] = idxOptVarParameter + j;
					valueAEqua[cntNzAEqua] = eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueAEqua[cntNzAEqua] * refParameter[j];
					cntNzAEqua = cntNzAEqua + 1;
				}
				idxGrdCol = idxGrdCol + numParameter;
				// 更新索引
				idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
			}
			// 右端项
			bEqua[cntRowAEqua] = temp + eventNlUpper[i];
			cntRowAEqua = cntRowAEqua + 1;
		}
	}

	// 3->3 类型3：上边界约束项
// A * x0 + B * xf + C * t0 + D * tf + F * p == b
	iType = 2;
	numType = consType->type[iType].num;
	idxType = consType->type[iType].idx;
	if (numType)
	{
		for (cnt = 0; cnt < numType; cnt++)
		{
			i = idxType[cnt];
			i0 = i + 1;
			idxOptVarPhaseStart = 0;
			idxGrdCol = 0; // 雅可比矩阵的列的计数器
			// 负左端常数项
			temp = -eventRef[i];
			// 每行首元素位置
			ptrRowACone[cntRowAIneq] = cntNzAIneq;
			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;
				// 稀疏信息
				iniStateNzPtrRow = objEventNzPos[iphase]->iniState->ptrRow;
				finStateNzPtrRow = objEventNzPos[iphase]->finState->ptrRow;
				iniTimeNzPtrRow = objEventNzPos[iphase]->iniTime->ptrRow;
				finTimeNzPtrRow = objEventNzPos[iphase]->finTime->ptrRow;
				parameterNzPtrRow = objEventNzPos[iphase]->parameter->ptrRow;
				iniStateNzIdxCol = objEventNzPos[iphase]->iniState->idxCol;
				finStateNzIdxCol = objEventNzPos[iphase]->finState->idxCol;
				parameterNzIdxCol = objEventNzPos[iphase]->parameter->idxCol;
				// 每段各种所需部分的起始索引
				idxOptVarState0 = idxOptVarPhaseStart;
				idxOptVarStatef = idxOptVarPhaseStart + (numMesh - 1) * numState;
				idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);
				idxOptVarTf = idxOptVarT0 + 1;
				idxOptVarParameter = idxOptVarPhaseStart + numMesh * (numState + numControl) + 2;
				// 参考轨迹
				refState0 = refTraj[iphase]->state->v;
				refStatef = refTraj[iphase]->state->v + (numMesh - 1) * numState;
				refT0 = refTraj[iphase]->initialtime;
				refTf = refTraj[iphase]->finaltime;
				refParameter = refTraj[iphase]->parameter;
				// x0 前面系数
				for (p = iniStateNzPtrRow[i0]; p < iniStateNzPtrRow[i0 + 1]; p++)
				{
					j = iniStateNzIdxCol[p]; // 列坐标
					idxColACone[cntNzAIneq] = idxOptVarState0 + j;
					valueACone[cntNzAIneq] = eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refState0[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + numState;
				// xf 前面系数
				for (p = finStateNzPtrRow[i0]; p < finStateNzPtrRow[i0 + 1]; p++)
				{
					j = finStateNzIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarStatef + j;
					valueACone[cntNzAIneq] = eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refStatef[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + numState;
				// t0 前面系数
				if (iniTimeNzPtrRow[i0] + 1 == iniTimeNzPtrRow[i0 + 1])
				{
					cntNzAIneq = cntNzAIneq + 1;
					valueACone[cntNzAIneq] = eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refT0;
					idxColACone[cntNzAIneq] = idxOptVarT0;
				}
				idxGrdCol = idxGrdCol + 1;
				// tf 前面系数
				if (finTimeNzPtrRow[i0] + 1 == finTimeNzPtrRow[i0 + 1])
				{
					cntNzAIneq = cntNzAIneq + 1;
					valueACone[cntNzAIneq] = eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refTf;
					idxColACone[cntNzAIneq] = idxOptVarTf;
				}
				idxGrdCol = idxGrdCol + 1;

				// p 前面系数
				for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
				{
					j = parameterNzIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarParameter + j;
					valueACone[cntNzAIneq] = eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refParameter[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + numParameter;
				// 更新索引
				idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
			}
			// 右端项
			bEqua[cntRowAIneq] = temp + eventNlUpper[i];
			cntRowAIneq = cntRowAIneq + 1;
		}
	}
	// 3->4 类型4：下边界约束项
	// A * x0 + B * xf + C * t0 + D * tf + F * p == b
	iType = 3;
	numType = consType->type[iType].num;
	idxType = consType->type[iType].idx;
	if (numType)
	{
		for (cnt = 0; cnt < numType; cnt++)
		{
			i = idxType[cnt];
			i0 = i + 1;
			idxOptVarPhaseStart = 0;
			idxGrdCol = 0; // 雅可比矩阵的列的计数器
			// 负左端常数项
			temp = -eventRef[i];
			// 每行首元素位置
			ptrRowACone[cntRowAIneq] = cntNzAIneq;
			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;
				// 稀疏信息
				iniStateNzPtrRow = objEventNzPos[iphase]->iniState->ptrRow;
				finStateNzPtrRow = objEventNzPos[iphase]->finState->ptrRow;
				iniTimeNzPtrRow = objEventNzPos[iphase]->iniTime->ptrRow;
				finTimeNzPtrRow = objEventNzPos[iphase]->finTime->ptrRow;
				parameterNzPtrRow = objEventNzPos[iphase]->parameter->ptrRow;
				iniStateNzIdxCol = objEventNzPos[iphase]->iniState->idxCol;
				finStateNzIdxCol = objEventNzPos[iphase]->finState->idxCol;
				parameterNzIdxCol = objEventNzPos[iphase]->parameter->idxCol;
				// 每段各种所需部分的起始索引
				idxOptVarState0 = idxOptVarPhaseStart;
				idxOptVarStatef = idxOptVarPhaseStart + (numMesh - 1) * numState;
				idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);
				idxOptVarTf = idxOptVarT0 + 1;
				idxOptVarParameter = idxOptVarPhaseStart + numMesh * (numState + numControl) + 2;
				// 参考轨迹
				refState0 = refTraj[iphase]->state->v;
				refStatef = refTraj[iphase]->state->v + (numMesh - 1) * numState;
				refT0 = refTraj[iphase]->initialtime;
				refTf = refTraj[iphase]->finaltime;
				refParameter = refTraj[iphase]->parameter;
				// x0 前面系数
				for (p = iniStateNzPtrRow[i0]; p < iniStateNzPtrRow[i0 + 1]; p++)
				{
					j = iniStateNzIdxCol[p]; // 列坐标
					idxColACone[cntNzAIneq] = idxOptVarState0 + j;
					valueACone[cntNzAIneq] = -eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refState0[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + numState;
				// xf 前面系数
				for (p = finStateNzPtrRow[i0]; p < finStateNzPtrRow[i0 + 1]; p++)
				{
					j = finStateNzIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarStatef + j;
					valueACone[cntNzAIneq] = -eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refStatef[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + numState;
				// t0 前面系数
				if (iniTimeNzPtrRow[i0] + 1 == iniTimeNzPtrRow[i0 + 1])
				{
					cntNzAIneq = cntNzAIneq + 1;
					idxColACone[cntNzAIneq] = idxOptVarT0;
					valueACone[cntNzAIneq] = -eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refT0;
				}
				idxGrdCol = idxGrdCol + 1;
				// tf 前面系数
				if (finTimeNzPtrRow[i0] + 1 == finTimeNzPtrRow[i0 + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarTf;
					valueACone[cntNzAIneq] = -eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refTf;
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + 1;

				// p 前面系数
				for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
				{
					j = parameterNzIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarParameter + j;
					valueACone[cntNzAIneq] = -eventGrd[i + idxGrdCol * numRowEvent];
					temp = temp + valueACone[cntNzAIneq] * refParameter[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxGrdCol = idxGrdCol + numParameter;
				// 更新索引
				idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
			}
			// 右端项
			bEqua[cntRowAIneq] = temp + eventNlUpper[i];
			cntRowAIneq = cntRowAIneq + 1;
		}
	}

	// 3->5 二阶锥约束
	// 事件约束维度信息
	if (numSocEvent)
	{
		for (i = 0; i < numRowSocEvent; i++)
		{
			i0 = numNlEvent + 1 + i;
			temp = -eventRef[numNlEvent + i];
			// 每行首个元素的位置
			ptrRowACone[cntRowASoc] = cntNzASoc;
			idxOptVarPhaseStart = 0;// 各段起始索引
			idxGrdCol = 0;// 雅可比矩阵的列的计数
			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;
				// 稀疏信息
				iniStateNzPtrRow = objEventNzPos[iphase]->iniState->ptrRow;
				finStateNzPtrRow = objEventNzPos[iphase]->finState->ptrRow;
				iniTimeNzPtrRow = objEventNzPos[iphase]->iniTime->ptrRow;
				finTimeNzPtrRow = objEventNzPos[iphase]->finTime->ptrRow;
				parameterNzPtrRow = objEventNzPos[iphase]->parameter->ptrRow;
				iniStateNzIdxCol = objEventNzPos[iphase]->iniState->idxCol;
				finStateNzIdxCol = objEventNzPos[iphase]->finState->idxCol;
				parameterNzIdxCol = objEventNzPos[iphase]->parameter->idxCol;
				// 每段各种所需部分的起始索引
				idxOptVarState0 = idxOptVarPhaseStart;
				idxOptVarStatef = idxOptVarPhaseStart + (numMesh - 1) * numState;
				idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);
				idxOptVarTf = idxOptVarT0 + 1;
				idxOptVarParameter = idxOptVarPhaseStart + numMesh * (numState + numControl) + 2;
				// 参考轨迹
				refState0 = refTraj[iphase]->state->v;
				refStatef = refTraj[iphase]->state->v + (numMesh - 1) * numState;
				refT0 = refTraj[iphase]->initialtime;
				refTf = refTraj[iphase]->finaltime;
				refParameter = refTraj[iphase]->parameter;
				// x0 前面系数
				for (p = iniStateNzPtrRow[i0]; p < iniStateNzPtrRow[i0 + 1]; p++)
				{
					j = iniStateNzIdxCol[p]; // 列
					cntNzASoc = cntNzASoc + 1;
					idxColACone[cntNzASoc] = idxOptVarState0 + j;
					valueACone[cntNzASoc] = eventGrd[numNlEvent + i + (idxGrdCol + j) * numRowEvent];
					temp = temp + valueACone[cntNzASoc] * refState0[j];
				}
				idxGrdCol = idxGrdCol + numState;
				// xf 前面系数
				for (p = finStateNzPtrRow[i0]; p < finStateNzPtrRow[i0 + 1]; p++)
				{
					j = finStateNzIdxCol[p]; // 列
					cntNzASoc = cntNzASoc + 1;
					idxColACone[cntNzASoc] = idxOptVarStatef + j;
					valueACone[cntNzASoc] = eventGrd[numNlEvent + i + (idxGrdCol + j) * numRowEvent];
					temp = temp + valueACone[cntNzASoc] * refStatef[j];
				}
				idxGrdCol = idxGrdCol + numState;
				// t0 前面系数
				if (iniTimeNzPtrRow[i0] + 1 == iniTimeNzPtrRow[i0 + 1])
				{
					cntNzASoc = cntNzASoc + 1;
					idxColACone[cntNzASoc] = idxOptVarT0 + 1;
					valueACone[cntNzASoc] = eventGrd[numNlEvent + i + (idxGrdCol) * numRowEvent];
					temp = temp + valueACone[cntNzASoc] * refT0;
				}
				idxGrdCol = idxGrdCol + 1;
				// tf 前面系数
				if (finTimeNzPtrRow[i0] + 1 == finTimeNzPtrRow[i0 + 1])
				{
					cntNzASoc = cntNzASoc + 1;
					idxColACone[cntNzASoc] = idxOptVarTf + 1;
					valueACone[cntNzASoc] = eventGrd[numNlEvent + i + (idxGrdCol) * numRowEvent];
					temp = temp + valueACone[cntNzASoc] * refTf;
				}
				idxGrdCol = idxGrdCol + 1;
				// p 前面系数
				for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
				{
					j = parameterNzIdxCol[p]; // 列
					cntNzASoc = cntNzASoc + 1;
					idxColACone[cntNzASoc] = idxOptVarParameter + j;
					valueACone[cntNzASoc] = eventGrd[numNlEvent + i + (idxGrdCol + j) * numRowEvent];
					temp = temp + valueACone[cntNzASoc] * refParameter[j];
				}
				idxGrdCol = idxGrdCol + numParameter;
				// 更新索引
				idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
			}
			cntRowASoc = cntRowASoc + 1;
		}
		for (i = 0; i < numSocEvent; i++)
		{
			dimCone->q[cntSubASoc] = dimSocEvent[i];
			cntSubASoc = cntSubASoc + 1;
		}
	}
	if (objDerInfo->ref) { free(objDerInfo->ref); }
	if (objDerInfo->der) { free(objDerInfo->der); }
	if (objDerInfo) { free(objDerInfo); }
	if (eventDerInfo->ref) { free(eventDerInfo->ref); }
	if (eventDerInfo->der) { free(eventDerInfo->der); }
	if (eventDerInfo) { free(eventDerInfo); }
}

void socpInfoSparseEndp(cmscp_socpInfo** socpInfo0, cmscp_trajInfo** refTraj, cmscp_sparseInfo* sparseInfo, cmscp_consTypeInfo* consTypeInfo, cmscp_setup* setup) {

	// 函数功能：获取端点函数约束的二阶锥规划问题参数
	// 输入：
	// socpInfo0：无获端点函数约束相关的二阶锥问题参数
	// refTraj：参考轨迹
	// consTypeInfo：约束类型信息
	// sparseInfo：稀疏（非零元位置）信息
	// setup：问题设置参数
	// 输出：
	// socpInfo0：有端点函数约束相关的二阶锥问题参数
	// 假设函数的自变量按照[x, u, p]排序
	// 谢磊：2022 / 6 / 27编写

	// 变量声明
	int numPhase, * numPhaseOptVar, iphase, numMesh, numState, numControl, iType, numType, * idxType, cnt, numNlEndp, * dimSocEndp, * idxColAEqua, * ptrRowAEqua, * idxColACone, * ptrRowACone, cntNzAEqua, cntRowAEqua, cntNzAIneq, cntRowAIneq, cntNzASoc, cntRowASoc, cntSubASoc, idxOptVarPhaseStart, numSocEndp, numRowSocEndp, * iniStateNzPtrRow, * finStateNzPtrRow, * iniTimeNzPtrRow, * finTimeNzPtrRow, * parameterNzPtrRow, * iniStateNzIdxCol, * finStateNzIdxCol, * parameterNzIdxCol, idxEndpGrdColStatef, idxEndpGrdColT0, idxEndpGrdColTf, idxEndpGrdColParameter, idxOptVarState0, idxOptVarStatef, idxOptVarT0, idxOptVarTf, idxOptVarParameter, nnzType1, i, p, j, numEndpOutput, i0, numOptVar;
	double* valueAEqua, * bEqua, * valueACone, * bCone, * endpNlLower, * endpNlUpper, * endpSocUpper, * refState0, * refStatef, refT0, refTf, * refParameter, * endpRef, * endpGrd, temp, *c;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_bound* bounds;
	cmscp_trust* trust;
	cmscp_consType** consType;
	cmscp_type* consTypePhase;
	cmscp_nzPos_endp** endpNzPos;
	cmscp_derInfo** gradInfo;
	cmscp_socpInfo* socpInfo;
	cmscp_dimCone* dimCone;
	//  1. 基本参数
	dimension = setup->dimension;// 问题维数信息
	mesh = setup->mesh;// 网格信息
	numPhase = setup->numPhase;// 阶段数
	bounds = setup->bounds;// 边界信息
	trust = setup->trust;// 信赖域信息
	socpInfo = (*socpInfo0);
	numOptVar = socpInfo->numOptVar;
	numPhaseOptVar = socpInfo->numPhaseOptVar;// 每段优化变量个数
	endpNzPos = sparseInfo->endpNzPos;// 稀疏信息
	consType = consTypeInfo->endp;// 约束类型信息
	// 二阶锥规划参数
	ptrRowAEqua = socpInfo->AEqua->ptrRow;
	idxColAEqua = socpInfo->AEqua->idxCol;
	valueAEqua = socpInfo->AEqua->v;
	bEqua = socpInfo->bEqua;
	ptrRowACone = socpInfo->ACone->ptrRow;
	idxColACone = socpInfo->ACone->idxCol;
	valueACone = socpInfo->ACone->v;
	bCone = socpInfo->bCone;
	dimCone = socpInfo->dimCone;
	// 计数变量
	cntNzAEqua = socpInfo->cntNzAEqua;
	cntRowAEqua = socpInfo->cntRowAEqua;
	cntNzAIneq = socpInfo->cntNzAIneq;
	cntRowAIneq = socpInfo->cntRowAIneq;
	cntNzASoc = socpInfo->cntNzASoc;
	cntRowASoc = socpInfo->cntRowASoc;
	cntSubASoc = socpInfo->cntSubASoc;

	// 梯度信息
	gradInfo = (cmscp_derInfo**)malloc(numPhase * sizeof(cmscp_derInfo));
	gradInfoSparseEndp(gradInfo, sparseInfo, refTraj, setup);
	
	//// 填充非零元素
	// 优化变量中各段起始索引
	idxOptVarPhaseStart = 0;
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		// 各段维数信息
		numMesh = mesh[iphase]->num;
		numState = dimension->phase[iphase]->state;
		numControl = dimension->phase[iphase]->control;
		numNlEndp = dimension->phase[iphase]->endpoint->nl;
		numSocEndp = dimension->phase[iphase]->endpoint->soc->n;
		dimSocEndp = dimension->phase[iphase]->endpoint->soc->q;
		if (numSocEndp)
		{
			numRowSocEndp = cmscp_sum(dimSocEndp, numSocEndp);
		}
		else
		{
			numRowSocEndp = 0;
		}
		if (dimension->phase[iphase]->endpoint->soc->n)
		{
			numEndpOutput = numNlEndp + cmscp_sum(dimSocEndp, numSocEndp);
		}
		else
		{
			numEndpOutput = numNlEndp;
		}
		// 各段约束类型信息
		consTypePhase = consType[iphase]->type;
		// 各段稀疏信息

		iniStateNzPtrRow = endpNzPos[iphase]->iniState->ptrRow;
		finStateNzPtrRow = endpNzPos[iphase]->finState->ptrRow;
		iniTimeNzPtrRow = endpNzPos[iphase]->iniTime->ptrRow;
		finTimeNzPtrRow = endpNzPos[iphase]->finTime->ptrRow;
		parameterNzPtrRow = endpNzPos[iphase]->parameter->ptrRow;
		iniStateNzIdxCol = endpNzPos[iphase]->iniState->idxCol;
		finStateNzIdxCol = endpNzPos[iphase]->finState->idxCol;
		parameterNzIdxCol = endpNzPos[iphase]->parameter->idxCol;
		// 各段约束上下界
		endpNlLower = bounds->phase[iphase]->endpoint->nl->lower;
		endpNlUpper = bounds->phase[iphase]->endpoint->nl->upper;
		endpSocUpper = bounds->phase[iphase]->endpoint->soc;
		// 各段参考轨迹
		refState0 = refTraj[iphase]->state->v;
		refStatef = refTraj[iphase]->state->v + (numMesh - 1) * numState;
		refT0 = refTraj[iphase]->initialtime;
		refTf = refTraj[iphase]->finaltime;
		refParameter = refTraj[iphase]->parameter;
		// 各段梯度信息
		endpRef = gradInfo[iphase]->ref;
		endpGrd = gradInfo[iphase]->der;
		// 各段所需变量的起始索引
		idxEndpGrdColStatef = numState;// EndpGrd矩阵中终端点状态的对应的列的起始索引
		idxEndpGrdColT0 = 2 * numState;// EndpGrd矩阵中初始时刻的对应的列的起始索引
		idxEndpGrdColTf = idxEndpGrdColT0 + 1;// EndpGrd矩阵中终端时刻的对应的列的起始索引
		idxEndpGrdColParameter = idxEndpGrdColTf + 1;// EndpGrd矩阵中终端时刻的对应的列的起始索引
		idxOptVarState0 = idxOptVarPhaseStart;// 优化变量中每段的初始点状态的索引
		idxOptVarStatef = idxOptVarPhaseStart + (numMesh - 1) * numState;// 优化变量中每段的终端点状态的索引
		idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);// 优化变量中每段的初始点时刻的索引
		idxOptVarTf = idxOptVarT0 + 1;// 优化变量中每段的终端点时刻的索引
		idxOptVarParameter = idxOptVarTf + 1;// 优化变量中每段的静态参数的索引
		//// 1 类型1：双不等式约束项
		// 上界：A * x0 + B * xf + C * t0 + D * tf + F * p <= b
		// 下界： - A * x0 - B * xf - C * t0 - D * tf - F * p <= b
		iType = 0;
		numType = consTypePhase[iType].num;
		idxType = consTypePhase[iType].idx;
		if (numType)
		{
			nnzType1 = getType1EndpNnz(consTypePhase[iType], endpNzPos[iphase]);// 一半非零元个数
			for (cnt = 0; cnt < numType; cnt++)
			{
				i = idxType[cnt];
				temp = -endpRef[i];
				// 每行首个非零元的位置
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				ptrRowACone[cntRowAIneq + numType] = cntNzAIneq + nnzType1;
				// x0前面系数
				for (p = iniStateNzPtrRow[i]; p < iniStateNzPtrRow[i + 1]; p++)
				{
					j = iniStateNzIdxCol[p];// 列
					idxColACone[cntNzAIneq] = idxOptVarState0 + j;
					valueACone[cntNzAIneq] = endpGrd[i+j * numEndpOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refState0[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// xf前面系数
				for (p = finStateNzPtrRow[i]; p < finStateNzPtrRow[i + 1]; p++)
				{
					j = finStateNzIdxCol[p];// 列
					idxColACone[cntNzAIneq] = idxOptVarStatef + j;
					valueACone[cntNzAIneq] = endpGrd[i+(idxEndpGrdColStatef + j) * numEndpOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refStatef[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// t0前面系数
				if (iniTimeNzPtrRow[i] + 1 == iniTimeNzPtrRow[i + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarT0;
					valueACone[cntNzAIneq] = endpGrd[i+idxEndpGrdColT0 * numEndpOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refT0;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// tf前面系数
				if (finTimeNzPtrRow[i] + 1 == finTimeNzPtrRow[i + 1]) {
					idxColACone[cntNzAIneq] = idxOptVarTf;
					valueACone[cntNzAIneq] = endpGrd[i+idxEndpGrdColTf * numEndpOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refTf;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// p前面系数
				for (p = parameterNzPtrRow[i]; p < parameterNzPtrRow[i + 1]; p++)
				{
					j = parameterNzIdxCol[p];// 列
					idxColACone[cntNzAIneq] = idxOptVarParameter + j;
					valueACone[cntNzAIneq] = endpGrd[i+(idxEndpGrdColParameter + j) * numEndpOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refParameter[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				bCone[cntRowAIneq] = temp + endpNlUpper[i];
				bCone[cntRowAIneq + numType] = -temp - endpNlLower[i];
				cntRowAIneq = cntRowAIneq + 1;
			}
			cntNzAIneq = cntNzAIneq + nnzType1;
			cntRowAIneq = cntRowAIneq + numType;
		}
		//// 2 类型2：等式约束项，表达式：
		// A * x0 + B * xf + C * t0 + D * tf + F * p == b
		iType = 1;
		numType = consTypePhase[iType].num;
		idxType = consTypePhase[iType].idx;
		if (numType)
		{
			for (cnt = 0; cnt < numType; cnt++)
			{
				i = idxType[cnt];
				temp = -endpRef[i];
				// 每行首个非零元的位置
				ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
				// x0前面系数
				for (p = iniStateNzPtrRow[i]; p < iniStateNzPtrRow[i + 1]; p++)
				{
					j = iniStateNzIdxCol[p];// 列
					idxColAEqua[cntNzAEqua] = idxOptVarState0 + j;
					valueAEqua[cntNzAEqua] = endpGrd[i+j * numEndpOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refState0[j];
					cntNzAEqua = cntNzAEqua + 1;
				}
				// xf前面系数
				for (p = finStateNzPtrRow[i]; p < finStateNzPtrRow[i + 1]; p++)
				{
					j = finStateNzIdxCol[p];// 列
					idxColAEqua[cntNzAEqua] = idxOptVarStatef + j;
					valueAEqua[cntNzAEqua] = endpGrd[i+(idxEndpGrdColStatef + j) * numEndpOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refStatef[j];
					cntNzAEqua = cntNzAEqua + 1;
				}
				// t0前面系数
				if (iniTimeNzPtrRow[i] + 1 == iniTimeNzPtrRow[i + 1])
				{
					idxColAEqua[cntNzAEqua] = idxOptVarT0;
					valueAEqua[cntNzAEqua] = endpGrd[i+idxEndpGrdColT0 * numEndpOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refT0;
					cntNzAEqua = cntNzAEqua + 1;
				}
				// tf前面系数
				if (finTimeNzPtrRow[i] + 1 == finTimeNzPtrRow[i + 1])
				{
					idxColAEqua[cntNzAEqua] = idxOptVarTf;
					valueAEqua[cntNzAEqua] = endpGrd[i+idxEndpGrdColTf * numEndpOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refTf;
					cntNzAEqua = cntNzAEqua + 1;
				}
				// p前面系数
				for (p = parameterNzPtrRow[i]; p < parameterNzPtrRow[i + 1]; p++)
				{
					j = parameterNzIdxCol[p];// 列
					idxColAEqua[cntNzAEqua] = idxOptVarParameter + j;
					valueAEqua[cntNzAEqua] = endpGrd[i+(idxEndpGrdColParameter + j) * numEndpOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refParameter[j];
					cntNzAEqua = cntNzAEqua + 1;
				}
				bEqua[cntRowAEqua] = temp + endpNlUpper[i];
				cntRowAEqua = cntRowAEqua + 1;
			}
		}
		//// 类型3：上不等式约束项，表达式：
		// A* x0 + B * xf + C * t0 + D * tf + F * p <= b
		iType = 2;
		numType = consTypePhase[iType].num;
		idxType = consTypePhase[iType].idx;
		if (numType)
		{
			for (cnt = 0; cnt < numType; cnt++)
			{
				i = idxType[cnt];
				temp = -endpRef[i];
				// 每行首个非零元的位置
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				// x0前面系数
				for (p = iniStateNzPtrRow[i]; p < iniStateNzPtrRow[i + 1]; p++)
				{
					j = iniStateNzIdxCol[p];// 列
					idxColACone[cntNzAIneq] = idxOptVarState0 + j;
					valueACone[cntNzAIneq] = endpGrd[i+j * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refState0[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// xf前面系数
				for (p = finStateNzPtrRow[i]; p < finStateNzPtrRow[i + 1]; p++)
				{
					j = finStateNzIdxCol[p];// 列
					idxColACone[cntNzAIneq] = idxOptVarStatef + j;
					valueACone[cntNzAIneq] = endpGrd[i+(idxEndpGrdColStatef + j) * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refStatef[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// t0前面系数
				if (iniTimeNzPtrRow[i] + 1 == iniTimeNzPtrRow[i + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarT0;
					valueACone[cntNzAIneq] = endpGrd[i+idxEndpGrdColT0 * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refT0;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// tf前面系数
				if (finTimeNzPtrRow[i] + 1 == finTimeNzPtrRow[i + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarTf;
					valueACone[cntNzAIneq] = endpGrd[i+idxEndpGrdColTf * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refTf;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// p前面系数
				for (p = parameterNzPtrRow[i]; p < parameterNzPtrRow[i + 1]; p++)
				{
					j = parameterNzIdxCol[p];// 列
					idxColACone[cntNzAIneq] = idxOptVarParameter + j;
					valueACone[cntNzAIneq] = endpGrd[i+(idxEndpGrdColParameter + j) * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refParameter[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				bCone[cntRowAIneq] = temp + endpNlUpper[i];
				cntRowAIneq = cntRowAIneq + 1;
			}
		}
		//// 类型4：下不等式约束项
		// 表达式： - A * x0 - B * xf - C * t0 - D * tf - F * p <= -b
		iType = 3;
		numType = consTypePhase[iType].num;
		idxType = consTypePhase[iType].idx;
		if (numType)
		{
			for (cnt = 0; cnt < numType; cnt++)
			{
				i = idxType[cnt];
				temp = endpRef[i];
				// 每行首个非零元的位置
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				// x0前面系数
				for (p = iniStateNzPtrRow[i]; p < iniStateNzPtrRow[i + 1]; p++)
				{
					j = iniStateNzIdxCol[p];// 列
					idxColACone[cntNzAIneq] = idxOptVarState0 + j;
					valueACone[cntNzAIneq] = -endpGrd[i+j * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refState0[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// xf前面系数
				for (p = finStateNzPtrRow[i]; p < finStateNzPtrRow[i + 1]; p++)
				{
					j = finStateNzIdxCol[p];// 列
					idxColACone[cntNzAIneq] = idxOptVarStatef + j;
					valueACone[cntNzAIneq] = -endpGrd[i+(idxEndpGrdColStatef + j) * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refStatef[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// t0前面系数
				if (iniTimeNzPtrRow[i] + 1 == iniTimeNzPtrRow[i + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarT0;
					valueACone[cntNzAIneq] = -endpGrd[i+idxEndpGrdColT0 * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refT0;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// tf前面系数
				if (finTimeNzPtrRow[i] + 1 == finTimeNzPtrRow[i + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarTf;
					valueACone[cntNzAIneq] = -endpGrd[i+idxEndpGrdColTf * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refTf;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// p前面系数
				for (p = parameterNzPtrRow[i]; p < parameterNzPtrRow[i + 1]; p++)
				{
					j = parameterNzIdxCol[p];// 列
					idxColACone[cntNzAIneq] = idxOptVarParameter + j;
					valueACone[cntNzAIneq] = -endpGrd[i+(idxEndpGrdColParameter + j) * numEndpOutput];
					temp = temp + valueACone[cntNzAIneq] * refParameter[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				bCone[cntRowAIneq] = temp - endpNlLower[i];
				cntRowAIneq = cntRowAIneq + 1;
			}
		}
		if (numSocEndp)
		{
			for (i = 0; i < numRowSocEndp; i++)
			{
				i0 = numNlEndp + i;
				temp = -endpRef[i0];
				// 每行首个非零元的位置
				ptrRowACone[cntRowASoc] = cntNzASoc;
				// x0前面系数
				for (p = iniStateNzPtrRow[i0]; p < iniStateNzPtrRow[i0 + 1]; p++)
				{
					j = iniStateNzIdxCol[p];// 列
					idxColACone[cntNzASoc] = idxOptVarState0 + j;
					valueACone[cntNzASoc] = endpGrd[i0+j * numEndpOutput];
					temp = temp + valueACone[cntNzASoc] * refState0[j];
					cntNzASoc = cntNzASoc + 1;
				}
				// xf前面系数
				for (p = finStateNzPtrRow[i0]; p < finStateNzPtrRow[i0 + 1]; p++)
				{
					j = finStateNzIdxCol[p];// 列
					idxColACone[cntNzASoc] = idxOptVarStatef + j;
					valueACone[cntNzASoc] = endpGrd[i0+(idxEndpGrdColStatef + j) * numEndpOutput];
					temp = temp + valueACone[cntNzASoc] * refStatef[j];
					cntNzASoc = cntNzASoc + 1;
				}
				// t0前面系数
				if (iniTimeNzPtrRow[i0] + 1 == iniTimeNzPtrRow[i0 + 1])
				{
					idxColACone[cntNzASoc] = idxOptVarT0;
					valueACone[cntNzASoc] = endpGrd[i0+idxEndpGrdColT0 * numEndpOutput];
					temp = temp + valueACone[cntNzASoc] * refT0;
					cntNzASoc = cntNzASoc + 1;
				}
				// tf前面系数
				if (finTimeNzPtrRow[i0] + 1 == finTimeNzPtrRow[i0 + 1])
				{
					idxColACone[cntNzASoc] = idxOptVarTf;
					valueACone[cntNzASoc] = endpGrd[i0+idxEndpGrdColTf * numEndpOutput];
					temp = temp + valueACone[cntNzASoc] * refTf;
					cntNzASoc = cntNzASoc + 1;
				}
				// p前面系数
				for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
				{
					j = parameterNzIdxCol[p];// 列
					idxColACone[cntNzASoc] = idxOptVarParameter + j;
					valueACone[cntNzASoc] = endpGrd[i0+(idxEndpGrdColParameter + j) * numEndpOutput];
					temp = temp + valueACone[cntNzASoc] * refParameter[j];
					cntNzASoc = cntNzASoc + 1;
				}
				bCone[cntRowASoc] = temp + endpSocUpper[i];
				cntRowASoc = cntRowASoc + 1;
			}
			for (i = 0; i < numSocEndp; i++)
			{
				dimCone->q[cntSubASoc] = dimSocEndp[i];
				cntSubASoc = cntSubASoc + 1;
			}
		}
		idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
		free(gradInfo[iphase]->ref);
		free(gradInfo[iphase]->der);
		free(gradInfo[iphase]);
	}
	socpInfo->cntNzAEqua = cntNzAEqua;
	socpInfo->cntRowAEqua = cntRowAEqua;
	socpInfo->cntNzAIneq = cntNzAIneq;
	socpInfo->cntRowAIneq = cntRowAIneq;
	socpInfo->cntNzASoc = cntNzASoc;
	socpInfo->cntRowASoc = cntRowASoc;
	socpInfo->cntSubASoc = cntSubASoc;

	free(gradInfo);
}

void socpInfoSparseCont(cmscp_socpInfo** socpInfo0, cmscp_trajInfo** refTraj, cmscp_sparseInfo* sparseInfo, cmscp_consTypeInfo* consTypeInfo, cmscp_setup* setup)
{
	// 函数功能：获取连续函数约束的二阶锥规划问题参数
	// 输入：
	// socpInfo：二阶锥信息结构体
	// refTraj：参考轨迹信息
	// consTypeInfo：约束类型信息
	// sparseInfo：稀疏（非零元位置）信息
	// setup：问题设置参数
	// 输出：
	// socpInfo：二阶锥信息结构体
	// 谢磊：2022 / 4 / 18编写
	// 变量声明
	int numPhase, * numPhaseOptVar, iphase, numMesh, numState, numControl, cnt, * idxColAEqua, * ptrRowAEqua, * idxColACone, * ptrRowACone, cntNzAEqua, cntRowAEqua, cntNzAIneq, cntRowAIneq, cntNzASoc, cntRowASoc, cntSubASoc, idxOptVarPhaseStart, * parameterNzPtrRow, * parameterNzIdxCol, idxOptVarT0, idxOptVarTf, idxOptVarParameter, nnzType1, i, p, j, i0, numParameter, numNlPath, numSocPath, * dimSocPath, numRowSocPath, numRowPath, * stateNzPtrRow, * controlNzPtrRow, * stateNzIdxCol, * controlNzIdxCol, * stateNzIdxRow, * controlNzIdxRow, * parameterNzIdxRow, * stateNzPtrCol, * controlNzPtrCol, * parameterNzPtrCol, numType1, numType2, numType3, numType4, * idxType1, * idxType2, * idxType3, * idxType4, idxPathGrdColControl, idxPathGrdColParameter, idxDynGrdColControl, idxDynGrdColParameter, idxOptVarStateK, idxOptVarControlK, k, idxDynGrd, idxOptVarStateKm1, idxOptVarControlKm1, numContInput, idxPathGrd;;
	double* valueAEqua, * bEqua, * valueACone, * bCone, refT0, refTf, * refParameter, temp, * refState, * refControl, * meshNode, * refStateK, * refControlK, * pathNlLower, * pathNlUpper, * pathSocUpper, * dynRefK, * dynGrdK, * pathRefK, * pathGrdK, * dynRefKm1, * dynGrdKm1, * pathRefKm1, * pathGrdKm1, a1, a2, * refStateKm1, * refControlKm1, * tempPtr;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_bound* bounds;
	cmscp_consType** consType;
	cmscp_nzPos_cont** contNzPos;
	cmscp_derInfo** dynGradInfo;
	cmscp_derInfo** pathGradInfo;
	cmscp_socpInfo* socpInfo;
	cmscp_dimCone* dimCone;

	// 分配空间
	dynGradInfo = (cmscp_derInfo**)malloc(sizeof(cmscp_derInfo*));
	dynGradInfo[0] = (cmscp_derInfo*)malloc(sizeof(cmscp_derInfo));
	pathGradInfo = (cmscp_derInfo**)malloc(sizeof(cmscp_derInfo*));
	pathGradInfo[0] = (cmscp_derInfo*)malloc(sizeof(cmscp_derInfo));
	// //基本参数
	dimension = setup->dimension;// 问题维数信息
	mesh = setup->mesh;// 网格信息
	bounds = setup->bounds;// 边界信息
	numPhase = setup->numPhase;// 阶段数
	socpInfo = *socpInfo0;
	numPhaseOptVar = socpInfo->numPhaseOptVar;// 每段优化变量个数
	contNzPos = sparseInfo->contNzPos;// 稀疏信息
	consType = consTypeInfo->path;// 约束类型信息
	// 二阶锥规划参数
	ptrRowAEqua = socpInfo->AEqua->ptrRow;
	idxColAEqua = socpInfo->AEqua->idxCol;
	valueAEqua = socpInfo->AEqua->v;
	bEqua = socpInfo->bEqua;
	ptrRowACone = socpInfo->ACone->ptrRow;
	idxColACone = socpInfo->ACone->idxCol;
	valueACone = socpInfo->ACone->v;
	bCone = socpInfo->bCone;
	dimCone = socpInfo->dimCone;
	// 计数变量
	cntNzAEqua = socpInfo->cntNzAEqua;
	cntRowAEqua = socpInfo->cntRowAEqua;
	cntNzAIneq = socpInfo->cntNzAIneq;
	cntRowAIneq = socpInfo->cntRowAIneq;
	cntNzASoc = socpInfo->cntNzASoc;
	cntRowASoc = socpInfo->cntRowASoc;
	cntSubASoc = socpInfo->cntSubASoc;
	// 优化变量中各段起始索引
	idxOptVarPhaseStart = 0;
	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;
		numNlPath = dimension->phase[iphase]->path->nl;
		numSocPath = dimension->phase[iphase]->path->soc->n;
		dimSocPath = dimension->phase[iphase]->path->soc->q;
		if (numSocPath)
		{
			numRowSocPath = cmscp_sum(dimSocPath, numSocPath);
		}
		else
		{
			numRowSocPath = 0;
		}
		numRowPath = numNlPath + numRowSocPath;
		// 约束上下界简写

		pathNlLower = bounds->phase[iphase]->path->nl->lower;
		pathNlUpper = bounds->phase[iphase]->path->nl->upper;
		pathSocUpper = bounds->phase[iphase]->path->soc;
		// 参考轨迹
		refState = refTraj[iphase]->state->v;
		refControl = refTraj[iphase]->control->v;
		refParameter = refTraj[iphase]->parameter;
		refT0 = refTraj[iphase]->initialtime;
		refTf = refTraj[iphase]->finaltime;
		// 稀疏信息

		stateNzPtrRow = contNzPos[iphase]->state->ptrRow;
		controlNzPtrRow = contNzPos[iphase]->control->ptrRow;
		parameterNzPtrRow = contNzPos[iphase]->parameter->ptrRow;
		stateNzIdxCol = contNzPos[iphase]->state->idxCol;
		controlNzIdxCol = contNzPos[iphase]->control->idxCol;
		parameterNzIdxCol = contNzPos[iphase]->parameter->idxCol;
		stateNzIdxRow = contNzPos[iphase]->state->idxRow;
		controlNzIdxRow = contNzPos[iphase]->control->idxRow;
		parameterNzIdxRow = contNzPos[iphase]->parameter->idxRow;
		stateNzPtrCol = contNzPos[iphase]->state->ptrCol;
		controlNzPtrCol = contNzPos[iphase]->control->ptrCol;
		parameterNzPtrCol = contNzPos[iphase]->parameter->ptrCol;
		// 约束类型简写

		numType1 = consType[iphase]->type[0].num;
		numType2 = consType[iphase]->type[1].num;
		numType3 = consType[iphase]->type[2].num;
		numType4 = consType[iphase]->type[3].num;
		idxType1 = consType[iphase]->type[0].idx;
		idxType2 = consType[iphase]->type[1].idx;
		idxType3 = consType[iphase]->type[2].idx;
		idxType4 = consType[iphase]->type[3].idx;

		nnzType1 = getType1PathNnz(consType[iphase]->type[0], contNzPos[iphase], numState); // Type1约束的一半非零元个数
		//离散节点
		meshNode = mesh[iphase]->node;
		// 索引
		idxPathGrdColControl = numRowPath * numState;
		idxPathGrdColParameter = numRowPath * (numState + numControl);
		idxDynGrdColControl = numState * numState;
		idxDynGrdColParameter = numState * (numState + numControl);
		idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);
		idxOptVarTf = idxOptVarT0 + 1;
		idxOptVarParameter = idxOptVarTf + 1;
		idxOptVarStateK = idxOptVarPhaseStart;
		idxOptVarControlK = idxOptVarPhaseStart + numMesh * numState;
		// 导数信息空间
		numContInput = numState + numControl + numParameter;
		dynRefKm1 = (double*)malloc(numState * sizeof(double));
		dynGrdKm1 = (double*)malloc(numState * numContInput * sizeof(double));
		pathRefKm1 = (double*)malloc(numRowPath * sizeof(double));
		pathGrdKm1 = (double*)malloc(numRowPath * numContInput * sizeof(double));
		dynRefK = (double*)malloc(numState * sizeof(double));
		dynGrdK = (double*)malloc(numState * numContInput * sizeof(double));
		pathRefK = (double*)malloc(numRowPath * sizeof(double));
		pathGrdK = (double*)malloc(numRowPath * numContInput * sizeof(double));
		for (k = 0; k < numMesh; k++)
		{
			refStateK = refState + k * numState;
			refControlK = refControl + k * numControl;
			refStateKm1 = refState + (k - 1) * numState;
			refControlKm1 = refControl + (k - 1) * numControl;
			// 梯度信息
			dynGradInfo[0]->ref = dynRefK;
			dynGradInfo[0]->der = dynGrdK;
			pathGradInfo[0]->ref = pathRefK;
			pathGradInfo[0]->der = pathGrdK;
			gradInfoSparseCont(dynGradInfo, pathGradInfo, refStateK, refControlK,
				refParameter, stateNzIdxRow, controlNzIdxRow,
				parameterNzIdxRow, stateNzPtrCol, controlNzPtrCol,
				parameterNzPtrCol, numState,
				numControl, numParameter, numRowPath,
				iphase, setup);
			if (k > 0) {
				// 常数项
				a1 = 0.5 * (meshNode[k] - meshNode[k - 1]);
				a2 = a1 * (refTf - refT0);
				for (i = 0; i < numState; i++) {  // 行
					ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
					temp = 0;
					// xK前面系数
					for (p = stateNzPtrRow[i]; p < stateNzPtrRow[i + 1]; p++)
					{
						j = stateNzIdxCol[p];// 列
						idxDynGrd = j * numState + i;// 第(i, j)个元素
						idxColAEqua[cntNzAEqua] = idxOptVarStateKm1 + j;
						valueAEqua[cntNzAEqua] = a2 * dynGrdKm1[idxDynGrd];
						temp = temp + valueAEqua[cntNzAEqua] * refStateKm1[j];
						if (i == j)
						{
							valueAEqua[cntNzAEqua] = valueAEqua[cntNzAEqua] + 1;
						}
						cntNzAEqua = cntNzAEqua + 1;
					}
					// xKp1前面系数
					for (p = stateNzPtrRow[i]; p < stateNzPtrRow[i + 1]; p++)
					{
						j = stateNzIdxCol[p];// 列
						idxDynGrd = j * numState + i;// 第(i, j)个元素
						idxColAEqua[cntNzAEqua] = idxOptVarStateK + j;
						valueAEqua[cntNzAEqua] = a2 * dynGrdK[idxDynGrd];
						temp = temp + valueAEqua[cntNzAEqua] * refStateK[j];
						if (i == j)
						{
							valueAEqua[cntNzAEqua] = valueAEqua[cntNzAEqua] - 1;
						}
						cntNzAEqua = cntNzAEqua + 1;
					}
					// uK前面系数
					for (p = controlNzPtrRow[i]; p < controlNzPtrRow[i + 1]; p++)
					{
						j = controlNzIdxCol[p];// 列
						idxDynGrd = idxDynGrdColControl + j * numState + i;// 第(i, j)个元素
						idxColAEqua[cntNzAEqua] = idxOptVarControlKm1 + j;
						valueAEqua[cntNzAEqua] = a2 * dynGrdKm1[idxDynGrd];
						temp = temp + valueAEqua[cntNzAEqua] * refControlKm1[j];
						cntNzAEqua = cntNzAEqua + 1;
					}
					// uKp1前面系数
					for (p = controlNzPtrRow[i]; p < controlNzPtrRow[i + 1]; p++)
					{
						j = controlNzIdxCol[p];// 列
						idxDynGrd = idxDynGrdColControl + j * numState + i;// 第(i, j)个元素
						idxColAEqua[cntNzAEqua] = idxOptVarControlK + j;
						valueAEqua[cntNzAEqua] = a2 * dynGrdK[idxDynGrd];
						temp = temp + valueAEqua[cntNzAEqua] * refControlK[j];
						cntNzAEqua = cntNzAEqua + 1;
					}
					// t0前面系数
					idxColAEqua[cntNzAEqua] = idxOptVarT0;
					valueAEqua[cntNzAEqua] = -a1 * (dynRefKm1[i] + dynRefK[i]);
					cntNzAEqua = cntNzAEqua + 1;
					// tf前面系数
					idxColAEqua[cntNzAEqua] = idxOptVarTf;
					valueAEqua[cntNzAEqua] = -valueAEqua[cntNzAEqua - 1];
					cntNzAEqua = cntNzAEqua + 1;
					// p前面系数
					for (p = parameterNzPtrRow[i]; p < parameterNzPtrRow[i + 1]; p++)
					{
						j = parameterNzIdxCol[p];// 列
						idxDynGrd = idxDynGrdColParameter + j * numState + i;// 第(i, j)个元素
						idxColAEqua[cntNzAEqua] = idxOptVarParameter + j;
						valueAEqua[cntNzAEqua] = a2 * (dynGrdKm1[idxDynGrd] + dynGrdK[idxDynGrd]);
						temp = temp + valueAEqua[cntNzAEqua] * refParameter[j];
						cntNzAEqua = cntNzAEqua + 1;
					}
					bEqua[cntRowAEqua] = temp;
					cntRowAEqua = cntRowAEqua + 1;
				}
			}

			//// 路径约束的系数和右端项
			// 类型1：双不等式约束项
			// 上界： Ak* xK + BK * uK + CK * p <= bK
			// 下界： - Ak * xK - BK * uK - CK * p <= -bK
			
			if (numType1)
			{
				for (cnt = 0; cnt < numType1; cnt++)
				{
					i = idxType1[cnt];// 在路径约束函数中的行位置
					i0 = i + numState;// 在连续约束函数中的行位置

					temp = -pathRefK[i];
					// 每行首个非零元的位置
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					ptrRowACone[cntRowAIneq + numType1] = cntNzAIneq + nnzType1;
					// xK前面系数矩阵
					for (p = stateNzPtrRow[i0]; p < stateNzPtrRow[i0 + 1]; p++)
					{
						j = stateNzIdxCol[p];// 列
						idxPathGrd = j * numRowPath + i;// 第(i, j)个元素
						idxColACone[cntNzAIneq] = idxOptVarStateK + j;
						valueACone[cntNzAIneq] = pathGrdK[idxPathGrd];
						idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
						valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
						temp = temp + valueACone[cntNzAIneq] * refStateK[j];
						cntNzAIneq = cntNzAIneq + 1;
					}
					// uK前面系数矩阵
					for (p = controlNzPtrRow[i0]; p < controlNzPtrRow[i0 + 1]; p++)
					{
						j = controlNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColControl + j * numRowPath + i;// 第(i, j)个元素
						idxColACone[cntNzAIneq] = idxOptVarControlK + j;
						valueACone[cntNzAIneq] = pathGrdK[idxPathGrd];
						idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
						valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
						temp = temp + valueACone[cntNzAIneq] * refControlK[j];
						cntNzAIneq = cntNzAIneq + 1;
					}
					// p前面系数矩阵
					for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++) {
						j = parameterNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColParameter + j * numRowPath + i;// 第(i, j)个元素
						idxColACone[cntNzAIneq] = idxOptVarParameter + j;
						valueACone[cntNzAIneq] = pathGrdK[idxPathGrd];
						idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
						valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
						temp = temp + valueACone[cntNzAIneq] * refParameter[j];
						cntNzAIneq = cntNzAIneq + 1;
					}
					// 右端项
					bCone[cntRowAIneq] = temp + pathNlUpper[i]; // 上界
					bCone[numType1 + cntRowAIneq] = -temp - pathNlLower[i]; // 下界
					cntRowAIneq = cntRowAIneq + 1;
				}
				cntNzAIneq = cntNzAIneq + nnzType1;
				cntRowAIneq = cntRowAIneq + numType1;
			}
			// 等式约束项，表达式：
			// Ak* xK + BK * uK + CK * p == bK
			if (numType2)
			{
				for (cnt = 0; cnt < numType2; cnt++)
				{
					i = idxType2[cnt];
					i0 = i + numState;
					temp = -pathRefK[i];
					ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
					// xK前面系数
					for (p = stateNzPtrRow[i0]; p < stateNzPtrRow[i0 + 1]; p++) {
						j = stateNzIdxCol[p];// 列
						idxPathGrd = j * numRowPath + i;// 第(i, j)个元素
						idxColAEqua[cntNzAEqua] = idxOptVarStateK + j;
						valueAEqua[cntNzAEqua] = pathGrdK[idxPathGrd];
						temp = temp + valueAEqua[cntNzAEqua] * refStateK[j];
						cntNzAEqua = cntNzAEqua + 1;
					}
					// uK前面系数
					for (p = controlNzPtrRow[i0]; p < controlNzPtrRow[i0 + 1]; p++)
					{
						j = controlNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColControl + j * numRowPath + i;// 第(i, j)个元素
						idxColAEqua[cntNzAEqua] = idxOptVarControlK + j;
						valueAEqua[cntNzAEqua] = pathGrdK[idxPathGrd];
						temp = temp + valueAEqua[cntNzAEqua] * refControlK[j];
						cntNzAEqua = cntNzAEqua + 1;
					}
					// p前面系数
					for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
					{
						j = parameterNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColParameter + j * numRowPath + i;// 第(i, j)个元素
						idxColAEqua[cntNzAEqua] = idxOptVarParameter + j;
						valueAEqua[cntNzAEqua] = pathGrdK[idxPathGrd];
						temp = temp + valueAEqua[cntNzAEqua] * refParameter[j];
						cntNzAEqua = cntNzAEqua + 1;
					}
					bEqua[cntRowAEqua] = temp + pathNlUpper[i];
					cntRowAEqua = cntRowAEqua + 1;
				}
			}
			// 上单边不等式约束项，表达式：
			// Ak* xK + BK * uK + CK * p <= bK
			if (numType3)
			{
				for (cnt = 0; cnt < numType3; cnt++)
				{
					i = idxType3[cnt];
					i0 = i + numState;
					temp = -pathRefK[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					// xK前面系数
					for (p = stateNzPtrRow[i0]; p < stateNzPtrRow[i0 + 1]; p++)
					{
						j = stateNzIdxCol[p];// 列
						idxPathGrd = j * numRowPath + i;// 第(i, j)个元素
						idxColACone[cntNzAIneq] = idxOptVarStateK + j;
						valueACone[cntNzAIneq] = pathGrdK[idxPathGrd];
						temp = temp + valueACone[cntNzAIneq] * refStateK[j];
						cntNzAIneq = cntNzAIneq + 1;
					}
					// uK前面系数
					for (p = controlNzPtrRow[i0]; p < controlNzPtrRow[i0 + 1]; p++)
					{
						j = controlNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColControl + j * numRowPath + i;// 第(i, j)个元素
						idxColACone[cntNzAIneq] = idxOptVarControlK + j;
						valueACone[cntNzAIneq] = pathGrdK[idxPathGrd];
						temp = temp + valueACone[cntNzAIneq] * refControlK[j];
						cntNzAIneq = cntNzAIneq + 1;
					}
					// p前面系数
					for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
					{
						j = parameterNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColParameter + j * numRowPath + i;// 第(i, j)个元素
						idxColACone[cntNzAIneq] = idxOptVarParameter + j;
						valueACone[cntNzAIneq] = pathGrdK[idxPathGrd];
						temp = temp + valueACone[cntNzAIneq] * refParameter[j];
						cntNzAIneq = cntNzAIneq + 1;
					}
					bCone[cntRowAIneq] = temp + pathNlUpper[i];
					cntRowAIneq = cntRowAIneq + 1;
				}
			}
			// 下单边不等式约束项，表达式：
			// -Ak * xK - BK * uK - CK * p <= -bK
			if (numType4) {
				for (cnt = 0; cnt < numType4; cnt++)
				{
					i = idxType4[cnt];
					i0 = i + numState;
					temp = pathRefK[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					// xK前面系数
					for (p = stateNzPtrRow[i0]; p < stateNzPtrRow[i0 + 1]; p++)
					{
						j = stateNzIdxCol[p];// 列
						idxPathGrd = j * numRowPath + i;// 第(i, j)个元素
						idxColACone[cntNzAIneq] = idxOptVarStateK + j;
						valueACone[cntNzAIneq] = -pathGrdK[idxPathGrd];
						temp = temp + valueACone[cntNzAIneq] * refStateK[j];
						cntNzAIneq = cntNzAIneq + 1;
					}
					// uK前面系数
					for (p = controlNzPtrRow[i0]; p < controlNzPtrRow[i0 + 1]; p++)
					{
						j = controlNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColControl + j * numRowPath + i;// 第(i, j)个元素
						idxColACone[cntNzAIneq] = idxOptVarControlK + j;
						valueACone[cntNzAIneq] = -pathGrdK[idxPathGrd];
						temp = temp + valueACone[cntNzAIneq] * refControlK[j];
						cntNzAIneq = cntNzAIneq + 1;
					}
					// p前面系数
					for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
					{
						j = parameterNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColParameter + j * numRowPath + i;// 第(i, j)个元素
						idxColACone[cntNzAIneq] = idxOptVarParameter + j;
						valueACone[cntNzAIneq] = -pathGrdK[idxPathGrd];
						temp = temp + valueACone[cntNzAIneq] * refParameter[j];
						cntNzAIneq = cntNzAIneq + 1;
					}
					bCone[cntRowAIneq] = temp - pathNlLower[i];
					cntRowAIneq = cntRowAIneq + 1;
				}
			}
			if (numSocPath)
			{
				for (i = 0; i < numRowSocPath; i++)
				{
					i0 = numState + numNlPath + i;
					temp = -pathRefK[numNlPath + i];
					ptrRowACone[cntRowASoc] = cntNzASoc;
					// xK前面系数
					for (p = stateNzPtrRow[i0]; p < stateNzPtrRow[i0 + 1]; p++)
					{
						j = stateNzIdxCol[p];// 列
						idxPathGrd = j * numRowPath + numNlPath + i;// 第(i, j)个元素
						idxColACone[cntNzASoc] = idxOptVarStateK + j;
						valueACone[cntNzASoc] = pathGrdK[idxPathGrd];
						temp = temp + valueACone[cntNzASoc] * refStateK[j];
						cntNzASoc = cntNzASoc + 1;
					}
					// uK前面系数
					for (p = controlNzPtrRow[i0]; p < controlNzPtrRow[i0 + 1]; p++)
					{
						j = controlNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColControl + j * numRowPath + numNlPath + i;// 第(i, j)个元素
						idxColACone[cntNzASoc] = idxOptVarControlK + j;
						valueACone[cntNzASoc] = pathGrdK[idxPathGrd];
						temp = temp + valueACone[cntNzASoc] * refControlK[j];
						cntNzASoc = cntNzASoc + 1;
					}
					// p前面系数
					for (p = parameterNzPtrRow[i0]; p < parameterNzPtrRow[i0 + 1]; p++)
					{
						j = parameterNzIdxCol[p];// 列
						idxPathGrd = idxPathGrdColParameter + j * numRowPath + numNlPath + i;// 第(i, j)个元素
						idxColACone[cntNzASoc] = idxOptVarParameter + j;
						valueACone[cntNzASoc] = pathGrdK[idxPathGrd];
						temp = temp + valueACone[cntNzASoc] * refParameter[j];
						cntNzASoc = cntNzASoc + 1;
					}
					bCone[cntRowASoc] = temp + pathSocUpper[i];
					cntRowASoc = cntRowASoc + 1;
				}
				for (i = 0; i < numSocPath; i++)
				{
					cntSubASoc = cntSubASoc + 1;
					dimCone->q[cntSubASoc] = dimSocPath[i];
				}
			}
			// dynRefKm1和dynRefK空间互换，实现的功能为dynRefKm1=dynRefK，且不需要另外创建空间;
			tempPtr = dynRefKm1; dynRefKm1 = dynRefK; dynRefK = tempPtr;
			tempPtr = dynGrdKm1; dynGrdKm1 = dynGrdK; dynGrdK = tempPtr;
			tempPtr = pathRefKm1; pathRefKm1 = pathRefK; pathRefK = tempPtr;
			tempPtr = pathGrdKm1; pathGrdKm1 = pathGrdK; pathGrdK = tempPtr;
			// 更新点索引
			idxOptVarStateKm1 = idxOptVarStateK;
			idxOptVarStateK = idxOptVarStateK + numState;
			idxOptVarControlKm1 = idxOptVarControlK;
			idxOptVarControlK = idxOptVarControlK + numControl;
		}
		if (dynRefK) { free(dynRefKm1); }
		if (dynGrdK) { free(dynGrdKm1); }
		if (pathRefK) { free(pathRefKm1); }
		if (pathGrdK) { free(pathGrdKm1); }
		if (dynRefK) { free(dynRefK); }
		if (dynGrdK) { free(dynGrdK); }
		if (pathRefK) { free(pathRefK); }
		if (pathGrdK) { free(pathGrdK); }
		// 更新段索引
		idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
	}
	free(dynGradInfo[0]);
	free(dynGradInfo);
	free(pathGradInfo[0]);
	free(pathGradInfo);
	socpInfo->cntNzAEqua = cntNzAEqua;
	socpInfo->cntRowAEqua = cntRowAEqua;
	socpInfo->cntNzAIneq = cntNzAIneq;
	socpInfo->cntRowAIneq = cntRowAIneq;
	socpInfo->cntNzASoc = cntNzASoc;
	socpInfo->cntRowASoc = cntRowASoc;
	socpInfo->cntSubASoc = cntSubASoc;
}


void socpInfoSparseLink(cmscp_socpInfo** socpInfo0, cmscp_trajInfo** refTraj, cmscp_sparseInfo* sparseInfo, cmscp_consTypeInfo* consTypeInfo, cmscp_setup* setup)
{
	// 函数功能：获取衔接函数约束的二阶锥规划问题参数
	// 输入：
	// socpInfo：二阶锥信息结构体
	// refTraj：参考轨迹信息
	// consTypeInfo：约束类型信息
	// sparseInfo：稀疏（非零元位置）信息
	// setup：问题设置参数
	// 输出：
	// socpInfo：二阶锥信息结构体
	// 谢磊：2022 / 4 / 18编写
	// //基本参数
	int numPhase, * numPhaseOptVar, iphase, cnt, numNlEndp, * idxColAEqua, * ptrRowAEqua, * idxColACone, * ptrRowACone, cntNzAEqua, cntRowAEqua, cntNzAIneq, cntRowAIneq, cntNzASoc, cntRowASoc, cntSubASoc,  nnzType1, i, p, j, i0, numType1, numType2, numType3, numType4, * idxType1, * idxType2, * idxType3, * idxType4,  idxOptVarLeftPhaseStart, idxOptVarRightPhaseStart, numLeftMesh, numRightMesh, numLeftStatef, numLeftControl, numRightState, numRightControl, numLeftParameter, numNlLink, numSocLink, * dimSocLink, * leftStatefPtrRow, * leftTfPtrRow, * leftParameterPtrRow, * rightState0PtrRow, * rightT0PtrRow, * rightParameterPtrRow, * leftStatefIdxCol, * leftParameterIdxCol, * rightState0IdxCol, * rightParameterIdxCol, idxLinkGrdColLeftTf, idxLinkGrdColLeftParameter, idxLinkGrdColRightState0,  idxLinkGrdColRightT0, idxLinkGrdColRightParameter, idxOptVarLeftStatef, idxOptVarLeftTf, idxOptVarLeftParameter, idxOptVarRightState0, idxOptVarRightT0, idxOptVarRightParameter,
		numLinkInput, numLinkOutput;
	double* valueAEqua, * bEqua, * valueACone, * bCone, temp,  * nlLower, * nlUpper, * linkSocUpper, * refLeftStatef, refLeftTf, * refLeftParameter, * refRightState0, refRightT0, * refRightParameter, * linkRef, * linkGrd;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_bound* bounds;
	cmscp_consType** consType;
	cmscp_type* consTypePhase;
	cmscp_nzPos_link** linkNzPos;
	cmscp_derInfo** gradInfo;
	cmscp_socpInfo* socpInfo;
	cmscp_dimCone* dimCone;
	dimension = setup->dimension; // 问题维数信息
	mesh = setup->mesh; // 网格信息
	bounds = setup->bounds; // 上下边界信息
	numPhase = setup->numPhase; // 阶段数
	socpInfo = *socpInfo0;
	numPhaseOptVar = socpInfo->numPhaseOptVar; // 每段优化变量个数
	linkNzPos = sparseInfo->linkNzPos; // 稀疏信息
	consType = consTypeInfo->link; // 约束类型
	// 二阶锥规划参数
	ptrRowAEqua = socpInfo->AEqua->ptrRow;
	idxColAEqua = socpInfo->AEqua->idxCol;
	valueAEqua = socpInfo->AEqua->v;
	bEqua = socpInfo->bEqua;
	ptrRowACone = socpInfo->ACone->ptrRow;
	idxColACone = socpInfo->ACone->idxCol;
	valueACone = socpInfo->ACone->v;
	bCone = socpInfo->bCone;
	dimCone = socpInfo->dimCone;
	// 计数变量
	cntNzAEqua = socpInfo->cntNzAEqua;
	cntRowAEqua = socpInfo->cntRowAEqua;
	cntNzAIneq = socpInfo->cntNzAIneq;
	cntRowAIneq = socpInfo->cntRowAIneq;
	cntNzASoc = socpInfo->cntNzASoc;
	cntRowASoc = socpInfo->cntRowASoc;
	cntSubASoc = socpInfo->cntSubASoc;
	// 梯度信息
	gradInfo = (cmscp_derInfo**)malloc(numPhase * sizeof(cmscp_derInfo));
	gradInfoSparseLink(gradInfo, sparseInfo, refTraj, setup);
	//// 填充非零元素
	// 索引
	idxOptVarLeftPhaseStart = 0; // 各段起始索引
	idxOptVarRightPhaseStart = numPhaseOptVar[0]; // 各段起始索引
	for (iphase = 0; iphase < numPhase - 1; iphase++)
	{
		
		// 维数
		numLeftMesh = mesh[iphase]->num;
		numRightMesh = mesh[iphase + 1]->num;
		numLeftStatef = dimension->phase[iphase]->state;
		numLeftControl = dimension->phase[iphase]->control;
		numRightState = dimension->phase[iphase + 1]->state;
		numRightControl = dimension->phase[iphase + 1]->control;
		numLeftParameter = dimension->phase[iphase]->parameter;
		numNlLink = dimension->phase[iphase]->linkage->nl;
		numSocLink = dimension->phase[iphase]->linkage->soc->n;
		dimSocLink = dimension->phase[iphase]->linkage->soc->q;
		numLinkInput = numLeftStatef + numRightState + numRightState + numLeftParameter + 2;
		if (dimension->phase[iphase]->linkage->soc->n)
		{
			numLinkOutput = dimension->phase[iphase]->linkage->nl + cmscp_sum(dimension->phase[iphase]->linkage->soc->q, dimension->phase[iphase]->linkage->soc->n);
		}
		else
		{
			numLinkOutput = dimension->phase[iphase]->linkage->nl;
		}
		// 稀疏信息
		leftStatefPtrRow = linkNzPos[iphase]->leftStatef->ptrRow;
		leftTfPtrRow = linkNzPos[iphase]->leftTf->ptrRow;
		leftParameterPtrRow = linkNzPos[iphase]->leftParameter->ptrRow;
		rightState0PtrRow = linkNzPos[iphase]->rightState0->ptrRow;
		rightT0PtrRow = linkNzPos[iphase]->rightT0->ptrRow;
		rightParameterPtrRow = linkNzPos[iphase]->rightParameter->ptrRow;
		leftStatefIdxCol = linkNzPos[iphase]->leftStatef->idxCol;
		leftParameterIdxCol = linkNzPos[iphase]->leftParameter->idxCol;
		rightState0IdxCol = linkNzPos[iphase]->rightState0->idxCol;
		rightParameterIdxCol = linkNzPos[iphase]->rightParameter->idxCol;
		// 约束类型简写
		numType1 = consType[iphase]->type[0].num;
		numType2 = consType[iphase]->type[1].num;
		numType3 = consType[iphase]->type[2].num;
		numType4 = consType[iphase]->type[3].num;
		idxType1 = consType[iphase]->type[0].idx;
		idxType2 = consType[iphase]->type[1].idx;
		idxType3 = consType[iphase]->type[2].idx;
		idxType4 = consType[iphase]->type[3].idx;
		// 第1类约束的一半非零元个数
		nnzType1 = getType1LinkNnz(consType[iphase]->type[0], linkNzPos[iphase]);
		// 上下界
		
		nlLower = bounds->phase[iphase]->linkage->nl->lower;
		nlUpper = bounds->phase[iphase]->linkage->nl->upper;
		linkSocUpper = bounds->phase[iphase]->linkage->soc;
		// 参考轨迹
		refLeftStatef = refTraj[iphase]->state->v + (numLeftMesh - 1) * numLeftStatef;
		refLeftTf = refTraj[iphase]->finaltime;
		refLeftParameter = refTraj[iphase]->parameter;
		refRightState0 = refTraj[iphase + 1]->state->v;
		refRightT0 = refTraj[iphase + 1]->initialtime;
		refRightParameter = refTraj[iphase + 1]->parameter;
		// 梯度信息
		linkRef = gradInfo[iphase]->ref;
		linkGrd = gradInfo[iphase]->der;
		// 索引
		idxLinkGrdColLeftTf = numLeftStatef;
		idxLinkGrdColLeftParameter = idxLinkGrdColLeftTf + 1;
		idxLinkGrdColRightState0 = idxLinkGrdColLeftParameter + numLeftParameter;
		idxLinkGrdColRightT0 = idxLinkGrdColRightState0 + numRightState; 
		idxLinkGrdColRightParameter = idxLinkGrdColRightT0 + 1;
		idxOptVarLeftStatef = idxOptVarLeftPhaseStart + (numLeftMesh - 1) * numLeftStatef;
		idxOptVarLeftTf = idxOptVarLeftPhaseStart + numLeftMesh * (numLeftStatef + numLeftControl) + 1;
		idxOptVarLeftParameter = idxOptVarLeftTf + 1;
		idxOptVarRightState0 = idxOptVarRightPhaseStart;
		idxOptVarRightT0 = idxOptVarRightPhaseStart + numRightMesh * (numRightState + numRightControl);
		idxOptVarRightParameter = idxOptVarRightT0 + 2;
		// 双不等式约束项，表达式：
		if (numType1)
		{
			// 上界:A * xfl + B * tfl + C * pl + D * x0r + E * t0r + F * pr <= b
			for (cnt = 0; cnt < numType1; cnt++) {
				i = idxType1[cnt];
				temp = -linkRef[i];
				// 每行首个非零元的位置
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				ptrRowACone[cntRowAIneq + numType1] = cntNzAIneq + nnzType1;
				// 左侧xf前面系数
				for (p = leftStatefPtrRow[i]; p < leftStatefPtrRow[i + 1]; p++)
				{
					j = leftStatefIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarLeftStatef + j;
					valueACone[cntNzAIneq] = linkGrd[i + j* numLinkOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refLeftStatef[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 左侧tf前面系数
				if (leftTfPtrRow[i] + 1 == leftTfPtrRow[i + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarLeftTf;
					valueACone[cntNzAIneq] = linkGrd[i + idxLinkGrdColLeftTf * numLinkOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refLeftTf;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 左侧p前面系数
				for (p = leftParameterPtrRow[i]; p < leftParameterPtrRow[i + 1]; p++)
				{
					j = leftParameterIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarLeftParameter + j;
					valueACone[cntNzAIneq] = linkGrd[i + (idxLinkGrdColLeftParameter + j) * numLinkOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refLeftParameter[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 右侧x0前面系数
				for (p = rightState0PtrRow[i]; p < rightState0PtrRow[i + 1]; p++)
				{
					j = rightState0IdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarRightState0 + j;
					valueACone[cntNzAIneq] = linkGrd[i + (idxLinkGrdColRightState0 + j) * numLinkOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refRightState0[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 右侧t0前面系数
				if (rightT0PtrRow[i] + 1 == rightT0PtrRow[i + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarRightT0;
					valueACone[cntNzAIneq] = linkGrd[i + idxLinkGrdColRightT0 * numLinkOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refRightT0;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 右侧p前面系数
				for (p = rightParameterPtrRow[i]; p < rightParameterPtrRow[i + 1]; p++) {
					j = rightParameterIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarRightParameter + j;
					valueACone[cntNzAIneq] = linkGrd[i + (idxLinkGrdColRightParameter + j) * numLinkOutput];
					idxColACone[cntNzAIneq + nnzType1] = idxColACone[cntNzAIneq];
					valueACone[cntNzAIneq + nnzType1] = -valueACone[cntNzAIneq];
					temp = temp + valueACone[cntNzAIneq] * refRightParameter[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				bCone[cntRowAIneq] = temp + nlUpper[i];
				bCone[cntRowAIneq] = -temp - nlLower[i];
				cntRowAIneq = cntRowAIneq + 1;
			}
		}
		// 等式约束项，表达式：
		// A* xfl + B * tfl + C * pl + D * x0r + E * t0r + F * pr == b
		if (numType2)
		{
			for (cnt = 0; cnt < numType2; cnt++) {
				i = idxType2[cnt];
				temp = -linkRef[i];
				// 每行首个非零元的位置
				ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
				// 左侧xf前面系数
				for (p = leftStatefPtrRow[i]; p < leftStatefPtrRow[i + 1]; p++)
				{
					j = leftStatefIdxCol[p]; // 列
					idxColAEqua[cntNzAEqua] = idxOptVarLeftStatef + j;
					valueAEqua[cntNzAEqua] = linkGrd[i + j * numLinkOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refLeftStatef[j];
					cntNzAEqua = cntNzAEqua + 1;
				}
				// 左侧tf前面系数
				if (leftTfPtrRow[i] + 1 == leftTfPtrRow[i + 1])
				{
					idxColAEqua[cntNzAEqua] = idxOptVarLeftTf;
					valueAEqua[cntNzAEqua] = linkGrd[i + (idxLinkGrdColLeftTf) * numLinkOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refLeftTf;
					cntNzAEqua = cntNzAEqua + 1;
				}
				// 左侧p前面系数
				for (p = leftParameterPtrRow[i]; p < leftParameterPtrRow[i + 1]; p++)
				{
					j = leftParameterIdxCol[p]; // 列
					idxColAEqua[cntNzAEqua] = idxOptVarLeftParameter + j;
					valueAEqua[cntNzAEqua] = linkGrd[i + (idxLinkGrdColLeftParameter + j) * numLinkOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refLeftParameter[j];
					cntNzAEqua = cntNzAEqua + 1;
				}
				// 右侧x0前面系数
				for (p = rightState0PtrRow[i]; p < rightState0PtrRow[i + 1]; p++) {
					j = rightState0IdxCol[p]; // 列
					idxColAEqua[cntNzAEqua] = idxOptVarRightState0 + j;
					valueAEqua[cntNzAEqua] = linkGrd[i + (idxLinkGrdColRightState0 + j) * numLinkOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refRightState0[j];
					cntNzAEqua = cntNzAEqua + 1;
				}
				// 右侧t0前面系数
				if (rightT0PtrRow[i] + 1 == rightT0PtrRow[i + 1]) {
					idxColAEqua[cntNzAEqua] = idxOptVarRightT0;
					valueAEqua[cntNzAEqua] = linkGrd[i + idxLinkGrdColRightT0 * numLinkOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refRightT0;
					cntNzAEqua = cntNzAEqua + 1;
				}
				// 右侧p前面系数
				for (p = rightParameterPtrRow[i]; p < rightParameterPtrRow[i + 1]; p++)
				{
					j = rightParameterIdxCol[p]; // 列
					idxColAEqua[cntNzAEqua] = idxOptVarRightParameter + j;
					valueAEqua[cntNzAEqua] = linkGrd[i + (idxLinkGrdColRightParameter + j) * numLinkOutput];
					temp = temp + valueAEqua[cntNzAEqua] * refRightParameter[j];
					cntNzAEqua = cntNzAEqua + 1;
				}
				bEqua[cntRowAEqua] = temp + nlUpper[i];
				cntRowAEqua = cntRowAEqua + 1;
			}
		}
		// 上不等式约束项，表达式：
		// A* xfl + B * tfl + C * pl + D * x0r + E * t0r + F * pr <= b
		if (numType3)
		{
			for (cnt = 0; cnt < numType3; cnt++) {
				i = idxType3[cnt];
				temp = -linkRef[i];
				// 每行首个非零元的位置
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				// 左侧xf前面系数
				for (p = leftStatefPtrRow[i]; p = leftStatefPtrRow[i + 1]; p++) {
					j = leftStatefIdxCol[p]; // 列
					cntNzAIneq = cntNzAIneq + 1;
					idxColACone[cntNzAIneq] = idxOptVarLeftStatef + j;
					valueACone[cntNzAIneq] = linkGrd[i + j * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refLeftStatef[j];
				}
				// 左侧tf前面系数
				if (leftTfPtrRow[i] + 1 == leftTfPtrRow[i + 1])
				{
					cntNzAIneq = cntNzAIneq + 1;
					idxColACone[cntNzAIneq] = idxOptVarLeftTf;
					valueACone[cntNzAIneq] = linkGrd[i + idxLinkGrdColLeftTf * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refLeftTf;
				}
				// 左侧p前面系数
				for (p = leftParameterPtrRow[i]; p < leftParameterPtrRow[i + 1]; p++) {
					j = leftParameterIdxCol[p]; // 列
					cntNzAIneq = cntNzAIneq + 1;
					idxColACone[cntNzAIneq] = idxOptVarLeftParameter + j;
					valueACone[cntNzAIneq] = linkGrd[i + (idxLinkGrdColLeftParameter + j) * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refLeftParameter[j];
				}
				// 右侧x0前面系数
				for (p = rightState0PtrRow[i]; p < rightState0PtrRow[i + 1]; p++) {
					j = rightState0IdxCol[p]; // 列
					cntNzAIneq = cntNzAIneq + 1;
					idxColACone[cntNzAIneq] = idxOptVarRightState0 + j;
					valueACone[cntNzAIneq] = linkGrd[i + (idxLinkGrdColRightState0 + j) * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refRightState0[j];
				}
				// 右侧t0前面系数
				if (rightT0PtrRow[i] + 1 == rightT0PtrRow[i + 1])
				{
					cntNzAIneq = cntNzAIneq + 1;
					idxColACone[cntNzAIneq] = idxOptVarRightT0 + 1;
					valueACone[cntNzAIneq] = linkGrd[i + idxLinkGrdColRightT0 * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refRightT0;
				}
				// 右侧p前面系数
				for (p = rightParameterPtrRow[i]; p < rightParameterPtrRow[i + 1]; p++)
				{
					j = rightParameterIdxCol[p]; // 列
					cntNzAIneq = cntNzAIneq + 1;
					idxColACone[cntNzAIneq] = idxOptVarRightParameter + j;
					valueACone[cntNzAIneq] = linkGrd[i + (idxLinkGrdColRightParameter + j) * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refRightParameter[j];
				}
				bCone[cntRowAIneq] = temp + nlUpper[i];
				cntRowAIneq = cntRowAIneq + 1;
			}
		}
		// 下不等式约束项，表达式：
		// -A * xfl - B * tfl - C * pl - D * x0r - E * t0r - F * pr <= -b
		if (numType4)
		{
			for (cnt = 0; cnt < numType4; cnt++)
			{
				i = idxType4[cnt];
				temp = linkRef[i];
				// 每行首个非零元的位置
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				// 左侧xf前面系数
				for (p = leftStatefPtrRow[i]; p < leftStatefPtrRow[i + 1]; p++)
				{
					j = leftStatefIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarLeftStatef + j;
					valueACone[cntNzAIneq] = -linkGrd[i + j * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refLeftStatef[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 左侧tf前面系数
				if (leftTfPtrRow[i] + 1 == leftTfPtrRow[i + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarLeftTf;
					valueACone[cntNzAIneq] = -linkGrd[i + idxLinkGrdColLeftTf * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refLeftTf;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 左侧p前面系数
				for (p = leftParameterPtrRow[i]; p < leftParameterPtrRow[i + 1]; p++)
				{
					j = leftParameterIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarLeftParameter + j;
					valueACone[cntNzAIneq] = -linkGrd[i + (idxLinkGrdColLeftParameter + j) * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refLeftParameter[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 右侧x0前面系数
				for (p = rightState0PtrRow[i]; p < rightState0PtrRow[i + 1]; p++)
				{
					j = rightState0IdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarRightState0 + j;
					valueACone[cntNzAIneq] = -linkGrd[i + (idxLinkGrdColRightState0 + j) * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refRightState0[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 右侧t0前面系数
				if (rightT0PtrRow[i] + 1 == rightT0PtrRow[i + 1])
				{
					idxColACone[cntNzAIneq] = idxOptVarRightT0;
					valueACone[cntNzAIneq] = -linkGrd[i + idxLinkGrdColRightT0 * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refRightT0;
					cntNzAIneq = cntNzAIneq + 1;
				}
				// 右侧p前面系数
				for (p = rightParameterPtrRow[i]; p < rightParameterPtrRow[i + 1]; p++)
				{
					j = rightParameterIdxCol[p]; // 列
					idxColACone[cntNzAIneq] = idxOptVarRightParameter + j;
					valueACone[cntNzAIneq] = -linkGrd[i + (idxLinkGrdColRightParameter + j) * numLinkOutput];
					temp = temp + valueACone[cntNzAIneq] * refRightParameter[j];
					cntNzAIneq = cntNzAIneq + 1;
				}
				bCone[cntRowAIneq] = temp - nlLower[i];
				cntRowAIneq = cntRowAIneq + 1;
			}
		}
		if (numSocLink)
		{
			for (i = 0; i < cmscp_sum(dimSocLink, numSocLink); i++)
			{
				cntRowASoc = cntRowASoc + 1;
				i0 = i + numNlLink;
				temp = -linkRef[i0];
				ptrRowACone[cntRowASoc] = cntNzASoc;
				// 左侧xf前面系数
				for (p = leftStatefPtrRow[i0]; p < leftStatefPtrRow[i0 + 1]; p++)
				{
					j = leftStatefIdxCol[p]; // 列
					idxColACone[cntNzASoc] = idxOptVarLeftStatef + j;
					valueACone[cntNzASoc] = linkGrd[i0 + j * numLinkOutput];
					temp = temp + valueACone[cntNzASoc] * refLeftStatef[j];
					cntNzASoc = cntNzASoc + 1;
				}
				// 左侧tf前面系数
				if (leftTfPtrRow[i0] + 1 == leftTfPtrRow[i0 + 1])
				{
					idxColACone[cntNzASoc] = idxOptVarLeftTf;
					valueACone[cntNzASoc] = linkGrd[i0 + idxLinkGrdColLeftTf * numLinkOutput];
					temp = temp + valueACone[cntNzASoc] * refLeftTf;
					cntNzASoc = cntNzASoc + 1;
				}
				// 左侧p前面系数
				for (p = leftParameterPtrRow[i0]; p < leftParameterPtrRow[i0 + 1]; p++)
				{
					j = leftParameterIdxCol[p]; // 列
					idxColACone[cntNzASoc] = idxOptVarLeftParameter + j;
					valueACone[cntNzASoc] = linkGrd[i0 + (idxLinkGrdColLeftParameter + j) * numLinkOutput];
					temp = temp + valueACone[cntNzASoc] * refLeftParameter[j];
					cntNzASoc = cntNzASoc + 1;
				}
				// 右侧x0前面系数
				for (p = rightState0PtrRow[i0]; p < rightState0PtrRow[i0 + 1]; p++)
				{
					j = rightState0IdxCol[p]; // 列
					idxColACone[cntNzASoc] = idxOptVarRightState0 + j;
					valueACone[cntNzASoc] = linkGrd[i0 + (idxLinkGrdColRightState0 + j) * numLinkOutput];
					temp = temp + valueACone[cntNzASoc] * refRightState0[j];
					cntNzASoc = cntNzASoc + 1;
				}
				// 右侧t0前面系数
				if (rightT0PtrRow[i0] + 1 == rightT0PtrRow[i0 + 1])
				{
					idxColACone[cntNzASoc] = idxOptVarRightT0 + 1;
					valueACone[cntNzASoc] = linkGrd[i0 + idxLinkGrdColRightT0 * numLinkOutput];
					temp = temp + valueACone[cntNzASoc] * refRightT0;
					cntNzASoc = cntNzASoc + 1;
				}
				// 右侧p前面系数
				for (p = rightParameterPtrRow[i0]; p < rightParameterPtrRow[i0 + 1]; p++)
				{
					j = rightParameterIdxCol[p]; // 列
					idxColACone[cntNzASoc] = idxOptVarRightParameter + j;
					valueACone[cntNzASoc] = linkGrd[i0 + (idxLinkGrdColRightParameter + j) * numLinkOutput];
					temp = temp + valueACone[cntNzASoc] * refRightParameter[j];
					cntNzASoc = cntNzASoc + 1;
				}
				bCone[cntRowASoc] = temp + linkSocUpper[i];
				cntRowAIneq = cntRowAIneq + 1;
			}
			for (i = 0; i < numSocLink; i++)
			{
				cntSubASoc = cntSubASoc + 1;
				dimCone->q[cntSubASoc] = dimSocLink[i];
			}
		}
		idxOptVarLeftPhaseStart = idxOptVarRightPhaseStart;
		idxOptVarRightPhaseStart = idxOptVarRightPhaseStart + numPhaseOptVar[iphase + 1];
	}
	for ( iphase = 0; iphase < numPhase-1; iphase++)
	{
		free(gradInfo[iphase]->ref);
		free(gradInfo[iphase]->der);
		free(gradInfo[iphase]);
	}
	free(gradInfo);


	socpInfo->cntNzAEqua = cntNzAEqua;
	socpInfo->cntRowAEqua = cntRowAEqua;
	socpInfo->cntNzAIneq = cntNzAIneq;
	socpInfo->cntRowAIneq = cntRowAIneq;
	socpInfo->cntNzASoc = cntNzASoc;
	socpInfo->cntRowASoc = cntRowASoc;
	socpInfo->cntSubASoc = cntSubASoc;
}


void socpInfoBound(cmscp_socpInfo** socpInfo0, cmscp_consTypeInfo* consTypeInfo, cmscp_setup* setup) {
	// socpInfo: 已有的二阶锥信息
	// refTraj : 参考轨迹信息
	// gradInfo : 梯度信息
	// setup : 设置信息
	int numPhase, * numPhaseOptVar, iphase, numMesh, numState, numControl, cnt, * idxColAEqua, * ptrRowAEqua, * idxColACone, * ptrRowACone, cntNzAEqua, cntRowAEqua, cntNzAIneq, cntRowAIneq, cntNzASoc, cntRowASoc, idxOptVarPhaseStart, idxOptVarState0, idxOptVarStatef, idxOptVarT0, idxOptVarTf, idxOptVarParameter, i, idxOptVarStateK, idxOptVarControlK, k, numOptVar, numIniTimeType1, numIniTimeType2, numIniTimeType3, numIniTimeType4, * idxIniTimeType1, * idxIniTimeType2, * idxIniTimeType3, * idxIniTimeType4, numFinTimeType1, numFinTimeType2, numFinTimeType3, numFinTimeType4, * idxFinTimeType1, * idxFinTimeType2, * idxFinTimeType3, * idxFinTimeType4, numIniStateType1, numIniStateType2, numIniStateType3, numIniStateType4, * idxIniStateType1, * idxIniStateType2, * idxIniStateType3, * idxIniStateType4, numFinStateType1, numFinStateType2, numFinStateType3, numFinStateType4, * idxFinStateType1, * idxFinStateType2, * idxFinStateType3, * idxFinStateType4, numStateType1, numStateType2, numStateType3, numStateType4, * idxStateType1, * idxStateType2, * idxStateType3, * idxStateType4, numControlType1, numControlType2, numControlType3, numControlType4, * idxControlType1, * idxControlType2, * idxControlType3, * idxControlType4, numParameterType1, numParameterType2, numParameterType3, numParameterType4, * idxParameterType1, * idxParameterType2, * idxParameterType3, * idxParameterType4;
	double* valueAEqua, * bEqua, * valueACone, * bCone, iniTimeLower, iniTimeUpper, finTimeLower, finTimeUpper, * iniStateLower, * iniStateUpper, * finStateLower, * finStateUpper, * stateLower, * stateUpper, * controlLower, * controlUpper, * parameterLower, * parameterUpper;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_bound* bounds;
	cmscp_consType_bound* consType;
	cmscp_socpInfo* socpInfo;
	cmscp_dimCone* dimCone;
	cmscp_consType** iniTimeType, ** finTimeType, ** iniStateType, ** finStateType, ** stateType, ** controlType, ** parameterType;
	// 1 基本参数
	dimension = setup->dimension;// 问题维数信息
	mesh = setup->mesh;// 网格信息
	bounds = setup->bounds;// 上下边界信息
	numPhase = setup->numPhase;// 阶段数
	socpInfo = *socpInfo0;
	numOptVar = socpInfo->numOptVar;// 总优化变量个数
	numPhaseOptVar = socpInfo->numPhaseOptVar;// 每段优化变量个数
	consType = consTypeInfo->bound;// 约束类型
	iniTimeType = consType->iniTime;
	finTimeType = consType->finTime;
	iniStateType = consType->iniState;
	finStateType = consType->finState;
	stateType = consType->state;
	controlType = consType->control;
	parameterType = consType->parameter;
	// 二阶锥规划参数
	ptrRowAEqua = socpInfo->AEqua->ptrRow;
	idxColAEqua = socpInfo->AEqua->idxCol;
	valueAEqua = socpInfo->AEqua->v;
	bEqua = socpInfo->bEqua;
	ptrRowACone = socpInfo->ACone->ptrRow;
	idxColACone = socpInfo->ACone->idxCol;
	valueACone = socpInfo->ACone->v;
	bCone = socpInfo->bCone;
	dimCone = socpInfo->dimCone;
	// 计数变量
	cntNzAEqua = socpInfo->cntNzAEqua;
	cntRowAEqua = socpInfo->cntRowAEqua;
	cntNzAIneq = socpInfo->cntNzAIneq;
	cntRowAIneq = socpInfo->cntRowAIneq;
	cntNzASoc = socpInfo->cntNzASoc;
	cntRowASoc = socpInfo->cntRowASoc;

	// 2 填充非零元素
	// 各段起始变量索引
	idxOptVarPhaseStart = 0;
	for (iphase = 0; iphase < numPhase; iphase++) {
		// 维数
		numMesh = mesh[iphase]->num;
		numState = dimension->phase[iphase]->state;
		numControl = dimension->phase[iphase]->control;
		// 初始时刻约束类型简写
		numIniTimeType1 = iniTimeType[iphase]->type[0].num;
		numIniTimeType2 = iniTimeType[iphase]->type[1].num;
		numIniTimeType3 = iniTimeType[iphase]->type[2].num;
		numIniTimeType4 = iniTimeType[iphase]->type[3].num;
		idxIniTimeType1 = iniTimeType[iphase]->type[0].idx;
		idxIniTimeType2 = iniTimeType[iphase]->type[1].idx;
		idxIniTimeType3 = iniTimeType[iphase]->type[2].idx;
		idxIniTimeType4 = iniTimeType[iphase]->type[3].idx;
		// 终端时刻约束类型简写
		numFinTimeType1 = finTimeType[iphase]->type[0].num;
		numFinTimeType2 = finTimeType[iphase]->type[1].num;
		numFinTimeType3 = finTimeType[iphase]->type[2].num;
		numFinTimeType4 = finTimeType[iphase]->type[3].num;
		idxFinTimeType1 = finTimeType[iphase]->type[0].idx;
		idxFinTimeType2 = finTimeType[iphase]->type[1].idx;
		idxFinTimeType3 = finTimeType[iphase]->type[2].idx;
		idxFinTimeType4 = finTimeType[iphase]->type[3].idx;
		// 初始状态约束类型简写
		numIniStateType1 = iniStateType[iphase]->type[0].num;
		numIniStateType2 = iniStateType[iphase]->type[1].num;
		numIniStateType3 = iniStateType[iphase]->type[2].num;
		numIniStateType4 = iniStateType[iphase]->type[3].num;
		idxIniStateType1 = iniStateType[iphase]->type[0].idx;
		idxIniStateType2 = iniStateType[iphase]->type[1].idx;
		idxIniStateType3 = iniStateType[iphase]->type[2].idx;
		idxIniStateType4 = iniStateType[iphase]->type[3].idx;
		// 终端状态约束类型简写
		numFinStateType1 = finStateType[iphase]->type[0].num;
		numFinStateType2 = finStateType[iphase]->type[1].num;
		numFinStateType3 = finStateType[iphase]->type[2].num;
		numFinStateType4 = finStateType[iphase]->type[3].num;
		idxFinStateType1 = finStateType[iphase]->type[0].idx;
		idxFinStateType2 = finStateType[iphase]->type[1].idx;
		idxFinStateType3 = finStateType[iphase]->type[2].idx;
		idxFinStateType4 = finStateType[iphase]->type[3].idx;
		// 状态约束类型简写
		numStateType1 = stateType[iphase]->type[0].num;
		numStateType2 = stateType[iphase]->type[1].num;
		numStateType3 = stateType[iphase]->type[2].num;
		numStateType4 = stateType[iphase]->type[3].num;
		idxStateType1 = stateType[iphase]->type[0].idx;
		idxStateType2 = stateType[iphase]->type[1].idx;
		idxStateType3 = stateType[iphase]->type[2].idx;
		idxStateType4 = stateType[iphase]->type[3].idx;
		// 控制约束类型简写

		numControlType1 = controlType[iphase]->type[0].num;
		numControlType2 = controlType[iphase]->type[1].num;
		numControlType3 = controlType[iphase]->type[2].num;
		numControlType4 = controlType[iphase]->type[3].num;
		idxControlType1 = controlType[iphase]->type[0].idx;
		idxControlType2 = controlType[iphase]->type[1].idx;
		idxControlType3 = controlType[iphase]->type[2].idx;
		idxControlType4 = controlType[iphase]->type[3].idx;

		// 静态变量约束类型简写
		numParameterType1 = parameterType[iphase]->type[0].num;
		numParameterType2 = parameterType[iphase]->type[1].num;
		numParameterType3 = parameterType[iphase]->type[2].num;
		numParameterType4 = parameterType[iphase]->type[3].num;
		idxParameterType1 = parameterType[iphase]->type[0].idx;
		idxParameterType2 = parameterType[iphase]->type[1].idx;
		idxParameterType3 = parameterType[iphase]->type[2].idx;
		idxParameterType4 = parameterType[iphase]->type[3].idx;
		// 初始和终端时刻约束边界简写
		iniTimeLower = bounds->phase[iphase]->initialtime->lower;
		iniTimeUpper = bounds->phase[iphase]->initialtime->upper;
		finTimeLower = bounds->phase[iphase]->finaltime->lower;
		finTimeUpper = bounds->phase[iphase]->finaltime->upper;
		// 初始、终端和过程状态约束边界简写
		iniStateLower = bounds->phase[iphase]->initialstate->lower;
		iniStateUpper = bounds->phase[iphase]->initialstate->upper;
		finStateLower = bounds->phase[iphase]->finalstate->lower;
		finStateUpper = bounds->phase[iphase]->finalstate->upper;
		stateLower = bounds->phase[iphase]->state->lower;
		stateUpper = bounds->phase[iphase]->state->upper;
		// 控制约束边界简写
		controlLower = bounds->phase[iphase]->control->lower;
		controlUpper = bounds->phase[iphase]->control->upper;
		// 静态变量约束边界简写
		parameterLower = bounds->phase[iphase]->parameter->lower;
		parameterUpper = bounds->phase[iphase]->parameter->upper;

		// 各个变量索引
		idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);
		idxOptVarTf = idxOptVarT0 + 1;
		idxOptVarParameter = idxOptVarTf + 1;
		// 初始状态变量
		idxOptVarState0 = idxOptVarPhaseStart;
		if (numIniStateType1) {
			for (cnt = 0; cnt < numIniStateType1; cnt++) {
				i = idxIniStateType1[cnt];
				// xK <= bUpper
				bCone[cntRowAIneq] = iniStateUpper[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarState0 + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
				// -xK <= -bLower
				bCone[cntRowAIneq] = -iniStateLower[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarState0 + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		if (numIniStateType2) {
			for (cnt = 0; cnt < numIniStateType2; cnt++) {
				i = idxIniStateType2[cnt];
				// xK == bUpper
				bEqua[cntRowAEqua] = iniStateUpper[i];
				ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
				idxColAEqua[cntNzAEqua] = idxOptVarState0 + i;
				valueAEqua[cntNzAEqua] = 1;
				cntRowAEqua = cntRowAEqua + 1;
				cntNzAEqua = cntNzAEqua + 1;
			}
		}
		if (numIniStateType3) {
			for (cnt = 0; cnt < numIniStateType3; cnt++) {
				i = idxIniStateType3[cnt];
				// xK <= bUpper
				bCone[cntRowAIneq] = iniStateUpper[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarState0 + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		if (numIniStateType4) {
			for (cnt = 0; cnt < numIniStateType4; cnt++) {
				i = idxIniStateType4[cnt];
				// -xK <= -bLower
				bCone[cntRowAIneq] = -iniStateLower[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarState0 + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}

		// 过程状态
		idxOptVarStateK = idxOptVarPhaseStart + numState;
		if (numStateType1 != 0) {
			for (k = 1; k < numMesh - 1; k++) {
				for (cnt = 0; cnt < numStateType1; cnt++) {
					i = idxStateType1[cnt];
					// xK <= bUpper
					bCone[cntRowAIneq] = stateUpper[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					idxColACone[cntNzAIneq] = idxOptVarStateK + i;
					valueACone[cntNzAIneq] = 1.0;
					cntRowAIneq = cntRowAIneq + 1;
					cntNzAIneq = cntNzAIneq + 1;
					// -xK <= -bLower
					bCone[cntRowAIneq] = -stateLower[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					idxColACone[cntNzAIneq] = idxOptVarStateK + i;
					valueACone[cntNzAIneq] = -1.0;
					cntRowAIneq = cntRowAIneq + 1;
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxOptVarStateK = idxOptVarStateK + numState;
			}
		}
		idxOptVarStateK = idxOptVarPhaseStart + numState;
		if (numStateType2 != 0) {
			for (k = 1; k < numMesh - 1; k++) {
				for (cnt = 0; cnt < numStateType2; cnt++) {
					i = idxStateType2[cnt];
					// xK == bUpper
					bEqua[cntRowAEqua] = stateUpper[i];
					ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
					idxColAEqua[cntNzAEqua] = idxOptVarStateK + i;
					valueAEqua[cntNzAEqua] = 1;
					cntRowAEqua = cntRowAEqua + 1;
					cntNzAEqua = cntNzAEqua + 1;
				}
				idxOptVarStateK = idxOptVarStateK + numState;
			}
		}
		idxOptVarStateK = idxOptVarPhaseStart + numState;
		if (numStateType3 != 0) {
			for (k = 1; k < numMesh - 1; k++) {
				for (cnt = 0; cnt < numStateType3; cnt++) {
					i = idxStateType3[cnt];
					// xK <= bUpper
					bCone[cntRowAIneq] = stateUpper[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					idxColACone[cntNzAIneq] = idxOptVarStateK + i;
					valueACone[cntNzAIneq] = 1.0;
					cntRowAIneq = cntRowAIneq + 1;
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxOptVarStateK = idxOptVarStateK + numState;
			}
		}
		idxOptVarStateK = idxOptVarPhaseStart + numState;
		if (numStateType4 != 0) {
			for (k = 1; k < numMesh - 1; k++) {
				for (cnt = 0; cnt < numStateType4; cnt++) {
					i = idxStateType4[cnt];
					// -xK <= -bLower
					bCone[cntRowAIneq] = -stateLower[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					idxColACone[cntNzAIneq] = idxOptVarStateK + i;
					valueACone[cntNzAIneq] = -1.0;
					cntRowAIneq = cntRowAIneq + 1;
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxOptVarStateK = idxOptVarStateK + numState;
			}
		}
		// 终端状态
		idxOptVarStatef = idxOptVarPhaseStart + (numMesh - 1) * numState;
		if (numFinStateType1 != 0) {
			for (cnt = 0; cnt < numFinStateType1; cnt++) {
				i = idxFinStateType1[cnt];
				// xK <= bUpper
				bCone[cntRowAIneq] = finStateUpper[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarStatef + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;

				// -xK <= -bLower
				bCone[cntRowAIneq] = -finStateLower[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarStatef + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		if (numFinStateType2 != 0) {
			for (cnt = 0; cnt < numFinStateType2; cnt++) {
				i = idxFinStateType2[cnt];
				// xK == bUpper
				bEqua[cntRowAEqua] = finStateUpper[i];
				ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
				idxColAEqua[cntNzAEqua] = idxOptVarStatef + i;
				valueAEqua[cntNzAEqua] = 1;
				cntRowAEqua = cntRowAEqua + 1;
				cntNzAEqua = cntNzAEqua + 1;
			}
		}
		if (numFinStateType3 != 0) {
			for (cnt = 0; cnt < numFinStateType3; cnt++) {
				i = idxFinStateType3[cnt];
				// xK <= bUpper
				bCone[cntRowAIneq] = finStateUpper[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarStatef + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		if (numFinStateType4 != 0) {
			for (cnt = 0; cnt < numFinStateType4; cnt++) {
				i = idxFinStateType4[cnt];
				// -xK <= -bLower
				bCone[cntRowAIneq] = -finStateLower[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarStatef + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		// 控制变量
		idxOptVarControlK = idxOptVarPhaseStart + numMesh * numState;
		if (numControlType1 != 0) {
			for (k = 0; k < numMesh; k++) {
				for (cnt = 0; cnt < numControlType1; cnt++) {
					i = idxControlType1[cnt];
					// xK <= bUpper
					bCone[cntRowAIneq] = controlUpper[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					idxColACone[cntNzAIneq] = idxOptVarControlK + i;
					valueACone[cntNzAIneq] = 1.0;
					cntRowAIneq = cntRowAIneq + 1;
					cntNzAIneq = cntNzAIneq + 1;
					// -xK <= -bLower
					bCone[cntRowAIneq] = -controlLower[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					idxColACone[cntNzAIneq] = idxOptVarControlK + i;
					valueACone[cntNzAIneq] = -1.0;
					cntRowAIneq = cntRowAIneq + 1;
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxOptVarControlK = idxOptVarControlK + numControl;
			}
		}
		idxOptVarControlK = idxOptVarPhaseStart + numMesh * numState;
		if (numControlType2 != 0) {
			for (k = 0; k < numMesh; k++) {
				for (cnt = 0; cnt < numControlType2; cnt++) {
					i = idxControlType2[cnt];
					// xK == bUpper
					bEqua[cntRowAEqua] = controlUpper[i];
					ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
					idxColAEqua[cntNzAEqua] = idxOptVarControlK + i;
					valueAEqua[cntNzAEqua] = 1;
					cntRowAEqua = cntRowAEqua + 1;
					cntNzAEqua = cntNzAEqua + 1;
				}
				idxOptVarControlK = idxOptVarControlK + numControl;
			}
		}
		idxOptVarControlK = idxOptVarPhaseStart + numMesh * numState;
		if (numControlType3 != 0) {
			for (k = 0; k < numMesh; k++) {
				for (cnt = 0; cnt < numControlType3; cnt++) {
					i = idxControlType3[cnt];
					// xK <= bUpper
					bCone[cntRowAIneq] = controlUpper[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					idxColACone[cntNzAIneq] = idxOptVarControlK + i;
					valueACone[cntNzAIneq] = 1.0;
					cntRowAIneq = cntRowAIneq + 1;
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxOptVarControlK = idxOptVarControlK + numControl;
			}
		}
		idxOptVarControlK = idxOptVarPhaseStart + numMesh * numState;
		if (numControlType4 != 0) {
			for (k = 0; k < numMesh; k++) {
				for (cnt = 0; cnt < numControlType4; cnt++) {
					i = idxControlType4[cnt];
					// -xK <= -bLower
					bCone[cntRowAIneq] = -controlLower[i];
					ptrRowACone[cntRowAIneq] = cntNzAIneq;
					idxColACone[cntNzAIneq] = idxOptVarControlK + i;
					valueACone[cntNzAIneq] = -1.0;
					cntRowAIneq = cntRowAIneq + 1;
					cntNzAIneq = cntNzAIneq + 1;
				}
				idxOptVarControlK = idxOptVarControlK + numControl;
			}
		}
		// 初始时刻
		if (numIniTimeType1 != 0) {
			for (cnt = 0; cnt < numIniTimeType1; cnt++) {
				i = idxIniTimeType1[cnt];
				// t0 <= bUpper
				bCone[cntRowAIneq] = iniTimeUpper;
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarT0 + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;

				// -t0 <= -bLower
				bCone[cntRowAIneq] = -iniTimeLower;
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarT0 + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		if (numIniTimeType2 != 0) {
			for (cnt = 0; cnt < numIniTimeType2; cnt++) {
				i = idxIniTimeType2[cnt];
				// t0 == bUpper
				bEqua[cntRowAEqua] = iniTimeUpper;
				ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
				idxColAEqua[cntNzAEqua] = idxOptVarT0 + i;
				valueAEqua[cntNzAEqua] = 1;
				cntRowAEqua = cntRowAEqua + 1;
				cntNzAEqua = cntNzAEqua + 1;
			}
		}
		if (numIniTimeType3 != 0) {
			for (cnt = 0; cnt < numIniTimeType3; cnt++) {
				i = idxIniTimeType3[cnt];
				// t0 <= bUpper
				bCone[cntRowAIneq] = iniTimeUpper;
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarT0 + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		if (numIniTimeType4 != 0) {
			for (cnt = 0; cnt < numIniTimeType4; cnt++) {
				i = idxIniTimeType4[cnt];
				// -t0 <= -bLower
				bCone[cntRowAIneq] = -iniTimeLower;
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarT0 + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		// 终端时刻
		if (numFinTimeType1 != 0) {
			for (cnt = 0; cnt < numFinTimeType1; cnt++) {
				i = idxFinTimeType1[cnt];
				// tf <= bUpper
				bCone[cntRowAIneq] = finTimeUpper;
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarTf + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;

				// -tf <= -bLower
				bCone[cntRowAIneq] = -finTimeLower;
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarTf + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;

			}
		}
		if (numFinTimeType2 != 0) {
			for (cnt = 0; cnt < numFinTimeType2; cnt++) {
				i = idxFinTimeType2[cnt];
				// tf == bUpper
				bEqua[cntRowAEqua] = finTimeUpper;
				ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
				idxColAEqua[cntNzAEqua] = idxOptVarTf + i;
				valueAEqua[cntNzAEqua] = 1;
				cntRowAEqua = cntRowAEqua + 1;
				cntNzAEqua = cntNzAEqua + 1;
			}
		}
		if (numFinTimeType3 != 0) {
			for (cnt = 0; cnt < numFinTimeType3; cnt++) {
				i = idxFinTimeType3[cnt];
				// tf <= bUpper
				bCone[cntRowAIneq] = finTimeUpper;
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarTf + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		if (numFinTimeType4 != 0) {
			for (cnt = 0; cnt < numFinTimeType4; cnt++) {
				i = idxFinTimeType4[cnt];
				// -tf <= -bLower
				bCone[cntRowAIneq] = -finTimeLower;
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarTf + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		// 静态参数变量
		if (numParameterType1 != 0) {
			for (cnt = 0; cnt < numParameterType1; cnt++) {
				i = idxParameterType1[cnt];
				// p <= bUpper
				bCone[cntRowAIneq] = parameterUpper[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarParameter + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
				// -p <= -bLower
				bCone[cntRowAIneq] = -parameterLower[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarParameter + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		if (numParameterType2 != 0) {
			for (cnt = 0; cnt < numParameterType2; cnt++) {
				i = idxParameterType2[cnt];
				// p == bUpper
				bEqua[cntRowAEqua] = parameterUpper[i];
				ptrRowAEqua[cntRowAEqua] = cntNzAEqua;
				idxColAEqua[cntNzAEqua] = idxOptVarParameter + i;
				valueAEqua[cntNzAEqua] = 1;
				cntRowAEqua = cntRowAEqua + 1;
				cntNzAEqua = cntNzAEqua + 1;
			}
		}
		if (numParameterType3 != 0) {
			for (cnt = 0; cnt < numParameterType3; cnt++) {
				i = idxParameterType3[cnt];
				// p <= bUpper
				bCone[cntRowAIneq] = parameterUpper[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarParameter + i;
				valueACone[cntNzAIneq] = 1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		if (numParameterType4 != 0) {
			for (cnt = 0; cnt < numParameterType4; cnt++) {
				i = idxParameterType4[cnt];
				// -p <= -bLower
				bCone[cntRowAIneq] = -parameterLower[i];
				ptrRowACone[cntRowAIneq] = cntNzAIneq;
				idxColACone[cntNzAIneq] = idxOptVarParameter + i;
				valueACone[cntNzAIneq] = -1.0;
				cntRowAIneq = cntRowAIneq + 1;
				cntNzAIneq = cntNzAIneq + 1;
			}
		}
		idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
	}
	socpInfo->cntNzAEqua = cntNzAEqua;
	socpInfo->cntRowAEqua = cntRowAEqua;
	socpInfo->cntNzAIneq = cntNzAIneq;
	socpInfo->cntRowAIneq = cntRowAIneq;
	socpInfo->cntNzASoc = cntNzASoc;
	socpInfo->cntRowASoc = cntRowASoc;
}

void socpInfoTrust(cmscp_socpInfo** socpInfo0, cmscp_trajInfo** refTraj, cmscp_setup* setup)
{
	// socpInfo: 已有的二阶锥信息
	// refTraj : 参考轨迹信息
	// gradInfo : 梯度信息
	// setup : 设置信息
	// 
	int numPhase, * numPhaseOptVar, iphase, numMesh, numState, numControl, * idxColAEqua, * ptrRowAEqua, * idxColACone, * ptrRowACone, cntNzAEqua, cntRowAEqua, cntNzAIneq, cntRowAIneq, cntNzASoc, cntRowASoc, cntSubASoc, idxOptVarPhaseStart, idxOptVarT0, idxOptVarTf, idxOptVarParameter,  p, j,  numParameter,  k, idxStateK, idxControlK, idxOptVarTrust;
	double* valueAEqua, * bEqua, * valueACone, * bCone, refT0, refTf, * refParameter, * refState, * refControl,  a1, a2, weightIniTime, weightFinTime, * weightState, * weightControl, * weightParameter;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_trust* trust;
	cmscp_socpInfo* socpInfo;
	cmscp_dimCone* dimCone;
	// //基本参数
	dimension = setup->dimension;// 问题维数信息
	mesh = setup->mesh;// 网格信息
	numPhase = setup->numPhase;// 阶段数
	socpInfo = *socpInfo0;
	numPhaseOptVar = socpInfo->numPhaseOptVar;// 每段优化变量个数
	trust = setup->trust;// 信赖域信息
	// 二阶锥规划问题参数
	// 二阶锥规划参数
	ptrRowAEqua = socpInfo->AEqua->ptrRow;
	idxColAEqua = socpInfo->AEqua->idxCol;
	valueAEqua = socpInfo->AEqua->v;
	bEqua = socpInfo->bEqua;
	ptrRowACone = socpInfo->ACone->ptrRow;
	idxColACone = socpInfo->ACone->idxCol;
	valueACone = socpInfo->ACone->v;
	bCone = socpInfo->bCone;
	dimCone = socpInfo->dimCone;
	// 计数变量
	cntNzAEqua = socpInfo->cntNzAEqua;
	cntRowAEqua = socpInfo->cntRowAEqua;
	cntNzAIneq = socpInfo->cntNzAIneq;
	cntRowAIneq = socpInfo->cntRowAIneq;
	cntNzASoc = socpInfo->cntNzASoc;
	cntRowASoc = socpInfo->cntRowASoc;
	cntSubASoc = socpInfo->cntSubASoc;
	//// 填充信赖域约束非零元
	// 硬信赖域：
	// [0] <= _K[delta]
	// [x][x_r]
	// 或者软信赖域：
	// [-delta] <= _K[0]
	// [x][x_r]
	// 每段起始点索引
	idxOptVarPhaseStart = 0;
	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;
		refState = refTraj[iphase]->state->v;
		refControl = refTraj[iphase]->control->v;
		refT0 = refTraj[iphase]->initialtime;
		refTf = refTraj[iphase]->finaltime;
		refParameter = refTraj[iphase]->parameter;
		if (strcmp(trust->model, "hard")==0) {
			a1 = 0;
			a2 = trust->phase[iphase]->hardRadius;
		}
		else if (strcmp(trust->model, "soft")==0) {
			a1 = -1;
			a2 = 0;
		}
		weightIniTime = trust->phase[iphase]->weight->initialtime;
		weightFinTime = trust->phase[iphase]->weight->finaltime;
		weightState = trust->phase[iphase]->weight->state;
		weightControl = trust->phase[iphase]->weight->control;
		weightParameter = trust->phase[iphase]->weight->parameter;
		// 索引
		
		idxStateK = idxOptVarPhaseStart;
		idxControlK = idxOptVarPhaseStart + numMesh * numState;
		idxOptVarT0 = idxOptVarPhaseStart + numMesh * (numState + numControl);
		idxOptVarTf = idxOptVarT0 + 1;
		idxOptVarParameter = idxOptVarTf + 1;
		idxOptVarTrust = idxOptVarParameter + numParameter;
		for (k = 0; k < numMesh; k++) {
			// 状态变量
			ptrRowACone[cntRowASoc] = cntNzASoc;
			idxColACone[cntNzASoc] = idxOptVarTrust;
			valueACone[cntNzASoc] = a1;
			bCone[cntRowASoc] = a2;
			cntRowASoc = cntRowASoc + 1;
			cntNzASoc = cntNzASoc + 1;
			for (j = 0; j < numState; j++) {
				ptrRowACone[cntRowASoc] = cntNzASoc;
				idxColACone[cntNzASoc] = idxStateK + j;
				valueACone[cntNzASoc] = weightState[j];
				bCone[cntRowASoc] = weightState[j] * refState[j + k * numState];
				cntRowASoc = cntRowASoc + 1;
				cntNzASoc = cntNzASoc + 1;
			}
			dimCone->q[cntSubASoc] = numState + 1;
			cntSubASoc = cntSubASoc + 1;
			// 控制变量
			ptrRowACone[cntRowASoc] = cntNzASoc;
			idxColACone[cntNzASoc] = idxOptVarTrust;
			valueACone[cntNzASoc] = a1;
			bCone[cntRowASoc] = a2;
			cntRowASoc = cntRowASoc + 1;
			cntNzASoc = cntNzASoc + 1;
			for (j = 0; j < numControl; j++) {
				ptrRowACone[cntRowASoc] = cntNzASoc;
				idxColACone[cntNzASoc] = idxControlK + j;
				valueACone[cntNzASoc] = weightControl[j];
				bCone[cntRowASoc] = weightControl[j] * refControl[j + k* numControl];
				cntRowASoc = cntRowASoc + 1;
				cntNzASoc = cntNzASoc + 1;
			}
			dimCone->q[cntSubASoc] = numControl + 1;
			cntSubASoc = cntSubASoc + 1;
			idxStateK = idxStateK + numState;
			idxControlK = idxControlK + numControl;
		}
		// 初始时刻
		ptrRowACone[cntRowASoc] = cntNzASoc;
		idxColACone[cntNzASoc] = idxOptVarTrust;
		valueACone[cntNzASoc] = a1;
		bCone[cntRowASoc] = a2;
		cntRowASoc = cntRowASoc + 1;
		cntNzASoc = cntNzASoc + 1;
		ptrRowACone[cntRowASoc] = cntNzASoc;
		idxColACone[cntNzASoc] = idxOptVarT0;
		valueACone[cntNzASoc] = weightIniTime;
		bCone[cntRowASoc] = weightIniTime * refT0;
		dimCone->q[cntSubASoc] = 2;
		cntRowASoc = cntRowASoc + 1;
		cntNzASoc = cntNzASoc + 1;
		cntSubASoc = cntSubASoc + 1;
		// 终端时刻
		ptrRowACone[cntRowASoc] = cntNzASoc;
		idxColACone[cntNzASoc] = idxOptVarTrust;
		valueACone[cntNzASoc] = a1;
		bCone[cntRowASoc] = a2;
		cntRowASoc = cntRowASoc + 1;
		cntNzASoc = cntNzASoc + 1;
		ptrRowACone[cntRowASoc] = cntNzASoc;
		idxColACone[cntNzASoc] = idxOptVarTf;
		valueACone[cntNzASoc] = weightFinTime;
		bCone[cntRowASoc] = weightFinTime * refTf;
		dimCone->q[cntSubASoc] = 2;
		cntSubASoc = cntSubASoc + 1;
		cntRowASoc = cntRowASoc + 1;
		cntNzASoc = cntNzASoc + 1;
		// 参数变量
		ptrRowACone[cntRowASoc] = cntNzASoc;
		idxColACone[cntNzASoc] = idxOptVarTrust;
		valueACone[cntNzASoc] = a1;
		bCone[cntRowASoc] = a2;
		cntRowASoc = cntRowASoc + 1;
		cntNzASoc = cntNzASoc + 1;
		for (j = 0; j < numParameter; j++) {
			ptrRowACone[cntRowASoc] = cntNzASoc;
			idxColACone[cntNzASoc] = idxOptVarParameter + j;
			valueACone[cntNzASoc] = weightParameter[j];
			bCone[cntRowASoc] = weightParameter[j] * refParameter[j];
			cntRowASoc = cntRowASoc + 1;
			cntNzASoc = cntNzASoc + 1;
		}
		dimCone->q[cntSubASoc] = numParameter + 1;
		cntSubASoc = cntSubASoc + 1;
		idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
	}
	socpInfo->cntNzAEqua = cntNzAEqua;
	socpInfo->cntRowAEqua = cntRowAEqua;
	socpInfo->cntNzAIneq = cntNzAIneq;
	socpInfo->cntRowAIneq = cntRowAIneq;
	socpInfo->cntNzASoc = cntNzASoc;
	socpInfo->cntRowASoc = cntRowASoc;
	socpInfo->cntSubASoc = cntSubASoc;
}



void getSocpInfo(cmscp_socpInfo** socpInfo0, cmscp_trajInfo** refTraj, cmscp_sparseInfo* sparseInfo, cmscp_consTypeInfo* consTypeInfo, cmscp_setup* setup)
{
	// 函数功能：获取二阶锥规划问题的参数
	// 输入：
	// refTraj：参考轨迹信息
	// sparseInfo：稀疏（非零元位置）信息
	// consTypeInfo：约束类型信息
	// setup：问题设置参数
	// 输出：
	// socpInfo：存储有连续函数稀疏信息的结构体指针
	// 谢磊：2022 / 6 / 27编写
	cmscp_socpInfo* socpInfo;
	// 1. 获取二阶锥规划问题的维度信息和申请空间
	socpInfoSparseDim(socpInfo0, consTypeInfo, sparseInfo, setup);
	socpInfo = *socpInfo0;
	// 2. 获取二阶锥规划问题的目标函数
	socpInfoSparseObjEvent(socpInfo0, refTraj, sparseInfo, consTypeInfo, setup);

	// 3. 获取二阶锥规划问题的端点函数
	socpInfoSparseEndp(socpInfo0, refTraj, sparseInfo, consTypeInfo, setup);

	// 3. 获取二阶锥规划问题的连续函数
	socpInfoSparseCont(socpInfo0, refTraj, sparseInfo, consTypeInfo, setup);

	// 4. 获取二阶锥规划问题的衔接函数
	socpInfoSparseLink(socpInfo0, refTraj, sparseInfo, consTypeInfo, setup);

	// 5. 获取二阶锥规划问题的边界函数
	socpInfoBound(socpInfo0, consTypeInfo, setup);

	// 6. 获取二阶锥规划问题的信赖域函数
	socpInfoTrust(socpInfo0, refTraj, setup);

	// 7. 数据格式转化

	floatRcsToCcs(socpInfo->AEqua->ptrRow, socpInfo->AEqua->idxCol, socpInfo->AEqua->v, 
		socpInfo->AEqua->m, socpInfo->AEqua->n, socpInfo->AEqua1->idxRow, socpInfo->AEqua1->ptrCol, socpInfo->AEqua1->v);
	floatRcsToCcs(socpInfo->ACone->ptrRow, socpInfo->ACone->idxCol, socpInfo->ACone->v,
		socpInfo->ACone->m, socpInfo->ACone->n, socpInfo->ACone1->idxRow, socpInfo->ACone1->ptrCol, socpInfo->ACone1->v);

	
	/* 打印二阶锥信息 */
	printIntVector(socpInfo->ACone->ptrRow, socpInfo->ACone->m + 1, "result\\AConePtrRow.txt");
	printIntVector(socpInfo->ACone->idxCol, socpInfo->ACone->ptrRow[socpInfo->ACone->m], "result\\AConeIdxCol.txt");
	printFloatVector(socpInfo->ACone->v, socpInfo->ACone->ptrRow[socpInfo->ACone->m], "result\\AConeValue.txt");
	printIntVector(socpInfo->AEqua->ptrRow, socpInfo->AEqua->m + 1, "result\\AEquaPtrRow.txt");
	printIntVector(socpInfo->AEqua->idxCol, socpInfo->AEqua->ptrRow[socpInfo->AEqua->m], "result\\AEquaIdxCol.txt");
	printFloatVector(socpInfo->AEqua->v, socpInfo->AEqua->ptrRow[socpInfo->AEqua->m], "result\\AEquaValue.txt");
	printFloatVector(socpInfo->bEqua, socpInfo->AEqua->m, "result\\bEqua.txt");
	printFloatVector(socpInfo->bCone, socpInfo->ACone->m, "result\\bCone.txt");
	printIntVector(socpInfo->dimCone->q, socpInfo->dimCone->nsoc, "result\\dimQ.txt");
	printFloatVector(socpInfo->c, socpInfo->numOptVar, "result\\c.txt");

	// 8 释放行压缩格式部分数据的空间
	freeSocpInfo(&socpInfo, setup, 0);
}