两条二维曲线求交点（c++实现）

#include<iostream>
#include<math.h>

/*
设第一条二维曲线方程为：
					y=a*x*x+b*x+c
则： 
			_									   _-1
	 __   |  _              _T   _              _ |    _              _T   _   _
	|a|   | |x1*x1   x1   1 |   |x1*x1   x1   1 | |   |x1*x1   x1   1 |   | y1 |
	|b| = | |...    ...  ...| * |...    ...  ...| | * |...    ...  ...| * |... |
	|c|   | |xn*xn   xn   1 |   |xn*xn   xn   1 | |   |xn*xn   xn   1 |   | yn |
	--	  | -              -    -              -  |   -              -    -   -
 		   -									 -
 		   
设第二条二维曲线方程为：
					y=d*x*x+e*x+f
则： 
			_									   _-1
	 __   |  _              _T   _              _ |    _              _T   _   _
	|d|   | |x1*x1   x1   1 |   |x1*x1   x1   1 | |   |x1*x1   x1   1 |   | y1 |
	|e| = | |...    ...  ...| * |...    ...  ...| | * |...    ...  ...| * |... |
	|f|   | |xn*xn   xn   1 |   |xn*xn   xn   1 | |   |xn*xn   xn   1 |   | yn |
	--	  | -              -    -              -  |   -              -    -   -
 		   -									 -
 
*/ 

using namespace std;

double a[][2] = {					// 第一条曲线周围的点 
					 -1, 0,
					  0, 1,
					  1, 0
				};
					
#define N  sizeof(a)/sizeof(a[0])

double d[][2] = {					// 第一条曲线周围的点 
					 -1, 0,
					  0, -1,
					  1, 0
				};
					
#define N  sizeof(a)/sizeof(a[0])
#define M  sizeof(d)/sizeof(d[0])

void matrix_inverse(double(*a)[3], double(*b)[3]);

