[运动控制]三点定圆

//三点定圆
EXTERN_C short WINAPI _Point3(unsigned short Axis,
	double dDistX1,double dDistY1,
	double dDistX2,double dDistY2,
	double dDistX3,double dDistY3,
	short Position_Mode = 0, unsigned short CardNum = 0)
{
	double mat1,mat2,mat3;  //参数
	double center1, center2;    //求出圆心坐标
	double k,b;  //直线斜率k； y=kx+b
	double direction;    //求出方向
	short stat = 0;
	unsigned short rtn;
	rtn = MCF_Switch_Card(CardNum);
	if (rtn != 0)
	{
		return rtn;
	}
	unsigned short temp[MaxAxis] = {0};
	unsigned short AxisNum=0;
	stat = MultiAxis_BitToDate(Axis,&AxisNum,&temp[AxisNum]);
	if (8 == stat)
	{
		return 8;
	}
	if (fabs(dDistX1-dDistX2) < 0.000001)   //第一个点与第二个点的直线垂直于X轴
	{
		if (fabs(dDistX3-dDistX2) < 0.000001)
		{
			return 8;
		}
		else
		{
			if (dDistX3 > dDistX1)
			{
				direction = 0;
			}
			else
			{
				direction = 1;
			}
		}

	}
	else
	{
		k = (dDistY2-dDistY1)/(dDistX2-dDistX1);
		b = dDistY1-k*dDistX1;
		if (fabs(k*dDistX3+b-dDistY3) < 0.000001)  //第三个点在直线上，return 8
		{
			return 8;
		}
		else
		{
			if (k*dDistX3+b > dDistY3)  //第三个点在直线下方——顺时针
			{
				direction = 0;
			}
			else
				direction = 1;			//第三个点在直线上方——逆时针
		}
	}
	mat1=((dDistX2*dDistX2)+(dDistY2*dDistY2)-(dDistX1*dDistX1+dDistY1*dDistY1))*(2*(dDistY3-dDistY1))-
		((dDistX3*dDistX3+dDistY3*dDistY3)-(dDistX1*dDistX1+dDistY1*dDistY1))*(2*(dDistY2-dDistY1));
	mat2=(2*(dDistX2-dDistX1))*((dDistX3*dDistX3+dDistY3*dDistY3)-(dDistX1*dDistX1 +dDistY1*dDistY1))-  
		(2*(dDistX3-dDistX1))*((dDistX2*dDistX2+dDistY2*dDistY2)-(dDistX1*dDistX1 +dDistY1*dDistY1));
	mat3 = 4*((dDistX2-dDistX1)*(dDistY3-dDistY1) - (dDistX3-dDistX1)*(dDistY2-dDistY1)); 
	center1 = mat1/mat3;
	center2 = mat2/mat3;

	dDistX1 = dDistX1*EquivalentMolecule[CardNum][temp[0]]/EquivalentDenominator[CardNum][temp[0]];
	dDistY1 = dDistY1*EquivalentMolecule[CardNum][temp[1]]/EquivalentDenominator[CardNum][temp[1]];
	dDistX2 = dDistX2*EquivalentMolecule[CardNum][temp[0]]/EquivalentDenominator[CardNum][temp[0]];
	dDistY2 = dDistY2*EquivalentMolecule[CardNum][temp[1]]/EquivalentDenominator[CardNum][temp[1]];
	dDistX3 = dDistX3*EquivalentMolecule[CardNum][temp[0]]/EquivalentDenominator[CardNum][temp[0]];
	dDistY3 = dDistY3*EquivalentMolecule[CardNum][temp[1]]/EquivalentDenominator[CardNum][temp[1]];
	center1 = center1*EquivalentMolecule[CardNum][temp[0]]/EquivalentDenominator[CardNum][temp[0]];
	center2 = center2*EquivalentMolecule[CardNum][temp[1]]/EquivalentDenominator[CardNum][temp[1]];
	if (dDistX1 > 2147483648)
	{
		return 26;
	}
	if (dDistY1 > 2147483648)
	{
		return 26;
	}
	if (dDistX2 > 2147483648)
	{
		return 26;
	}
	if (dDistY2 > 2147483648)
	{
		return 26;
	}
	if (dDistX3 > 2147483648)
	{
		return 26;
	}
	if (dDistY3 > 2147483648)
	{
		return 26;
	}

	return stat;
}