public ST_XYZ ToXYZ()
{
ST_XYZ xyz = new ST_XYZ();
xyz.x = x;
xyz.y = y;
xyz.z = z;
return(xyz);
}
/// <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);
}
