平面关节机械手位置角度关系

void getAngle(double x, double y, double BigAngle[2], double smaAngle[2])
{
	double a = 50.0, b = 50.0;
	double gma = 0.0, pai = acos(-1);
	if (x > 0 && y > 0)
		gma = atan(y / x) * 180 / pai;
	else if (x < 0 && y>0)
		gma = 180 - atan(y / abs(x)) * 180 / pai;
	else if (x < 0 && y < 0)
		gma = 180+atan(y / x) * 180 / pai ;
	else if (x > 0 && y < 0)
		gma = 360+atan(y / abs(x)) * 180 / pai ;
	double aerfa = 0.0, beita = 0.0;
	aerfa = acos((x*x + y*y - a*a - b*b)*1.00 / 2 / a / b) * 180 / pai;
	beita = acos((x*x + y*y + a*a - b*b)*1.00 / 2 / a / sqrt(x*x + y*y)) * 180 / pai;
	smaAngle[0] = aerfa;
	BigAngle[0] = gma - beita;
	smaAngle[1] = -aerfa;
	BigAngle[1] = gma + beita;
	//cout << "small:" << xiaobiangle << "    big:" << gma - dabiangle << endl;
	//cout << "small:" << -xiaobiangle << "   big:" << gma + dabiangle << endl;
}
void getPos(double Dangle, double xangle, double &x, double &y)
{
	int a = 50, b = 50;
	double pai = acos(-1);
	if (Dangle > 0 && Dangle <= 90)
	{
		if (xangle >= -Dangle && xangle <= 90 - Dangle)
		{
			x = a*cos(Dangle / 180 * pai) - b*cos((Dangle + xangle - 180) / 180 * pai);
			y = a*sin(Dangle / 180 * pai) - b*sin((Dangle + xangle - 180) / 180 * pai);
		}
	}
	else if (Dangle > 90 && Dangle <= 180)
	{
		if ((xangle >= -Dangle && xangle <= 90 - Dangle) || (xangle >= 360 - Dangle && xangle <= 450 - Dangle))
		{
			x = a*cos(Dangle / 180 * pai) - b*cos((Dangle + xangle - 180) / 180 * pai);
			y = a*sin(Dangle / 180 * pai) - b*sin((Dangle + xangle - 180) / 180 * pai);
		}

	}
	else if (Dangle > 180 && Dangle <= 270)
	{
		if ((xangle >= -Dangle && xangle <= 90 - Dangle) || (xangle >= 360 - Dangle && xangle <= 450 - Dangle))
		{
			x = a*cos(Dangle / 180 * pai) - b*cos((Dangle + xangle - 180) / 180 * pai);
			y = a*sin(Dangle / 180 * pai) - b*sin((Dangle + xangle - 180) / 180 * pai);
		}

	}
	else if (Dangle > 270 && Dangle <= 360)
	{
		if ((xangle >= -Dangle && xangle <= 90 - Dangle) || (xangle >= 360 - Dangle && xangle <= 450 - Dangle))
		{
			x = a*cos(Dangle / 180 * pai) - b*cos((Dangle + xangle - 180) / 180 * pai);
			y = a*sin(Dangle / 180 * pai) - b*sin((Dangle + xangle - 180) / 180 * pai);
		}

	}
	cout << x << "   " << y << endl;
}