Convex_Optimization_VS/CMSCP2/numericalFunctions.c

#include "numericalFunctions.h"

// 生成线性间距向量
void linSpace(double x1, double x2, int n, double* y) {
	int i;
	double d;
	d = (x2 - x1) / (n - 1);
	y[0] = x1;
	y[n - 1] = x2;
	for (i = 1; i < n - 1; i++)
	{
		y[i] = y[i - 1] + d;
	}
}

void ascLinearInterp(double* X, double* Y, int N, int M, double* x, int n, double* y) {
	// 升序线性插值
	// 函数参数 :
	// 为插值节点, 维数 1 * N
	// 为插值节点处的函数输出值，维数 M * N
	// N 为插值节点数目
	// M 为插值函数输出维数
	// 函数输入输出:
	// x 为函数输入, 维数 1 * n
	// n 为函数输入维数
	// y 为函数输入的维数，维数 M * n
	// 输入要求：
	// X 为升序列向量，即X(1) <= X(2) <= ...X(i) <= ... <= X(N)
	// x 为升序列向量，即x(1) <= x(2) <= ...x(i) <= ... <= x(n)

	int i, j, k, js, yidxs, Yidxs, Y1idxs;
	js = 0;
	yidxs = 0;
	for (i = 0; i < n; i++) {
		if (x[i] <= X[0]) {
			for (k = 0; k < M; k++) {
				y[yidxs + k] = Y[k];
			}
		}
		else if (x[i] > X[N - 1]) {
			Yidxs = (N - 1) * M;
			for (k = 0; k < M; k++) {
				y[yidxs + k] = Y[Yidxs + k];
			}
		}
		else {
			for (j = js; j < N - 1; j++) {
				if (x[i] > X[j] && x[i] <= X[j + 1]) {
					Yidxs = j * M;
					Y1idxs = (j + 1) * M;
					for (k = 0; k < M; k++) {
						y[yidxs + k] = Y[Yidxs + k] + (Y[Y1idxs + k] - Y[Yidxs + k]) / (X[j + 1] - X[j]) * (x[i] - X[j]);
					}
					js = j;
					break;
				}
			}
		}
		yidxs += M;
	}

}

void computeInteg(double* newState, // 新状态
	dynIntegFunc pfun, // 动力学方程
	double* oldState, double* control, double* parameter, void* auxdata, // 动力学方程输入
	int numState, int numControl, int numParameter, int numPath, // 变量维数
	double* tempWork, // 临时空间，避免重复申请和释放空间
	double stepSize, int integMode) // 积分步长和积分模式
{
	int i;
	double* path, * dynRhs1, * dynRhs2, * dynRhs3, * dynRhs4, temp;
	/* 积分过程中计算的路径函数和右函数值 */
	path = tempWork;
	dynRhs1 = path + numPath;
	dynRhs2 = dynRhs1 + numState;
	dynRhs3 = dynRhs2 + numState;
	dynRhs4 = dynRhs3 + numState;
	/* 数值积分公式 */
	switch (integMode) {
	case 1: // 欧拉积分
		pfun(dynRhs1, NULL, oldState, control, parameter, auxdata);
		for (i = 0; i < numState; i++) {
			newState[i] = oldState[i] + stepSize * dynRhs1[i];
		};
		break;
	case 2: // 改进型欧拉积分
		temp = 0.5 * stepSize;
		pfun(dynRhs1, NULL, oldState, control, parameter, auxdata);
		for (i = 0; i < numState; i++) {
			newState[i] = oldState[i] + stepSize * dynRhs1[i];
		};
		pfun(dynRhs2, NULL, newState, control, parameter, auxdata);
		for (i = 0; i < numState; i++) {
			newState[i] = oldState[i] + temp * (dynRhs1[i] + dynRhs2[i]);
		};
		break;
	case 3: // 四阶龙哥库塔积分
		temp = 0.5 * stepSize;
		pfun(dynRhs1, NULL, oldState, control, parameter, auxdata);
		for (i = 0; i < numState; i++) {
			newState[i] = oldState[i] + temp * dynRhs1[i];
		};
		pfun(dynRhs2, NULL, newState, control, parameter, auxdata);
		for (i = 0; i < numState; i++) {
			newState[i] = oldState[i] + temp * dynRhs2[i];
		};
		pfun(dynRhs3, NULL, newState, control, parameter, auxdata);
		for (i = 0; i < numState; i++) {
			newState[i] = oldState[i] + stepSize * dynRhs3[i];
		};
		temp = stepSize / 6;
		pfun(dynRhs4, NULL, newState, control, parameter, auxdata);
		for (i = 0; i < numState; i++) {
			newState[i] = oldState[i] + temp * (dynRhs1[i] + 2 * dynRhs2[i] + 2 * dynRhs3[i] + dynRhs4[i]);
		};
		break;
	default:
		break;
	}
}

void copyIntVec(int* x, int* y, int n, int isNegtive) {
	// 函数功能：将n维度向量x复制给y
	// 输入：
	// x：原向量
	// n：向量长度
	// isNegtive：是否取负号复制
	// 输出：
	// y：复制后的向量
	// 谢磊：2022 / 6 / 25编写
	int i;
	if (isNegtive == 0) {
		for (i = 0; i < n; i++)
		{
			y[i] = x[i];
		}
	}
	else
	{
		for (i = 0; i < n; i++)
		{
			y[i] = -x[i];
		}
	}
}
void copyFloatVec(double* x, double* y, int n, int isNegtive) {
	int i;
	if (isNegtive == 0) {
		for (i = 0; i < n; i++)
		{
			y[i] = x[i];
		}
	}
	else
	{
		for (i = 0; i < n; i++)
		{
			y[i] = -x[i];
		}
	}
}

void floatVecFillin(double* x, int n, double a) {
	int i;
	for ( i = 0; i < n; i++)
	{
		x[i] = a;
	}
}
void intVecFillin(int* x, int n, int a) {
	// 函数功能：将n维度向量全部赋值为a
	// 输入：
	// x：向量首地址
	// n：向量长度
	// a：要赋值的向量值
	// 输出：
	// x：赋值后的向量
	// 谢磊：2022 / 6 / 25编写
	int i;
	for (i = 0; i < n; i++)
	{
		x[i] = a;
	}
}