function.h

#ifndef function
#define function
#include <Eigen\Dense>

typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 7, 1> Vector7d;
typedef Eigen::Matrix<double, 1, 7> Vector1_7d;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 7, 7> Matrix7d;

double	f(Vector6d s);
Vector6d Df_Ds(Vector6d s);
Matrix6d Dr_Ds(Vector6d s);
Matrix7d A(Matrix6d inverse, double lab, Matrix6d Dr_Ds); //计算A矩阵 7*7
Vector1_7d fsfp(Vector6d s, double p);  //返回一个行向量1*7 由fs fp 组成
Vector7d joint(Vector6d a, double b);
#endif // !function

谢军波

function.cpp

#include<iostream>
#include<cmath>
#include <Eigen\Dense>


typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 7, 1> Vector7d;
typedef Eigen::Matrix<double, 1, 7> Vector1_7d;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 7, 7> Matrix7d;

double	f(Vector6d s)  //势函数
{
	double x;
	x = sqrt(0.04 * (s(0)*s(0) - s(0)*s(1) + s(1)*s(1)) + s(3)*s(3)) + 0.5*(s(0) + s(1));
	return x;

}


Vector6d Df_Ds(Vector6d s) //r 势函数对应力的一阶导
{
	Vector6d k;
	k(0) = 0.04 * (2 * s(0) - s(1)) / 2 * sqrt(0.04 * (s(0)*s(0) - s(0)*s(1) + s(1)*s(1)) + s(3)*s(3))  + 0.5;
	k(1) = 0.04 * (2 * s(1) - s(0)) / 2 * sqrt(0.04 * (s(0)*s(0) - s(0)*s(1) + s(1)*s(1)) + s(3)*s(3)) + 0.5;
	k(2) = 0;
	k(3) = 2 * s(3) / 2 * sqrt(0.04 * (s(0)*s(0) - s(0)*s(1) + s(1)*s(1)) + s(3)*s(3));
	k(4) = 0;
	k(5) = 0;
	return k;
}

Matrix6d Dr_Ds(Vector6d s)   // r对应力的一阶导，势函数对应力的二阶导
{
	Matrix6d a;
	double inRadicalSign, RadicalSign;
	RadicalSign = sqrt(0.04 * (s(0)*s(0) - s(0)*s(1) + s(1)*s(1)) + s(3)*s(3));
	inRadicalSign = 0.04 * (s(0)*s(0) - s(0)*s(1) + s(1)*s(1)) + s(3)*s(3);
	a.setZero();

	a(0, 0) = (4 * 0.04*RadicalSign - pow(0.04 * (2 * s(0) - s(1)), 2) / RadicalSign) / 4 * inRadicalSign;
	a(0, 1)= (-2*0.04*RadicalSign-0.04*0.04*(2 * s(0) - s(1))*(2 * s(1) - s(0))/ RadicalSign)/ 4 * inRadicalSign;
	a(0, 3) = -0.04*(2 * s(0) - s(1)) * 2 * s(3) / RadicalSign / 4 * inRadicalSign;
	
	a(1, 0) = (-0.08*RadicalSign-0.04*0.04*(2 * s(0) - s(1))*(2 * s(1) - s(0)) / RadicalSign)/ 4 * inRadicalSign;
	a(1, 1) = (0.08*2* RadicalSign- pow(0.04 * (2 * s(1) - s(0)), 2)/ RadicalSign) / 4 * inRadicalSign;
	a(1, 3) = -0.04*(2 * s(1) - s(0)) * 2 * s(3) / RadicalSign / 4 * inRadicalSign;

	a(3, 0) = -s(3)*0.04*(2 * s(0) - s(1)) / 2 * RadicalSign / inRadicalSign;
	a(3, 1) = -s(3)*0.04*(2 * s(1) - s(0)) / 2 * RadicalSign / inRadicalSign;
	a(3, 3) = (RadicalSign - s(3)*s(3) / RadicalSign) / inRadicalSign;

	return a;
}   

Matrix7d A(Matrix6d inverse, double lambada, Matrix6d Dr_Ds)  //计算A矩阵 7*7
{
	Matrix7d L;//A的逆
	L.setZero();
	for (size_t i = 0; i < 6; i++)
	{
		for (size_t j = 0; j < 6; j++)
		{
			L(i, j) = inverse(i, j) + lambada*Dr_Ds(i, j);
		}
	}
	L(6, 6) = -1;
	return L.inverse();
}


Vector1_7d fsfp(Vector6d r,double p)  //将Vector6d r, double p 拼接为一个1*7的行向量
{
	Vector7d fd_fp;
	
	for (size_t i = 0; i <6; i++)
	{
		fd_fp(i) = r(i);
	}
	if (p>1e-8)
	{
		fd_fp(6) = -150*0.51*pow(p, 0.51 - 1);
	}
	else
	{
		fd_fp(6) = -150*0.51*pow(p, 0.3);
	}
	
	return fd_fp.transpose();
}

Vector7d joint(Vector6d a, double b) //将Vector6d a, double b 拼接为一个7*1的列向量
{
	Vector7d c;
	for (size_t i = 0; i < 6; i++)
	{
		c(i) = a(i);
	}
	c(6) = b;
	return c;
}