机械手定位（带角度）的思路及3点计算旋转中心

机械手定位（带角度）的思路及3点计算旋转中心

1、旋转中心为：RectCenter_Row, RectCenter_Column

2、基准模板图像中Mark的中心点： ModelRow, ModelCol

3、拍摄得到的Mark新的中心点：Row,Column 角度：Angle
hom_mat2d_identity(HomMat2DIdentity3)
hom_mat2d_rotate(HomMat2DIdentity3,Angle,RectCenter_Row, RectCenter_Column,HomMat2DRotate3)
因为绕着实际的中心轴做了旋转，mark点的位置发生了变化，可以通过旋转变换得到新的位置
affine_trans_point_2d(HomMat2DRotate3,ModelRow, ModelCol, Qx, Qy)
实际操作中，告诉手臂旋转角度Angle，以及位移（Row - Qx, Column - Qy）

3点计算旋转中心、旋转中心的应用纯C#方法

1、已知圆上三点坐标为（X1,Y1），（X2,Y2），（X3,Y3），求圆心坐标x，y

public void GetCenterPos(double x1, double y1, double x2, double y2, double x3, double y3, ref double X, ref double Y, ref double R) 
{
	double a = 0;
	double b = 0;
	double c = 0;
	double g = 0;
	double e = 0;
	double f = 0;
	e = 2 * (x2 - x1);
	f = 2 * (y2 - y1);
	g = x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1;
	a = 2 * (x3 - x2);
	b = 2 * (y3 - y2);
	c = x3 * x3 - x2 * x2 + y3 * y3 - y2 * y2;
	X = (g * b - c * f) / (e * b - a * f);
	Y = (a * g - c * e) / (a * f - b * e);
	R = Sqrt((X - x1) * (X - x1) + (Y - y1) * (Y - y1));
}

2、已知某点坐标和旋转中心，求该点绕旋转中心运动某一角度后的新坐标

public string GetRoratedCoordinator(double XRotation, double YRotation, double ARotate, double XBefore, double YBefore, ref double XAfter, ref double YAfter)
{
	try {
		double Rad = 0;
		Rad = ARotate * Acos(-1) / 180;
		 
		XAfter = (XBefore - XRotation) * Cos(Rad) - (YBefore - YRotation) * Sin(Rad) + XRotation;
		YAfter = (YBefore - YRotation) * Cos(Rad) + (XBefore - XRotation) * Sin(Rad) + YRotation;
		 
		return "OK";
	} catch (Exception ex) {
		return ex.Message;
	}
}