/// <summary>
/// 根据上下相机数据与校准数据，计算放置位置
/// </summary>
/// <param name="PosUpCam">上相机拍照位置</param>
/// <param name="VsDatUpCam">上相机视觉数据</param>
/// <param name="PosDwCam">下相机拍照位置</param>
/// <param name="VsDatDwCam">下相机视觉数据</param>
/// <param name="CaliOfs">校准值=放料位置-下拍照位置-上拍照位置</param>
/// <param name="RotCent">旋转中心</param>
/// <param name="RotCap">对应旋转中心的拍照位置</param>
/// <param name="VsOfs">校准值=下相机识别点与放置识别点的偏差</param>
/// <param name="VsUpCamAglRef">校准VsOfs时上相机的视觉角度</param>
/// <param name="VsDwCamAglRef">校准VsOfs时下相机的视觉角度</param>
/// <returns></returns>
public static ST_XYA CalcCamPos(ST_XY PosUpCam, ST_XYA VsDatUpCam, ST_XY PosDwCam, ST_XYA VsDatDwCam, ST_XYA CaliOfs, ST_XY RotCent, ST_XYZ RotCap, ST_XYA VsOfs, double VsUpCamAglRef, double VsDwCamAglRef)
{
//cali_ofs = st_pos_affine_place - st_pos_affine_downcam_cap - st_pos_affine_upcam_cap
//点对点
ST_XYA StPlacePos = CaliOfs + PosDwCam.ToXYA() + PosUpCam.ToXYA() + (VsDatDwCam - VsDatUpCam);
//偏差旋转
double a = VsDatUpCam.a - VsUpCamAglRef;
ST_XYA NewOfs = new ST_XYA();
Utility.PointRotToNewPoint(VsOfs, new ST_XY(), a, ref NewOfs);
StPlacePos.x += NewOfs.x;
StPlacePos.y += NewOfs.y;
//吸头旋转
ST_XY NewRot = RotCent - (PosDwCam - RotCap.ToXY());
ST_XYA NewPos = new ST_XYA();
a = -VsOfs.a + (VsDatDwCam.a - VsDwCamAglRef) - (VsDatUpCam.a - VsUpCamAglRef);
Utility.PointRotToNewPoint(VsDatDwCam, NewRot, -a, ref NewPos);
NewPos = NewPos - VsDatDwCam;
StPlacePos = StPlacePos + NewPos;
StPlacePos.a = a;
return(StPlacePos);
}
public ST_XY ToXY()
{
ST_XY xy = new ST_XY();
xy.x = x;
xy.y = y;
return(xy);
}
/// <summary>
/// 获取以指定点为中心旋转指定角度后的新坐标
/// </summary>
/// <param name="CurPos">需要旋转的点</param>
/// <param name="RolCenter">旋转中心</param>
/// <param name="Angle">旋转角度</param>
public static void PointRotToNewPoint(ST_XYA CurPos, ST_XY RotCenter, double Angle, ref ST_XYA NewPos)
{
//转为弧度
Angle = Angle / 180.0 * Math.PI;
//计算旋转后XY偏差
NewPos.x = Math.Cos(Angle) * (CurPos.x - RotCenter.x) - Math.Sin(Angle) * (CurPos.y - RotCenter.y) + RotCenter.x;
NewPos.y = Math.Cos(Angle) * (CurPos.y - RotCenter.y) + Math.Sin(Angle) * (CurPos.x - RotCenter.x) + RotCenter.y;
NewPos.a = CurPos.a + Angle;
}
// VsTg 目标识别点坐标（上相机），st_VsShapeOrgOffset轮廓原点坐标（上相机），st_VsShapeInit轮廓初始识别点（下相机），返回偏差Offset
public static EM_RES CalcOffset(ST_XYA st_VsTg, ST_XYA st_VsShapeInit, ST_XYA st_VsShapeOrgOffset, ref ST_XYA st_Offset)
{
//目标点(0,0)绕偏移点(原点偏移)旋转
ST_XYA st_new = new ST_XYA();
ST_XY st_org = new ST_XY();
// st_Offset.a = st_VsShapeOrgOffset.a;// -st_VsShapeInit.z;
PointRotToNewPoint(st_VsShapeInit, st_org, st_VsShapeOrgOffset.a, ref st_new);
st_Offset.x = st_VsTg.x - (st_VsShapeOrgOffset.x + st_new.x);
st_Offset.y = st_VsTg.y - (st_VsShapeOrgOffset.y + st_new.y);
st_Offset.a = st_VsShapeOrgOffset.a;// -st_VsShapeInit.z;
return(EM_RES.OK);
}