int main()
{
// 第一条曲线求系数 
	double a0[N][3] = {0};
	double a1[3][N] = {0};
	double a2[3][3] = {0};
	double a3[3][3] = {0};
	double a4[3][N] = {0};
	double b0[N][1] = {0};
	double c0[3][1] = {0};
	
	for(int i = 0; i < N; ++i)
	{
		a0[i][0] = pow(a[i][0], 2);
		a0[i][1] = a[i][0];
		a0[i][2] = 1;
		
		a1[0][i] = pow(a[i][0], 2);		// 	矩阵转置 
		a1[1][i] = a[i][0];
		a1[2][i] = 1;
		
		b0[i][0] = a[i][1];
	}

	for(int i = 0; i < 3; ++i)		// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数） 
		for(int j = 0; j < 3; ++j)
			for(int k = 0; k < N; ++k)
				a2[i][j] += (a1[i][k] * a0[k][j]);
	
	matrix_inverse(a2, a3);		// 矩阵求逆 
	
	for(int i = 0; i < 3; ++i)
		for(int j = 0; j < N; ++j)
			for(int k = 0; k < 3; ++k)
				a4[i][j] += (a3[i][k] * a1[k][j]);
	
	for(int i = 0; i < 3; ++i)
		for(int j = 0; j < 1; ++j)
			for(int k = 0; k < N; ++k)
				c0[i][j] += (a4[i][k] * b0[k][j]);
	
//	for(int i = 0; i < 3; ++i)
//	{
//		for(int j = 0; j < 1; ++j)
//		{
//			cout << " " << c0[i][j] << "\t";
//		}
//		cout << "\n";
//	}

// 第二条曲线求系数 
	double d0[M][3] = {0};
	double d1[3][M] = {0};
	double d2[3][3] = {0};
	double d3[3][3] = {0};
	double d4[3][M] = {0};
	double e0[M][1] = {0};
	double f0[3][1] = {0};
	
	for(int i = 0; i < M; ++i)
	{
		d0[i][0] = pow(d[i][0], 2);
		d0[i][1] = d[i][0];
		d0[i][2] = 1;
		
		d1[0][i] = pow(d[i][0], 2);		// 	矩阵转置 
		d1[1][i] = d[i][0];
		d1[2][i] = 1;
		
		e0[i][0] = d[i][1];
	}

	for(int i = 0; i < 3; ++i)		// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数） 
		for(int j = 0; j < 3; ++j)
			for(int k = 0; k < M; ++k)
				d2[i][j] += (d1[i][k] * d0[k][j]);
	
	matrix_inverse(d2, d3);		// 矩阵求逆 
	
	for(int i = 0; i < 3; ++i)
		for(int j = 0; j < M; ++j)
			for(int k = 0; k < 3; ++k)
				d4[i][j] += (d3[i][k] * d1[k][j]);
	
	for(int i = 0; i < 3; ++i)
		for(int j = 0; j < 1; ++j)
			for(int k = 0; k < M; ++k)
				f0[i][j] += (d4[i][k] * e0[k][j]);
	
//	for(int i = 0; i < 3; ++i)
//	{
//		for(int j = 0; j < 1; ++j)
//		{
//			cout << " " << f0[i][j] << "\t";
//		}
//		cout << "\n";
//	}

// 交点所在方程
// (c0[0][0] - f0[0][0]) * x*x + (c0[0][1] - f0[0][1]) * x + (c0[0][2] - f0[0][2]) = 0
	double g[2][2] = {0};
	if ( ( pow(c0[0][1] - f0[0][1], 2) - 4 * (c0[0][0] - f0[0][0]) * (c0[0][2] - f0[0][2]) ) < 0 )
	{
		cout << "曲线不存在交点" << "\t";
		return 0; 
	}
	else
	{
		g[0][0] = -(( (c0[0][1] - f0[0][1]) + pow((pow(c0[0][1] - f0[0][1], 2) - 4 * (c0[0][0] - f0[0][0]) * (c0[0][2] - f0[0][2])), 0.5) ) / (2 * (c0[0][0] - f0[0][0])) );
		g[0][1] = (c0[0][0] - f0[0][0]) * pow(g[0][0], 2) + (c0[0][1] - f0[0][1]) * g[0][0] + (c0[0][2] - f0[0][2]);
		g[1][0] = -(( (c0[0][1] - f0[0][1]) - pow((pow(c0[0][1] - f0[0][1], 2) - 4 * (c0[0][0] - f0[0][0]) * (c0[0][2] - f0[0][2])), 0.5) ) / (2 * (c0[0][0] - f0[0][0])) );
		g[1][1] = (c0[0][0] - f0[0][0]) * pow(g[1][0], 2) + (c0[0][1] - f0[0][1]) * g[1][0] + (c0[0][2] - f0[0][2]);
	}
	
	for(int i = 0; i < 2; ++i)
	{
		for(int j = 0; j < 2; ++j)
		{
			cout << " " << g[i][j] << "\t";
		}
		cout << "\n";
	}
	return 0;
 }
 
 void matrix_inverse(double(*a)[3], double(*b)[3])			// 矩阵求逆 
{
	using namespace std;
	int i, j, k;
	double max, temp;
	// 定义一个临时矩阵t
	double t[3][3];
	// 将a矩阵临时存放在矩阵t[n][n]中
	for (i = 0; i < 3; i++)
		for (j = 0; j < 3; j++)
			t[i][j] = a[i][j];
	// 初始化B矩阵为单位矩阵
	for (i = 0; i < 3; i++)
		for (j = 0; j < 3; j++)
			b[i][j] = (i == j) ? (double)1 : 0;
	// 进行列主消元，找到每一列的主元
	for (i = 0; i < 3; i++)
	{
		max = t[i][i];
		// 用于记录每一列中的第几个元素为主元
		k = i;
		// 寻找每一列中的主元元素
		for (j = i + 1; j < 3; j++)
		{
			if (fabs(t[j][i]) > fabs(max))
			{
				max = t[j][i];
				k = j;
			}
		}
		//cout<<"the max number is "<<max<<endl;
		// 如果主元所在的行不是第i行，则进行行交换
		if (k != i)
		{
			// 进行行交换
			for (j = 0; j < 3; j++)
			{
				temp = t[i][j];
				t[i][j] = t[k][j];
				t[k][j] = temp;
				// 伴随矩阵B也要进行行交换
				temp = b[i][j];
				b[i][j] = b[k][j];
				b[k][j] = temp;
			}
		}
		if (t[i][i] == 0)
		{
			cout << "\nthe matrix does not exist inverse matrix\n";
			break;
		}
		// 获取列主元素
		temp = t[i][i];
		// 将主元所在的行进行单位化处理
		//cout<<"\nthe temp is "<<temp<<endl;
		for (j = 0; j < 3; j++)
		{
			t[i][j] = t[i][j] / temp;
			b[i][j] = b[i][j] / temp;
		}
		for (j = 0; j < 3; j++)
		{
			if (j != i)
			{
				temp = t[j][i];
				//消去该列的其他元素
				for (k = 0; k < 3; k++)
				{
					t[j][k] = t[j][k] - temp * t[i][k];
					b[j][k] = b[j][k] - temp * b[i][k];
				}
			}
		}
	}
}