#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "BQ_workpieceCornerExtraction_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>

//version 1.1.0 : base version release to customer, output corner coordinate
//version 1.2.0 : add position length output
//version 1.2.1 : fix bugs for ver1.2.0
//version 1.3.0 : 算法对同一brach判断添加了距离判断
//version 1.4.0 : 使用轮廓点拐角判断目标类型
//version 1.4.1 : (1)使用聚类方法寻找准确轮廓点（2）使用拐角和R极值方法判断目标类型
std::string	m_strVersion = "1.4.1";
const char* wd_BQWorkpieceCornerVersion(void)
{
	return m_strVersion.c_str();
}

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sx_BQ_getBaseCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getPlaneCalibPara2(scanLines);
}

//相机姿态调平，并去除地面
void sx_BQ_lineDataR(
	std::vector< SVzNL3DPosition>& a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT_vector(a_line, camPoseR, groundH);
}

SVzNL3DPoint _translatePoint(SVzNL3DPoint point, const double rMatrix[9])
{
	SVzNL3DPoint result;
	double x = point.x * rMatrix[0] + point.y * rMatrix[1] + point.z * rMatrix[2];
	double y = point.x * rMatrix[3] + point.y * rMatrix[4] + point.z * rMatrix[5];
	double z = point.x * rMatrix[6] + point.y * rMatrix[7] + point.z * rMatrix[8];
	result.x = x;
	result.y = y;
	result.z = z;
	return result;
}

//获取生长树的ROI
void sg_getTreeROI(SSG_featureTree* a_tree)
{
	if (a_tree->treeNodes.size() == 0)
	{
		a_tree->roi.left = 0;
		a_tree->roi.right = 0;
		a_tree->roi.top = 0;
		a_tree->roi.bottom = 0;
	}
	else
	{
		a_tree->roi.left = a_tree->treeNodes[0].jumpPos.x;
		a_tree->roi.right = a_tree->treeNodes[0].jumpPos.x;
		a_tree->roi.top = a_tree->treeNodes[0].jumpPos.y;
		a_tree->roi.bottom = a_tree->treeNodes[0].jumpPos.y;
		for (int i = 1, i_max = a_tree->treeNodes.size(); i < i_max; i++)
		{
			if (a_tree->roi.left > a_tree->treeNodes[i].jumpPos.x)
				a_tree->roi.left = a_tree->treeNodes[i].jumpPos.x;
			if (a_tree->roi.right < a_tree->treeNodes[i].jumpPos.x)
				a_tree->roi.right = a_tree->treeNodes[i].jumpPos.x;
			if (a_tree->roi.top > a_tree->treeNodes[i].jumpPos.y)
				a_tree->roi.top = a_tree->treeNodes[i].jumpPos.y;
			if (a_tree->roi.bottom < a_tree->treeNodes[i].jumpPos.y)
				a_tree->roi.bottom = a_tree->treeNodes[i].jumpPos.y;
		}
	}
	return;
}

void _getEdgeContour(SSG_featureTree* a_tree, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool isVScan)
{
	for (int j = 0, j_max = (int)a_tree->treeNodes.size(); j < j_max; j++)
	{
		SSG_basicFeature1D* a_feature = &a_tree->treeNodes[j];
		SVzNL3DPoint a_pt;
		if (true == isVScan)
			a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
		else
			a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
		if (a_pt.z > 1e-4)//虚假目标过滤后点会置0
		{
			contour.push_back(a_pt);
		}
	}
}

int _getPointClosestContour(std::vector<SSG_featureTree> trees, bool isVscanTrees, SVzNL3DPoint seedPt, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead)
{
	double minDist = -1.0;
	int idx = -1;
	for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
	{
		SSG_basicFeature1D a_feature;
		if (true == fromHead)
			a_feature = trees[i].treeNodes[0];
		else
			a_feature = trees[i].treeNodes.back();

		SVzNL3DPoint a_pt;
		if (true == isVscanTrees)
			a_pt = scanLines[a_feature.jumpPos2D.x][a_feature.jumpPos2D.y].pt3D;
		else
			a_pt = scanLines[a_feature.jumpPos2D.y][a_feature.jumpPos2D.x].pt3D;

		double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
		if (minDist < 0)
		{
			minDist = dist;
			idx = i;
		}
		else
		{
			if(dist < minDist)
			{
				minDist = dist;
				idx = i;
			}
		}
	}
	return idx;
}

void _getEdgeLinkingContour(SSG_featureTree* a_tree, bool isVScanTree, SVzNL3DPoint seedPt, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead, double lineLen)
{
	for (int i = 0, i_max = (int)a_tree->treeNodes.size(); i < i_max; i++)
	{
		int idx = i;
		if (false == fromHead)
			idx = i_max - 1 - i;
		SSG_basicFeature1D* a_feature = &a_tree->treeNodes[idx];

		SVzNL3DPoint a_pt;
		if (true == isVScanTree)
			a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
		else
			a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
		if (a_pt.z > 1e-4)//虚假目标过滤后点会置0
		{
			double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
			if (dist > lineLen)
				break;
			contour.push_back(a_pt);
		}
	}
}

typedef struct
{
	int rgnIdx;
	std::vector<SVzNL3DPoint> edge;
	SVzNL3DPoint edge_ends[2];
	std::vector<SVzNL3DPoint> edgeLink_1;
	SVzNL3DPoint edge_link1_ends[2];
	std::vector<SVzNL3DPoint> edgeLink_2;
	SVzNL3DPoint edge_link2_ends[2];
}SSX_featureContour;

typedef struct
{
	double angle;
	SVzNL3DPoint corner[3];
	double line_a, line_b, line_c; //边的垂线方程 ax+by+c = 0
}SWD_branchInfo;

//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 
cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta)
{
	return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta)));
}

bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
	return a.angle < b.angle;
}

int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
{
	int ptNum = 0;
	for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++)
	{
		if ( (abs(lineData[i].pt3D.z) > 1e-4) || 
			(abs(lineData[i].pt3D.x > 1e-4)) ||
			(abs(lineData[i].pt3D.y > 1e-4)))
			ptNum++;
	}
	return ptNum;
}

//计算分支信息
int _getBranchInfo(
	int validStartLine,   //开始扫描边界
	int validEndLine, //结束扫描边界
	bool partialScan, //当相机视野不能覆盖工件全部时，partialScan为true,
	std::vector<SWD_polarPt>& polarPoints,
	SWD_polarPt branchCorner,
	SWD_polarPeakInfo branchCornerInfo,
	SWD_branchInfo* resultBranchInfo,
	std::vector<SVzNL3DPoint>& LinePts,
	std::vector< SVzNL3DPoint>& edgePt1,
	std::vector< SVzNL3DPoint>& edgePt2
	)
{
	int contourPtSize = (int)polarPoints.size();
	std::vector<SWD_polarPt> branchContourPts;

	int validStartLineWin = validStartLine + 3;
	int validEndLineWin = validEndLine - 3; 
	if (validEndLineWin < 0)
		validEndLineWin = 0;
	int cornerWin = 2;
	int LineDir, contourDir, startIdx, endingIdx;
	if (branchCornerInfo.cornerDir == 2)//逆时针
	{
		startIdx = branchCorner.cptIndex - cornerWin;  //此处将corner周围的点不计算在内
		if (startIdx < 0)
			startIdx += contourPtSize;
		LineDir = -1;
		contourDir = 1;
		endingIdx = branchCornerInfo.L1_ptIndex;
	}
	else  //顺时针
	{
		startIdx = (branchCorner.cptIndex + cornerWin) % contourPtSize;
		LineDir = 1;
		contourDir = -1;
		endingIdx = branchCornerInfo.L2_ptIndex;
	}

	int ptIdx = startIdx;
	while (1)
	{
		SVzNL3DPoint a_pt = { polarPoints[ptIdx].x, polarPoints[ptIdx].y, polarPoints[ptIdx].z };
		LinePts.push_back(a_pt);
		if (ptIdx == endingIdx)
			break;
		ptIdx += LineDir;
		if (ptIdx < 0)
			ptIdx += contourPtSize;
		else
			ptIdx = ptIdx % contourPtSize;
	}
	if (LinePts.size() < 5)
		return -1;

	//当直角由于扫描边界被切成一个上边很小的梯形时，检查出的角点可能不是真正的角点，需要进行检查
	//迭代计算真正的角点：取corner两边5个点，若有边界，检查
	int chkWin = 5;
	bool toRefine = false;
	for (int i = -chkWin; i <= chkWin; i++)
	{
		int idx = branchCorner.cptIndex + i;
		if (idx < 0)
			idx += contourPtSize;
		else
			idx = idx % contourPtSize;
		if ((partialScan == true) &&
			((polarPoints[idx].lineIdx == validStartLine) || (polarPoints[idx].lineIdx == validEndLine)))
		{
			toRefine = true;
			break;
		}
	}
	if (true == toRefine)
	{
		SVzNL3DPoint pt1 = LinePts[0];
		SVzNL3DPoint pt2 = LinePts.back();
		double aa, bb, cc;
		compute2ptLine(	pt1, pt2, &aa, &bb, &cc);
		//计算真正的角点
		double maxH = 0;
		int maxHPos = 0;
		for (int i = 0; i < (int)LinePts.size(); i++)
		{
			double H = computePtDistToLine(LinePts[i].x, LinePts[i].y, aa, bb, cc);
			if (maxH < H)
			{
				maxH = H;
				maxHPos = i;
			}
		}
		//迭代一次
		int recEnding = maxHPos * 2;
		if (recEnding >= LinePts.size())
			recEnding = (int)LinePts.size() - 1;
		pt1 = LinePts[0];
		pt2 = LinePts[recEnding];
		compute2ptLine(pt1, pt2, &aa, &bb, &cc);
		//计算真正的角点
		maxH = 0;
		maxHPos = 0;
		for (int i = 0; i < recEnding; i++)
		{
			double H = computePtDistToLine(LinePts[i].x, LinePts[i].y, aa, bb, cc);
			if (maxH < H)
			{
				maxH = H;
				maxHPos = i;
			}
		}

		int cptIdx = startIdx + LineDir * maxHPos;
		if (cptIdx < 0)
			cptIdx += contourPtSize;
		else
			cptIdx = cptIdx % contourPtSize;
		branchCorner = polarPoints[cptIdx];	
		if (branchCornerInfo.cornerDir == 2)//逆时针
		{
			startIdx = branchCorner.cptIndex - cornerWin;  //此处将corner周围的点不计算在内
			if (startIdx < 0)
				startIdx += contourPtSize;
		}
		else  //顺时针
			startIdx = (branchCorner.cptIndex + cornerWin) % contourPtSize;

		LinePts.clear();
		ptIdx = startIdx;
		while (1)
		{
			SVzNL3DPoint a_pt = { polarPoints[ptIdx].x, polarPoints[ptIdx].y, polarPoints[ptIdx].z };
			LinePts.push_back(a_pt);
			if (ptIdx == endingIdx)
				break;
			ptIdx += LineDir;
			if (ptIdx < 0)
				ptIdx += contourPtSize;
			else
				ptIdx = ptIdx % contourPtSize;
		}
		if (LinePts.size() < 5)
			return -1;
	}

	//拟合直线: ax+by+c = 0
	double _a = 0, _b = 0, _c = 0;
	lineFitting_abc(LinePts, &_a, &_b, &_c);
	SVzNL3DPoint endingPt0 = LinePts.back();
	//计算端点垂足
	SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c);
	//提取其它轮廓点
	double minDist = -1;
	for (int i = cornerWin; i < contourPtSize; i++)  //避开角点周围
	{
		int ptIdx = i * contourDir + branchCorner.cptIndex;
		if (ptIdx < 0)
			ptIdx += contourPtSize;
		else
			ptIdx = ptIdx % contourPtSize;
		SWD_polarPt a_pt = polarPoints[ptIdx];
		SVzNL2DPointD contourFoot = sx_getFootPoint_abc(a_pt.x, a_pt.y, _a, _b, _c);
		double dist = sqrt(pow(footPt.x - contourFoot.x, 2) + pow(footPt.y - contourFoot.y, 2));
		if (minDist < 0)
			minDist = dist;
		else
			minDist = minDist > dist ? dist : minDist;

		if ((dist < 10) &&(dist > (minDist + 1.0)))
			break;
		branchContourPts.push_back(a_pt);
	}
	//检查轮廓点中有没有边界点。如果有边界点，需要进行推理
	int contourPtNum = (int)branchContourPts.size();
	bool hasSidePt = false;
	for (int m = 0; m < contourPtNum; m++)
	{
		if ((partialScan == true) &&
			((branchContourPts[m].lineIdx == validStartLine) ||
				(branchContourPts[m].lineIdx == validEndLine)))
		{
			branchContourPts[m].z = -1.0;  //label
			hasSidePt = true;
		}
	}
	//分开branch的另两段
	if (false == hasSidePt)
	{
		double maxDist = -1;
		int maxPos = -1;
		//找到拐点:计算所有点与端点的距离，以最大值作为拐点
		for (int m = 0; m < contourPtNum; m++)
		{
			double dist = sqrt(pow(branchContourPts[m].x - endingPt0.x, 2) +
								pow(branchContourPts[m].y - endingPt0.y, 2));
			if (maxDist < 0)
			{
				maxDist = dist;
				maxPos = m;
			}
			else
			{
				if (maxDist < dist)
				{
					maxDist = dist;
					maxPos = m;
				}
			}
		}
		//分隔两段
		int start0 = 0;
		int end0 = maxPos - cornerWin;
		
		for (int m = start0; m <= end0; m++)
		{
			SVzNL3DPoint a_pt = { branchContourPts[m].x, branchContourPts[m].y, branchContourPts[m].z };
			edgePt1.push_back(a_pt);
		}

		int start1 = maxPos + cornerWin;
		int end1 = contourPtNum - 1;
		
		for (int m = start1; m <= end1; m++)
		{
			SVzNL3DPoint a_pt = { branchContourPts[m].x, branchContourPts[m].y, branchContourPts[m].z };
			edgePt2.push_back(a_pt);
		}

		//拟合求交点
		double edge1_a, edge1_b, edge1_c;
		lineFitting_abc(edgePt1, &edge1_a, &edge1_b, &edge1_c);
		double edge2_a, edge2_b, edge2_c;
		lineFitting_abc(edgePt2, &edge2_a, &edge2_b, &edge2_c);

		//计算交点
		SWD_branchInfo a_branchInfo;
		a_branchInfo.corner[0] = computeLineCrossPt_abs(_a, _b, _c, edge1_a, edge1_b, edge1_c);
		a_branchInfo.corner[0].z = computeMeanZ(edgePt1);
		a_branchInfo.corner[2] = computeLineCrossPt_abs(edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c);
		a_branchInfo.corner[2].z = a_branchInfo.corner[0].z;
		a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2;
		a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2;
		a_branchInfo.corner[1].z = a_branchInfo.corner[0].z;
		//
		a_branchInfo.angle = branchCorner.angle;

		a_branchInfo.line_a = edge1_b;
		a_branchInfo.line_b = -edge1_a;
		a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y);
		*resultBranchInfo = a_branchInfo;
	}
	else
	{
		//将边界去除，剩下的两段进行处理
		std::vector< SVzNL3DPoint> edgePt1;
		for (int i = 0; i < contourPtNum; i++)
		{
			SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z };
			if ((partialScan == true) &&
				((branchContourPts[i].lineIdx <= validStartLineWin)|| (branchContourPts[i].lineIdx >= validEndLineWin)))
			{
				break;
			}
			edgePt1.push_back(a_pt);
		}
		std::vector< SVzNL3DPoint> edgePt2;
		for (int i = contourPtNum - 1; i >= 0; i--)
		{
			SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z };
			if ((partialScan == true) &&
				((branchContourPts[i].lineIdx <= validStartLineWin) || (branchContourPts[i].lineIdx >= validEndLineWin)))
				break;
			edgePt2.insert(edgePt2.begin(), a_pt);
		}
		//计算第一段在直线上的垂足，取垂足的质心。过质心的垂线
		int edge1PtNum = (int)edgePt1.size();
		int edge2PtNum = (int)edgePt2.size();
		if ((edge1PtNum == 0) || (edge2PtNum == 0))
		{
			return -1;
		}
		SVzNL2DPointD foot = { 0.0,0.0 };
		for (int i = 0; i < edge1PtNum; i++)
		{
			SVzNL2DPointD a_foot = sx_getFootPoint_abc(edgePt1[i].x, edgePt1[i].y, _a, _b, _c);
			foot.x += a_foot.x;
			foot.y += a_foot.y;
		}
		foot.x = foot.x / edge1PtNum;
		foot.y = foot.y / edge1PtNum;
		double edge1_a, edge1_b, edge1_c;
		edge1_a = _b;
		edge1_b = -_a;
		edge1_c = _a * foot.y - _b * foot.x;
		//计算第二段在直线上的垂足，取垂足的质心。过质心的垂线
		foot.x = 0;
		foot.y = 0;
		for (int i = 0; i < edge2PtNum; i++)
		{
			SVzNL2DPointD a_foot = sx_getFootPoint_abc(edgePt2[i].x, edgePt2[i].y, edge1_a, edge1_b, edge1_c);
			foot.x += a_foot.x;
			foot.y += a_foot.y;
		}
		foot.x = foot.x / edge2PtNum;
		foot.y = foot.y / edge2PtNum;
		double edge2_a, edge2_b, edge2_c;
		edge2_a = _a;
		edge2_b = _b;
		edge2_c = -_a * foot.x - _b * foot.y;
		//计算交点
		SWD_branchInfo a_branchInfo;
		a_branchInfo.corner[0] = computeLineCrossPt_abs(_a, _b, _c, edge1_a, edge1_b, edge1_c);
		a_branchInfo.corner[0].z = computeMeanZ(edgePt1);
		a_branchInfo.corner[2] = computeLineCrossPt_abs(edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c);
		a_branchInfo.corner[2].z = a_branchInfo.corner[0].z;
		a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2;
		a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2;
		a_branchInfo.corner[1].z = a_branchInfo.corner[0].z;
		//
		a_branchInfo.angle = branchCorner.angle;
		a_branchInfo.line_a = _a;
		a_branchInfo.line_b = _b;
		a_branchInfo.line_c = -(_a * a_branchInfo.corner[1].x + _b * a_branchInfo.corner[1].y);
		*resultBranchInfo = a_branchInfo;
	}
	return 0;
}


SVzNL3DPoint computeEdgeCross(double angle, std::vector<SWD_polarPt>& polarPoints, double a, double b, double c)
{
	double minDist = -1;
	int angleIdx = -1;
	int ptSize = (int)polarPoints.size();
	for (int i = 0; i < ptSize; i++)
	{
		double diff = computeAngleDiff(angle, polarPoints[i].angle);
		if (diff < 30)  //搜索60度范围
		{
			double H = computePtDistToLine(polarPoints[i].x, polarPoints[i].y, a, b, c);
			if (minDist < 0)
			{
				minDist = H;
				angleIdx = i;
			}
			else
			{
				if (minDist > H)
				{
					minDist = H;
					angleIdx = i;
				}
			}
		}
	}
	SVzNL3DPoint a_pt = { polarPoints[angleIdx].x, polarPoints[angleIdx].y, polarPoints[angleIdx].z };
	return a_pt;
}


bool checkSameBranch(SWD_polarPt& corner_1, SWD_polarPt& corner_2, 
	std::vector<SWD_polarPt>& polarPoints,
	double center_x, double center_y)
{
	int size = (int)polarPoints.size();
	int midPtIdx;
	if (corner_2.cptIndex > corner_1.cptIndex)
		midPtIdx = (corner_1.cptIndex + corner_2.cptIndex) / 2;
	else
	{
		midPtIdx = (size - corner_1.cptIndex + corner_2.cptIndex) / 2 + corner_1.cptIndex;
		if (midPtIdx >= size)
			midPtIdx = midPtIdx - size;
	}

	SWD_polarPt& midPt = polarPoints[midPtIdx];
	double a1, b1, c1;
	compute2ptLine_2(center_x, center_y, midPt.x, midPt.y, &a1, &b1, &c1);
	double a2, b2, c2;
	compute2ptLine_2(corner_1.x, corner_1.y, corner_2.x, corner_2.y, &a2, &b2, &c2);
	//计算交点
	SVzNL3DPoint crossPt = computeLineCrossPt_abs(a1, b1, c1, a2, b2, c2);
	double len = sqrt(pow(crossPt.x - center_x, 2) + pow(crossPt.y - center_y, 2));
	double dist_diff = midPt.R / len;
	if ( (dist_diff > 0.95) &&(dist_diff < 1.05))
		return true;
	else
		return false;

}

//计算封闭序列（首尾相连）在XOY平面内前向角和后向角以及拐角。 前向角：序号+； 后向角：序号-
void _computeClosedPntListDirCorners(std::vector<SWD_polarPt>& polarPoints, double scale, std::vector< SSG_dirCornerAngle>& dirCornerAngles)
{
	int pntSize = (int)polarPoints.size();
	//std::fill(dirCornerAngles.begin(), dirCornerAngles.end(), SSG_dirCornerAngle{ 0, 0, 0, 0, 0, 0 });
	dirCornerAngles.resize(pntSize);
	for (int i = 0; i < pntSize; i++)
	{
		if (i == 68)
			int kkk = 1;
		memset(&dirCornerAngles[i], 0, sizeof(SSG_dirCornerAngle));
		SWD_polarPt& a_polarPt = polarPoints[i];
		//前向寻找
		int minus_i = -1;
		for (int loop = i - 1; loop >= i-pntSize; loop--)
		{
			int j = loop >= 0 ? loop : (loop + pntSize);
			double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
				pow(polarPoints[i].y - polarPoints[j].y, 2));
			if (dist >= scale)
			{
				minus_i = j;
				break;
			}
		}
		//后向寻找
		int plus_i = -1;
		for (int loop = i + 1; loop < i+pntSize; loop++)
		{
			int j = loop % pntSize;
			double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
				pow(polarPoints[i].y - polarPoints[j].y, 2));
			if (dist >= scale)
			{
				plus_i = j;
				break;
			}
		}
		//计算拐角
		if ((minus_i >= 0) && (plus_i >= 0))
		{
			double backwardAngle = atan2(polarPoints[i].y - polarPoints[minus_i].y, polarPoints[i].x - polarPoints[minus_i].x) * 180.0 / PI;
			double forwardAngle = atan2(polarPoints[plus_i].y - polarPoints[i].y, polarPoints[plus_i].x - polarPoints[i].x) * 180.0 / PI;
			dirCornerAngles[i].forwardAngle = forwardAngle;
			dirCornerAngles[i].backwardAngle = backwardAngle;

			double corner = forwardAngle - backwardAngle;
			if (corner < -180)
				corner += 360;
			else if (corner > 180)
				corner = corner - 360;

			dirCornerAngles[i].flag = 0;
			//过滤掉由于极角和方向角相近产生的轮廓点位序错误导致的corner错误
			double polarAngle = dirCornerAngles[i].point.angle;
			double angleDiff1 = abs(polarAngle - dirCornerAngles[i].forwardAngle);
			if (angleDiff1 > 180)
				angleDiff1 = 360 - angleDiff1;
			double angleDiff2 = abs(polarAngle - dirCornerAngles[i].backwardAngle);
			if (angleDiff2 > 180)
				angleDiff2 = 360 - angleDiff2;
			if ((angleDiff1 < 10) || (angleDiff2 < 10))  //过滤掉边与极线方向相近导致的波动
			{
				if (dirCornerAngles[i].corner > 160)  //乱序产生角度错误
				{
					dirCornerAngles[i].flag = -1;
				}
			}
			
			dirCornerAngles[i].corner = corner;  //图像坐标系与正常坐标系y方向相反，所以有“-”号
			dirCornerAngles[i].pntIdx = i;
			dirCornerAngles[i].forward_pntIdx = plus_i;
			dirCornerAngles[i].backward_pntIdx = minus_i;
			dirCornerAngles[i].point = polarPoints[i];
		}
	}
}

//提取corner极值（较早实现函数可以使用此函数进行代码优化）
void _searchPlusCornerPeaks(
	std::vector< SSG_dirCornerAngle>& corners,
	double cutAngleTh,
	std::vector< SSG_dirCornerAngle>& cornerPlusPeaks
)
{
	std::vector<SSG_dirCornerAngle> peakCorners;
	int cornerSize = (int)corners.size();
	//搜索拐角极值
	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	SSG_dirCornerAngle* pre_data = NULL;
	for (int i = 0, i_max = cornerSize; i < i_max; i++)
	{
		if (i == 451)
			int kkk = 1;
		SSG_dirCornerAngle* curr_data = &corners[i];
		if (curr_data->flag < 0)
			continue;

		if (NULL == pre_data)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		eEdgePtIdx = i;
		double cornerDiff = curr_data->corner - pre_data->corner;
		switch (_state)
		{
		case 0:  //初态
			if (cornerDiff < 0) //下降
			{
				_state = 2;
			}
			else if (cornerDiff > 0) //上升
			{
				_state = 1;
			}
			break;
		case 1: //上升
			if (cornerDiff < 0)   //下降
			{
				if (pre_data->corner > cutAngleTh)  //截角门限，滤除点波动导致的角度变化
					peakCorners.push_back(*pre_data);
				_state = 2;
			}
			break;
		case 2: //下降
			if (cornerDiff > 0)   //  上升
				_state = 1;
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//注意：最后一个不处理，为基座位置

	//极小值点（峰顶）
	//极值比较，在尺度窗口下寻找局部极值点
	for (int i = 0, i_max = (int)peakCorners.size(); i < i_max; i++)
	{
		bool isPeak = true;
		SSG_dirCornerAngle& a_dirAngle = peakCorners[i];
		int curr_dist1 = a_dirAngle.pntIdx - a_dirAngle.backward_pntIdx;
		if (curr_dist1 < 0)
			curr_dist1 += cornerSize;
		int curr_dist2 = a_dirAngle.forward_pntIdx - a_dirAngle.pntIdx;
		if (curr_dist2 < 0)
			curr_dist2 += cornerSize;

		//minus方向寻找
		int minus_i = i;
		while (1)
		{
			minus_i--;
			if (minus_i < 0)
				minus_i += i_max;
			SSG_dirCornerAngle& minus_dirAngle = peakCorners[minus_i];
			int pntDist_0 = a_dirAngle.pntIdx - minus_dirAngle.pntIdx;
			if (pntDist_0 < 0)
				pntDist_0 += cornerSize;
			if (pntDist_0 > curr_dist1)
				break;

			if (a_dirAngle.corner < minus_dirAngle.corner)
			{
				isPeak = false;
				break;
			}
		}
		//plus方向寻找
		int plus_i = i;
		while (1)
		{
			plus_i++;
			if (plus_i >= i_max)
				plus_i = 0;
			SSG_dirCornerAngle& plus_dirAngle = peakCorners[plus_i];
			int pntDist_0 = plus_dirAngle.pntIdx - a_dirAngle.pntIdx;
			if (pntDist_0 < 0)
				pntDist_0 += cornerSize;
			if (pntDist_0 > curr_dist2)
				break;

			if (a_dirAngle.corner < plus_dirAngle.corner)
			{
				isPeak = false;
				break;
			}
		}
		if (true == isPeak)
		{
			double corner_curr = a_dirAngle.corner;
			double corner_1 = corners[a_dirAngle.forward_pntIdx].corner;
			double corner_2 = corners[a_dirAngle.backward_pntIdx].corner;
			double diff_1 = corner_curr - corner_1;
			double diff_2 = corner_curr - corner_2;
			if ((diff_1 > corner_curr / 4) && (diff_2 > corner_curr / 4))
			{
				double polarAngle = a_dirAngle.point.angle;
				double angleDiff1 = abs(polarAngle - a_dirAngle.forwardAngle);
				if (angleDiff1 > 180)
					angleDiff1 = 360 - angleDiff1;
				double angleDiff2 = abs(polarAngle - a_dirAngle.backwardAngle);
				if(angleDiff2 > 180)
					angleDiff2 = 360 - angleDiff2;
				if ((angleDiff1 < 10) || (angleDiff2 < 10))  //过滤掉边与极线方向相近导致的波动
				{
					if (a_dirAngle.corner < 160)  //乱序产生角度错误
					{
						a_dirAngle.flag = 0;
						cornerPlusPeaks.push_back(a_dirAngle);
					}
				}
				else
				{
					a_dirAngle.flag = 0;
					cornerPlusPeaks.push_back(a_dirAngle);
				}
			}
		}
	}
}

//判断两个有序点之间是否是一条直线
bool checkIsLine(
	SSG_dirCornerAngle& pt1, SSG_dirCornerAngle& pt2, 
	std::vector<SWD_polarPt>& polarPoints, 
	double cutLineDeviationTh,
	double* maxDevaion)
{
	int ringBuffSize = (int)polarPoints.size();

	int dist1 = pt2.pntIdx - pt1.pntIdx;
	if(dist1 <0)
		dist1 = dist1 + ringBuffSize;

	int dist2 = pt2.backward_pntIdx - pt1.forward_pntIdx;
	if (dist2 < 0)
		dist2 = dist2 + ringBuffSize;
	int idx1, idx2;
	if ( dist2 > dist1)  //无法避开一个尺度
	{
		idx1 = pt1.pntIdx; 
		idx2 = pt2.pntIdx; 
	}
	else
	{
		idx1 = pt1.forward_pntIdx;  //避开一个尺度
		idx2 = pt2.backward_pntIdx; //避开一个尺度
	}
	//偏离度判断
	//计算两点连线
	double a, b, c;
	compute2ptLine_2(
		polarPoints[idx1].x, polarPoints[idx1].y,
		polarPoints[idx2].x, polarPoints[idx2].y,
		&a, &b, &c);
	double tmp = sqrt(a * a + b * b);
	a = a / tmp;
	b = b / tmp;
	c = c / tmp;

	double maxDist = 0;
	int number = idx2 - idx1;
	if (number < 0)
		number += ringBuffSize;
	for (int m = 0; m < number; m++)
	{
		int idx = (m + idx1) % ringBuffSize;
		double dist = abs(a * polarPoints[idx].x + b * polarPoints[idx].y + c);
		if (maxDist < dist)
			maxDist = dist;
	}
	*maxDevaion = maxDist;
	if (maxDist < cutLineDeviationTh)  //判断为直线
		return true;
	else
		return false;
}

//根据给定的两个拐点，计算BranchInfo
void computeBranchInfo(
	SSG_dirCornerAngle& corner_1, SSG_dirCornerAngle& corner_2,
	std::vector<SWD_polarPt>& polarPoints,
	std::vector< SSG_dirCornerAngle>& cornerPlusPeaks,
	const SSX_BQworkpiecePara& workpieceParam,
#if _OUTPUT_DEBUG_DATA
	SSX_debugInfo& a_branchDebug,
#endif
	SWD_branchInfo& a_branchInfo
)
{
	int ringBuffSize = (int)polarPoints.size();
	//拟合三条直线
	// 直线0：两个角点之间的直线
	std::vector<SVzNL3DPoint> linePts_0;
	int ptSize = corner_2.backward_pntIdx - corner_1.forward_pntIdx;
	if (ptSize < 0)
		ptSize += ringBuffSize;
	for (int m = 0; m <= ptSize; m++)
	{
		int idx = (corner_1.forward_pntIdx + m) % ringBuffSize;
		SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
		linePts_0.push_back(a_pt);
	}
	double _a0 = 0, _b0 = 0, _c0 = 0;
	lineFitting_abc(linePts_0, &_a0, &_b0, &_c0);
	//直线1
	std::vector<SVzNL3DPoint> linePts_1;
	int startIdx = corner_1.backward_pntIdx;
	SWD_polarPt startPt = polarPoints[startIdx];
	for (int m = 0; m < ringBuffSize; m++)
	{
		int idx = startIdx - m;
		if (idx < 0)
			idx += ringBuffSize;

		double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
		if (dist > workpieceParam.lineLen)
			break;
		SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
		linePts_1.push_back(a_pt);
	}
	double _a1 = 0, _b1 = 0, _c1 = 0;
	lineFitting_abc(linePts_1, &_a1, &_b1, &_c1);
	//直线2
	std::vector<SVzNL3DPoint> linePts_2;
	startIdx = corner_2.forward_pntIdx;
	startPt = polarPoints[startIdx];
	for (int m = 0; m < ringBuffSize; m++)
	{
		int idx = (startIdx + m) % ringBuffSize;
		double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
		if (dist > workpieceParam.lineLen)
			break;
		SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
		linePts_2.push_back(a_pt);
	}
	double _a2 = 0, _b2 = 0, _c2 = 0;
	lineFitting_abc(linePts_2, &_a2, &_b2, &_c2);

#if _OUTPUT_DEBUG_DATA
	a_branchDebug.rgnIdx = 0;
	a_branchDebug.edge_size = (int)linePts_0.size();
	a_branchDebug.edgeLink1_size = (int)linePts_1.size();
	a_branchDebug.edgeLink2_size = (int)linePts_2.size();
	a_branchDebug.edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edge_size);
	a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size);
	a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size);

	for (int m = 0; m < a_branchDebug.edge_size; m++)
		a_branchDebug.edge[m] = linePts_0[m];
	for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
		a_branchDebug.edgeLink_1[m] = linePts_1[m];
	for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
		a_branchDebug.edgeLink_2[m] = linePts_2[m];
#endif

	linePts_1.insert(linePts_1.end(), linePts_0.begin(), linePts_0.end());
	linePts_2.insert(linePts_2.end(), linePts_0.begin(), linePts_0.end());
	//计算branch信息
	a_branchInfo.corner[0] = computeLineCrossPt_abs(_a0, _b0, _c0, _a1, _b1, _c1);
	a_branchInfo.corner[0].z = computeMeanZ(linePts_1);
	a_branchInfo.corner[2] = computeLineCrossPt_abs(_a0, _b0, _c0, _a2, _b2, _c2);
	a_branchInfo.corner[2].z = computeMeanZ(linePts_2);
	a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2;
	a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2;
	a_branchInfo.corner[1].z = computeMeanZ(linePts_0);
	//
	a_branchInfo.angle = corner_1.point.angle;
	a_branchInfo.line_a = _b0;
	a_branchInfo.line_b = -_a0;
	a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y);

	return;
}

//切角补全，输出补全后的拐角点
SVzNL3DPoint _fittingCutAngle(
	SSG_dirCornerAngle& corner_1, SSG_dirCornerAngle& corner_2,
	std::vector<SWD_polarPt>& polarPoints,
	const SSX_BQworkpiecePara& workpieceParam
)
{
	int ringBuffSize = (int)polarPoints.size();
	//拟合计算交点
	//直线1
	std::vector<SVzNL3DPoint> linePts_1;
	int startIdx = corner_1.backward_pntIdx;
	SWD_polarPt startPt = polarPoints[startIdx];
	for (int m = 0; m < ringBuffSize; m++)
	{
		int idx = startIdx - m;
		if (idx < 0)
			idx += ringBuffSize;

		double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
		if (dist > workpieceParam.lineLen)
			break;
		SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
		linePts_1.push_back(a_pt);
	}
	double _a1 = 0, _b1 = 0, _c1 = 0;
	lineFitting_abc(linePts_1, &_a1, &_b1, &_c1);
	//直线2
	std::vector<SVzNL3DPoint> linePts_2;
	startIdx = corner_2.forward_pntIdx;
	startPt = polarPoints[startIdx];
	for (int m = 0; m < ringBuffSize; m++)
	{
		int idx = (startIdx + m) % ringBuffSize;
		double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
		if (dist > workpieceParam.lineLen)
			break;
		SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
		linePts_2.push_back(a_pt);
	}
	double _a2 = 0, _b2 = 0, _c2 = 0;
	lineFitting_abc(linePts_2, &_a2, &_b2, &_c2);
	//求交点
	SVzNL3DPoint crossPt = computeLineCrossPt_abs(_a1, _b1, _c1, _a2, _b2, _c2);
	linePts_1.insert(linePts_1.end(), linePts_2.begin(), linePts_2.end());
	crossPt.z = computeMeanZ(linePts_1);
	return crossPt;
}

int _searchClosestCorner(int searchSeed, std::vector< SSG_dirCornerAngle>& cornerPlusPeaks)
{
	int minDist = -1;
	int minIdx = -1;
	for (int i = 0; i < (int)cornerPlusPeaks.size(); i++)
	{
		if (cornerPlusPeaks[i].flag > 0)
			continue;

		int dist = abs(cornerPlusPeaks[i].pntIdx - searchSeed);
		if (minDist < 0)
		{
			minDist = dist;
			minIdx = i;
		}
		else
		{
			if (minDist > dist)
			{
				minDist = dist;
				minIdx = i;
			}
		}
	}
	return minIdx;
}

//在curr_corner的正方向进行搜索
bool _fittingCutAngleBranch_1(
	std::vector<SWD_polarPt>& polarPoints,
	std::vector< SSG_dirCornerAngle>& cornerPlusPeaks,
	SSG_dirCornerAngle& curr_corner, 
	SSG_dirCornerAngle& corner_1, 
	SSG_dirCornerAngle& corner_2,
	const SSX_BQworkpiecePara& workpieceParam,
	const double branchAngle,
	const double angleTh, double cutLineDeviationTh,
#if _OUTPUT_DEBUG_DATA
	SSX_debugInfo& a_branchDebug,
#endif
	SWD_branchInfo& a_branchInfo)
{
	int ringBuffSize = (int)polarPoints.size();
	//首先判断相邻3个构成切角branch：前一corner的forward与后一corner的backward相等，后两个的拐角和与第一个相等
	double angle_diff1 = abs(curr_corner.forwardAngle - corner_1.backwardAngle);
	if (angle_diff1 > 180)
		angle_diff1 = 360 - angle_diff1;
	double angle_diff2 = abs(corner_1.forwardAngle - corner_2.backwardAngle);
	if (angle_diff2 > 180)
		angle_diff2 = 360 - angle_diff2;
	double angle_diff3 = abs(branchAngle - corner_1.corner - corner_2.corner);
	if ((angle_diff1 < angleTh) && (angle_diff2 < angleTh) && (angle_diff3 < angleTh*2.0) && 
		(corner_1.flag != 1) && (corner_2.flag != 1)&&(corner_1.pntIdx>=0) &&(corner_2.pntIdx>=0))
	{
		//偏离度判断
		//判断两个有序点之间是否是一条直线
		double maxDeviation = 0;
		bool isLine = checkIsLine(corner_1, corner_2, polarPoints, cutLineDeviationTh, &maxDeviation);
		if (true == isLine)  //判断为截角
		{
			SVzNL3DPoint cutCornerPt = _fittingCutAngle(corner_1, corner_2, polarPoints, workpieceParam);
			corner_1.point.x = cutCornerPt.x;
			corner_1.point.y = cutCornerPt.y;
			corner_1.point.z = cutCornerPt.z;
			corner_2.pntIdx = -1;
			//更新corner
			corner_1.corner = corner_1.corner + corner_2.corner;
			corner_1.forward_pntIdx = corner_2.forward_pntIdx;

			//计算branchInfo
			//根据给定的两个拐点，计算BranchInfo
			computeBranchInfo(
				curr_corner, corner_1,
				polarPoints,
				cornerPlusPeaks,
				workpieceParam,
#if _OUTPUT_DEBUG_DATA
				a_branchDebug,
#endif
				a_branchInfo
			);
			return true;
		}
	}
	else if (corner_1.flag != 1)//判断相邻两个构成切角branch，此时由于相邻很近，缺少一个corner。
	{
		double anti_dir = curr_corner.backwardAngle;
		if (anti_dir > 0)
			anti_dir = anti_dir - 180;
		else if (anti_dir < 0)
			anti_dir = anti_dir + 180;

		angle_diff1 = abs(anti_dir - corner_1.forwardAngle);
		if (angle_diff1 > 180)
			angle_diff1 = 360 - angle_diff1;

		angle_diff2 = abs(curr_corner.forwardAngle - corner_1.backwardAngle);
		if (angle_diff2 > 180)
			angle_diff2 = 360 - angle_diff2;
		if ( ((angle_diff1 < angleTh * 2)|| (angle_diff2 < angleTh)) && (corner_1.pntIdx>=0))
		{
			//判断两个有序点之间是否是一条直线
			double maxDeviation = 0;
			bool isLine = checkIsLine( curr_corner, corner_1, polarPoints, cutLineDeviationTh*2, &maxDeviation);
			if (true == isLine)  //判断为截角
			{
				//直接计算brachInfo：以curr_corner的一条边为基准
				// 直线1：对应currCorner的backward
				std::vector<SVzNL3DPoint> linePts_1;
				int startIdx = curr_corner.backward_pntIdx;
				SWD_polarPt startPt = polarPoints[startIdx];
				for (int m = 0; m < ringBuffSize; m++)
				{
					int idx = startIdx - m;
					if (idx < 0)
						idx += ringBuffSize;

					double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
					if (dist > workpieceParam.lineLen)
						break;
					SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
					linePts_1.push_back(a_pt);
				}
				double _a1 = 0, _b1 = 0, _c1 = 0;
				lineFitting_abc(linePts_1, &_a1, &_b1, &_c1);
				//直线2: 对应currCorner的fordward
				std::vector<SVzNL3DPoint> linePts_2;
				startIdx = corner_1.forward_pntIdx;
				startPt = polarPoints[startIdx];
				for (int m = 0; m < ringBuffSize; m++)
				{
					int idx = (startIdx + m)% ringBuffSize;

					double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
					if (dist > workpieceParam.lineLen)
						break;
					SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
					linePts_2.push_back(a_pt);
				}
				double _a2 = 0, _b2 = 0, _c2 = 0;
				lineFitting_abc(linePts_2, &_a2, &_b2, &_c2);
				//直线1： 过curr_corner的拐点且与直线0垂直
				double _a0 = 0, _b0 = 0, _c0 = 0;
				_a0 = _b1;
				_b0 = -_a1;
				_c0 = -(_a0 * curr_corner.point.x + _b0 * curr_corner.point.y);
				//
#if _OUTPUT_DEBUG_DATA
				a_branchDebug.rgnIdx = 0;
				a_branchDebug.edgeLink1_size = (int)linePts_1.size();
				a_branchDebug.edgeLink2_size = (int)linePts_2.size();
				a_branchDebug.edge_size = 0;
				a_branchDebug.edge = NULL;
				a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size);
				a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size);
				for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
					a_branchDebug.edgeLink_1[m] = linePts_1[m];
				for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
					a_branchDebug.edgeLink_2[m] = linePts_2[m];
#endif
				//计算branch信息
				a_branchInfo.corner[0] = { curr_corner.point.x, curr_corner.point.y, curr_corner.point.z };
				a_branchInfo.corner[2] = computeLineCrossPt_abs(_a0, _b0, _c0, _a2, _b2, _c2);
				a_branchInfo.corner[2].z = computeMeanZ(linePts_2);
				a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2;
				a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2;
				a_branchInfo.corner[1].z = (a_branchInfo.corner[0].z + a_branchInfo.corner[2].z)/2;
				//
				a_branchInfo.angle = curr_corner.point.angle;
				a_branchInfo.line_a = _b0;
				a_branchInfo.line_b = -_a0;
				a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y);
				return true;
			}
		}
	}
	return false;
}

//在curr_corner的反方向进行搜索
bool _fittingCutAngleBranch_2(
	std::vector<SWD_polarPt>& polarPoints,
	std::vector< SSG_dirCornerAngle>& cornerPlusPeaks,
	SSG_dirCornerAngle& curr_corner,
	SSG_dirCornerAngle& corner_1,
	SSG_dirCornerAngle& corner_2,
	const SSX_BQworkpiecePara& workpieceParam,
	const double branchAngle,
	const double angleTh, double cutLineDeviationTh,
#if _OUTPUT_DEBUG_DATA
	SSX_debugInfo& a_branchDebug,
#endif
	SWD_branchInfo& a_branchInfo)
{
	int ringBuffSize = (int)polarPoints.size();
	//首先判断相邻3个构成切角branch：前一corner的forward与后一corner的backward相等，后两个的拐角和与第一个相等
	double angle_diff1 = abs(curr_corner.backwardAngle - corner_1.forwardAngle);
	if (angle_diff1 > 180)
		angle_diff1 = 360 - angle_diff1;
	double angle_diff2 = abs(corner_1.backwardAngle - corner_2.forwardAngle);
	if (angle_diff2 > 180)
		angle_diff2 = 360 - angle_diff2;
	double angle_diff3 = abs(branchAngle - corner_1.corner - corner_2.corner);
	if ((angle_diff1 < angleTh) && (angle_diff2 < angleTh) && (angle_diff3 < angleTh * 2) &&
		(corner_1.flag != 1) && (corner_2.flag != 1)&&(corner_1.pntIdx>=0) &&(corner_2.pntIdx>=0))
	{
		//偏离度判断
		//判断两个有序点之间是否是一条直线
		double maxDeviation = 0;
		bool isLine = checkIsLine(corner_2, corner_1, polarPoints, cutLineDeviationTh, &maxDeviation);
		if (true == isLine)  //判断为截角
		{
			SVzNL3DPoint cutCornerPt = _fittingCutAngle(corner_2, corner_1, polarPoints, workpieceParam);
			corner_1.point.x = cutCornerPt.x;
			corner_1.point.y = cutCornerPt.y;
			corner_1.point.z = cutCornerPt.z;
			corner_2.pntIdx = -1;
			//更新corner
			corner_1.corner = corner_1.corner + corner_2.corner;
			corner_1.backward_pntIdx = corner_2.backward_pntIdx;

			//计算branchInfo
			//根据给定的两个拐点，计算BranchInfo
			computeBranchInfo(
				corner_1, curr_corner,
				polarPoints,
				cornerPlusPeaks,
				workpieceParam,
#if _OUTPUT_DEBUG_DATA
				a_branchDebug,
#endif
				a_branchInfo
			);
			return true;
		}
	}
	else if (corner_1.flag != 1)//判断相邻两个构成切角branch，此时由于相邻很近，缺少一个corner。
	{
		double anti_dir = curr_corner.forwardAngle;
		if (anti_dir > 0)
			anti_dir = anti_dir - 180;
		else if (anti_dir < 0)
			anti_dir = anti_dir + 180;

		angle_diff1 = abs(anti_dir - corner_1.backwardAngle);
		if (angle_diff1 > 180)
			angle_diff1 = 360 - angle_diff1;

		angle_diff2 = abs(curr_corner.backwardAngle - corner_1.forwardAngle);
		if (angle_diff2 > 180)
			angle_diff2 = 360 - angle_diff2;
		if ( ((angle_diff1 < angleTh * 2)||(angle_diff2 < angleTh)) && (corner_1.pntIdx >= 0))
		{
			//判断两个有序点之间是否是一条直线
			double maxDeviation = 0;
			bool isLine = checkIsLine(corner_1, curr_corner, polarPoints, cutLineDeviationTh * 2, &maxDeviation);
			if (true == isLine)  //判断为截角
			{
				//直接计算brachInfo：以curr_corner的一条边为基准
				// 直线1：对应currCorner的backward
				std::vector<SVzNL3DPoint> linePts_1;
				int startIdx = curr_corner.forward_pntIdx;
				SWD_polarPt startPt = polarPoints[startIdx];
				for (int m = 0; m < ringBuffSize; m++)
				{
					int idx = (startIdx + m) % ringBuffSize;
					double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
					if (dist > workpieceParam.lineLen)
						break;
					SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
					linePts_1.push_back(a_pt);
				}
				double _a1 = 0, _b1 = 0, _c1 = 0;
				lineFitting_abc(linePts_1, &_a1, &_b1, &_c1);
				//直线2: 对应currCorner的fordward
				std::vector<SVzNL3DPoint> linePts_2;
				startIdx = corner_1.backward_pntIdx;
				startPt = polarPoints[startIdx];
				for (int m = 0; m < ringBuffSize; m++)
				{
					int idx = startIdx - m;
					if (idx < 0)
						idx += ringBuffSize;
					double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
					if (dist > workpieceParam.lineLen)
						break;
					SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
					linePts_2.push_back(a_pt);
				}
				double _a2 = 0, _b2 = 0, _c2 = 0;
				lineFitting_abc(linePts_2, &_a2, &_b2, &_c2);
				//直线1： 过curr_corner的拐点且与直线0垂直
				double _a0 = 0, _b0 = 0, _c0 = 0;
				_a0 = _b1;
				_b0 = -_a1;
				_c0 = -(_a0 * curr_corner.point.x + _b0 * curr_corner.point.y);
				//
#if _OUTPUT_DEBUG_DATA
				a_branchDebug.rgnIdx = 0;
				a_branchDebug.edgeLink1_size = (int)linePts_1.size();
				a_branchDebug.edgeLink2_size = (int)linePts_2.size();
				a_branchDebug.edge_size = 0;
				a_branchDebug.edge = NULL;
				a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size);
				a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size);
				for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
					a_branchDebug.edgeLink_1[m] = linePts_1[m];
				for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
					a_branchDebug.edgeLink_2[m] = linePts_2[m];
#endif
				//计算branch信息
				a_branchInfo.corner[2] = { curr_corner.point.x, curr_corner.point.y, curr_corner.point.z };
				a_branchInfo.corner[0] = computeLineCrossPt_abs(_a0, _b0, _c0, _a2, _b2, _c2);
				a_branchInfo.corner[0].z = computeMeanZ(linePts_2);
				a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2;
				a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2;
				a_branchInfo.corner[1].z = (a_branchInfo.corner[0].z + a_branchInfo.corner[2].z) / 2;
				//
				a_branchInfo.angle = curr_corner.point.angle;
				a_branchInfo.line_a = _b0;
				a_branchInfo.line_b = -_a0;
				a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y);
				return true;
			}
		}
	}
	return false;
}

bool compareByBranchAngle(const SWD_branchInfo& a, const SWD_branchInfo& b) {
	return a.angle < b.angle;
}

//根据轮廓的角点计算Branch， 并判断是否缺角。缺角时通过拟合方式补全
// std::vector<SWD_polarPt>& polarPoints: 点的循环对列
// polarRPeakInfo: R极值点
// cornerPlusPeaks： corner极值点。当R极值点和corner极值点在同一个位置时，为有效的拐点
//branchCornerAngle: 支持寻找不同角度的Branch，由branchCornerAngle指定。90度为直角。
//angleTh: 角度误差范围。比如名义为90的直角，实际加工会有误差。
//cutLineDeviationTh:以截角两点连线，截角上点到直线的最大偏离。保证截角是直线
void _computeBranchesFromCornerPeaks(
	std::vector<SWD_polarPt>& polarPoints, 
	std::vector<SWD_polarPeakInfo>& polarRPeakInfo,
	std::vector< SSG_dirCornerAngle>& cornerPlusPeaks, 
	const SSX_BQworkpiecePara& workpieceParam,
	std::vector<SWD_branchInfo>& branches, 
#if _OUTPUT_DEBUG_DATA
	std::vector<SSX_debugInfo>& branchDebugData,
#endif
	double branchCornerAngle, double angleTh, double cutLineDeviationTh
)
{
	int ringBuffSize = (int)polarPoints.size();

	//以R极值为主，判断拐点是否有缺角
	//首先将R极值和Corner极值对应的极值挑选出来，这个极值视为信度最高的拐点
	std::vector<SSG_intPair> realCorners;
	double samePosDistTh = 20.0;  //R极值和corner极值位置同一的距离门限
	int samePosIdxDistTh = 15;
	for (int j = 0; j < (int)polarRPeakInfo.size(); j++)
	{
		if (polarRPeakInfo[j].flag > 0)
			continue;

		double minDist = -1;
		int minIdx = -1;

		for (int i = 0; i < (int)cornerPlusPeaks.size(); i++)
		{
			if (cornerPlusPeaks[i].flag > 0)
				continue;

			//计算两个极值点距离。距离小于5mm，视为真正拐点
			double dist = sqrt(pow(cornerPlusPeaks[i].point.x - polarRPeakInfo[j].point.x, 2) +
				pow(cornerPlusPeaks[i].point.y - polarRPeakInfo[j].point.y, 2));
			if (minDist < 0)
			{
				minDist = dist;
				minIdx = i;
			}
			else if (minDist > dist)
			{
				minDist = dist;
				minIdx = i;
			}
		}
		int idx_diff = abs(cornerPlusPeaks[minIdx].pntIdx - polarRPeakInfo[j].cptIndex);
		if ( (minDist < samePosDistTh) || (idx_diff < samePosIdxDistTh))
		{
			double angleDiff = abs(cornerPlusPeaks[minIdx].corner - branchCornerAngle);
			if (angleDiff < angleTh)
			{
				cornerPlusPeaks[minIdx].flag = 1;
				polarRPeakInfo[j].flag = 1;
			}
			else
			{
				if (cornerPlusPeaks[minIdx].corner > branchCornerAngle)
				{
					cornerPlusPeaks[minIdx].flag = 1;
					polarRPeakInfo[j].flag = 1;
				}
				else
				{
					cornerPlusPeaks[minIdx].flag = 2;
					polarRPeakInfo[j].flag = 2;
				}
			}
			SSG_intPair a_pair;
			a_pair.data_0 = minIdx;
			a_pair.data_1 = j;
			a_pair.idx = 0;
			realCorners.push_back(a_pair);
		}
	}

	//判断是否是切角
	for (int i = 0; i < (int)realCorners.size(); i++)
	{
		int idx_corner = realCorners[i].data_0;
		if (cornerPlusPeaks[idx_corner].flag != 2)
			continue;

		SSG_dirCornerAngle& curr_corner = cornerPlusPeaks[idx_corner];
		//判断是否有切角
		//与相邻corner比对，检查是否为切角
		int idx_1 = idx_corner - 1;
		if (idx_1 < 0)
			idx_1 = (int)cornerPlusPeaks.size() - 1;
		SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[idx_1];

		int idx_2 = idx_corner + 1;
		if (idx_2 >= (int)cornerPlusPeaks.size())
			idx_2 = 0;
		SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[idx_2];

		double sum1 = curr_corner.corner + corner_1.corner;
		double sum2 = curr_corner.corner + corner_2.corner;
		double angleDiff_1 = abs(sum1 - branchCornerAngle);
		double angleDiff_2 = abs(sum2 - branchCornerAngle);

		bool foundCutAngle = false;
		if ( (angleDiff_1 < angleTh) &&(corner_1.flag ==0)&&(corner_1.pntIdx >=0))
		{
			//偏离度判断
			//判断两个有序点之间是否是一条直线
			double maxDeviation = 0;
			bool isLine = checkIsLine(corner_1, curr_corner, polarPoints, cutLineDeviationTh, &maxDeviation);
			if (true == isLine)  //判断为截角
			{
				SVzNL3DPoint cutCornerPt = _fittingCutAngle(corner_1, curr_corner, polarPoints, workpieceParam);
				curr_corner.point.x = cutCornerPt.x;
				curr_corner.point.y = cutCornerPt.y;
				curr_corner.point.z = cutCornerPt.z;
				corner_1.pntIdx = -1;
				//更新corner
				curr_corner.corner = sum1;
				curr_corner.backward_pntIdx = corner_1.backward_pntIdx;
				foundCutAngle = true;
				cornerPlusPeaks[idx_corner].flag = 1;
				polarRPeakInfo[realCorners[i].data_1].flag = 1;
			}
		}
		else if ( (angleDiff_2 < angleTh) &&(false == foundCutAngle) && (corner_2.flag == 0) && (corner_2.pntIdx >= 0))
		{
			//偏离度判断
			//判断两个有序点之间是否是一条直线
			double maxDeviation = 0;
			bool isLine = checkIsLine(curr_corner, corner_2, polarPoints, cutLineDeviationTh, &maxDeviation);
			if (true == isLine)  //判断为截角
			{
				SVzNL3DPoint cutCornerPt = _fittingCutAngle(curr_corner, corner_2, polarPoints, workpieceParam);
				//拟合计算交点
				curr_corner.point.x = cutCornerPt.x;
				curr_corner.point.y = cutCornerPt.y;
				curr_corner.point.z = cutCornerPt.z;
				corner_2.pntIdx = -1;
				//更新corner
				curr_corner.corner = sum2;
				curr_corner.forward_pntIdx = corner_2.forward_pntIdx;
				cornerPlusPeaks[idx_corner].flag = 1;
				polarRPeakInfo[realCorners[i].data_1].flag = 1;
			}
		}
		else
		{
			double angleDiff = abs(curr_corner.corner - branchCornerAngle);
			if (angleDiff < angleTh * 1.5)
			{
				cornerPlusPeaks[idx_corner].flag = 1;
				polarRPeakInfo[realCorners[i].data_1].flag = 1;
			}
		}
	}

	//对没有对上的R极值进行处理：以R极值为基准，位置以corner为准
	int size_peaks = (int)polarRPeakInfo.size();
	for (int i = 0; i < size_peaks; i++)
	{
		if (polarRPeakInfo[i].flag > 0)
			continue;

		//搜索最近的corner
		int nearestCorner = _searchClosestCorner(polarRPeakInfo[i].cptIndex, cornerPlusPeaks);
		if (nearestCorner < 0)
			continue;

		//检查有没有切角
		SSG_dirCornerAngle& curr_corner = cornerPlusPeaks[nearestCorner];
		int tmpIdx_1 = nearestCorner - 1;
		if (tmpIdx_1 < 0)
			tmpIdx_1 = size_peaks - 1;
		SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[tmpIdx_1];

		int tmpIdx_2 = (nearestCorner + 1) % size_peaks;
		SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[tmpIdx_2];

		double sumCorner_1 = curr_corner.corner + corner_1.corner;
		double angleDiff_1 = abs(sumCorner_1 - branchCornerAngle);

		double sumCorner_2 = curr_corner.corner + corner_2.corner;
		double angleDiff_2 = abs(sumCorner_2 - branchCornerAngle);

		bool foundCutAngle = false;
		if ((angleDiff_1 < angleTh) && (corner_1.flag == 0)&&(corner_1.pntIdx >=0))
		{
			//偏离度判断
			//判断两个有序点之间是否是一条直线
			double maxDeviation = 0;
			bool isLine = checkIsLine(corner_1, curr_corner, polarPoints, cutLineDeviationTh, &maxDeviation);
			if (true == isLine)  //判断为截角
			{
				SVzNL3DPoint cutCornerPt = _fittingCutAngle(corner_1, curr_corner, polarPoints, workpieceParam);
				curr_corner.point.x = cutCornerPt.x;
				curr_corner.point.y = cutCornerPt.y;
				curr_corner.point.z = cutCornerPt.z;
				corner_1.pntIdx = -1;
				//更新corner
				curr_corner.corner = sumCorner_1;
				curr_corner.backward_pntIdx = corner_1.backward_pntIdx;
				foundCutAngle = true;
				curr_corner.flag = 1;
			}

		}
		if ((angleDiff_2 < angleTh) && (corner_2.flag == 0)&&(corner_2.pntIdx>=0) && (false == foundCutAngle))
		{
			//偏离度判断
			//判断两个有序点之间是否是一条直线
			double maxDeviation = 0;
			bool isLine = checkIsLine(curr_corner, corner_2, polarPoints, cutLineDeviationTh, &maxDeviation);
			if (true == isLine)  //判断为截角
			{
				SVzNL3DPoint cutCornerPt = _fittingCutAngle(curr_corner, corner_2, polarPoints, workpieceParam);
				//拟合计算交点
				curr_corner.point.x = cutCornerPt.x;
				curr_corner.point.y = cutCornerPt.y;
				curr_corner.point.z = cutCornerPt.z;
				corner_2.pntIdx = -1;
				//更新corner
				curr_corner.corner = sumCorner_2;
				curr_corner.forward_pntIdx = corner_2.forward_pntIdx;
				curr_corner.flag = 1;
			}
		}
	}

#if 0
	//生成branch配对,
	int pairSize = (int)realCorners.size();
	for (int i = 0; i < (int)pairSize; i++)
	{
		int cornerPeakIdx_1 = realCorners[i].data_0;
		if (cornerPeakIdx_1 < 0)
			continue;

		SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[cornerPeakIdx_1];
		int tmpIdx = (i + 1) % pairSize;
		if (realCorners[tmpIdx].data_0)
			continue;

		int cornerPeakIdx_2 = realCorners[tmpIdx].data_0;
		SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[cornerPeakIdx_2];
		//判断两个有序点之间是否是一条直线
		bool isBranch = checkIsLine(corner_1, corner_2, polarPoints, cutLineDeviationTh);
		if (true == isBranch)  //判断为branch
		{
			//根据给定的两个拐点，计算BranchInfo
#if _OUTPUT_DEBUG_DATA
			SSX_debugInfo a_branchDebug;
#endif
			SWD_branchInfo a_branchInfo;
			computeBranchInfo(
				corner_1, corner_2,
				polarPoints,
				cornerPlusPeaks,
				workpieceParam,
#if _OUTPUT_DEBUG_DATA
				a_branchDebug,
#endif
				a_branchInfo
			);
			branches.push_back(a_branchInfo);
#if _OUTPUT_DEBUG_DATA
			branchDebugData.push_back(a_branchDebug);
#endif
			realCorners[i].data_0 = -1;
			realCorners[tmpIdx].data_0 = -1;
		}
	}

	//对剩下的没有配对的拐点进行处理


	//判断是否有缺角
	int size_peaks = (int)cornerPlusPeaks.size();
	for (int i = 0; i < size_peaks; i++)
	{
		if (cornerPlusPeaks[i].pntIdx < 0)
			continue;

		SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[i];
		int tmpIdx = (i + 1) % size_peaks;
		SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[tmpIdx];
		if (corner_2.pntIdx < 0)
			continue;

		double sumCorner = corner_1.corner + corner_2.corner;
		double angleDiff = abs(sumCorner - branchCornerAngle);
		if (angleDiff < angleTh)
		{
			//偏离度判断
			//判断两个有序点之间是否是一条直线
			bool isLine = checkIsLine(	corner_1, corner_2, polarPoints, cutLineDeviationTh);
			if (true == isLine)  //判断为截角
			{
				//拟合计算交点
				//直线1
				std::vector<SVzNL3DPoint> linePts_1;
				int startIdx = corner_1.backward_pntIdx;
				SWD_polarPt startPt = polarPoints[startIdx];
				for (int m = 0; m < ringBuffSize; m++)
				{
					int idx = startIdx - m;
					if (idx < 0)
						idx += ringBuffSize;

					double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
					if (dist > workpieceParam.lineLen)
						break;
					SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
					linePts_1.push_back(a_pt);
				}
				double _a1 = 0, _b1 = 0, _c1 = 0;
				lineFitting_abc(linePts_1, &_a1, &_b1, &_c1);
				//直线2
				std::vector<SVzNL3DPoint> linePts_2;
				startIdx = corner_2.forward_pntIdx;
				startPt = polarPoints[startIdx];
				for (int m = 0; m < ringBuffSize; m++)
				{
					int idx = (startIdx + m) % ringBuffSize;
					double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
					if (dist > workpieceParam.lineLen)
						break;
					SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
					linePts_2.push_back(a_pt);
				}
				double _a2 = 0, _b2 = 0, _c2 = 0;
				lineFitting_abc(linePts_2, &_a2, &_b2, &_c2);
				//求交点
				SVzNL3DPoint crossPt = computeLineCrossPt_abs(_a1, _b1, _c1, _a2, _b2, _c2);
				linePts_1.insert(linePts_1.end(), linePts_2.begin(), linePts_2.end());
				crossPt.z = computeMeanZ(linePts_1);
				corner_1.point.x = crossPt.x;
				corner_1.point.y = crossPt.y;
				corner_1.point.z = crossPt.z;
				corner_2.pntIdx = -1;
				//更新corner
				corner_1.corner = sumCorner;
				corner_1.forward_pntIdx = corner_2.forward_pntIdx;
			}
		}
	}

	//滤除错误的Corner, 并删除
	for (int i = size_peaks-1; i >= 0; i--)
	{
		SSG_dirCornerAngle& a_corner = cornerPlusPeaks[i];
		if (a_corner.pntIdx < 0)
		{
			cornerPlusPeaks.erase(cornerPlusPeaks.begin() + i);
		}
		else
		{
			double angleDiff = abs(a_corner.corner - branchCornerAngle);
			if (angleDiff > angleTh)
			{
				cornerPlusPeaks.erase(cornerPlusPeaks.begin() + i);
			}
		}
	}
#endif
	//寻找branch: branch的特征：相邻之间为直线
	size_peaks = (int)cornerPlusPeaks.size();
	std::vector<int> validCornerPeaks;
	for (int i = 0; i < size_peaks; i++)
	{
		if (cornerPlusPeaks[i].flag == 1)
			validCornerPeaks.push_back(i);
	}
	int validPeak_size = (int)validCornerPeaks.size();
	int branchID = 1;
	for (int i = 0; i < validPeak_size; i++)
	{
		int cornerIdx = validCornerPeaks[i];
		if (cornerPlusPeaks[cornerIdx].flag == 0)
			continue;

		SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[cornerIdx];
		int tmpIdx = (i + 1) % validPeak_size;
		int nxtCornerIdx = validCornerPeaks[tmpIdx];
		SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[nxtCornerIdx];
		//判断两个有序点之间是否是一条直线
		double maxDevaion = 0;
		bool isBranch = false;
		if( (corner_1.pntIdx >=0) && (corner_2.pntIdx >=0))
			isBranch = checkIsLine(corner_1, corner_2, polarPoints, cutLineDeviationTh, &maxDevaion);
		if (true == isBranch)  //判断为branch
		{
			//根据给定的两个拐点，计算BranchInfo
#if _OUTPUT_DEBUG_DATA
			SSX_debugInfo a_branchDebug;
#endif
			SWD_branchInfo a_branchInfo;
			computeBranchInfo(
				corner_1, corner_2,
				polarPoints,
				cornerPlusPeaks,
				workpieceParam,
#if _OUTPUT_DEBUG_DATA
				a_branchDebug,
#endif
				a_branchInfo
			);
			branches.push_back(a_branchInfo);
#if _OUTPUT_DEBUG_DATA
			branchDebugData.push_back(a_branchDebug);
#endif
			corner_1.flag = 0;
			corner_2.flag = 0;
		}
	}
	//迭代检查没有形成branchInfo的极值点
	//对于不对配对的R极值点，以极值点为基准，寻找相信的平行边（两个corner的后向角相反值相等）
	for (int i = 0; i < validPeak_size; i++)
	{
		int cornerIdx = validCornerPeaks[i];
		if (cornerPlusPeaks[cornerIdx].flag == 0)
			continue;

		SSG_dirCornerAngle& curr_corner = cornerPlusPeaks[cornerIdx];
		//顺向搜索
		int tmpIdx = (cornerIdx + 1)% size_peaks;
		SSG_dirCornerAngle corner_1 = cornerPlusPeaks[tmpIdx];
		tmpIdx = (cornerIdx + 2) % size_peaks;
		SSG_dirCornerAngle corner_2 = cornerPlusPeaks[tmpIdx];
		//检查切角branch
#if _OUTPUT_DEBUG_DATA
		SSX_debugInfo a_branchDebug;
#endif
		SWD_branchInfo a_branchInfo;
		bool foundBranch = _fittingCutAngleBranch_1(
			polarPoints,
			cornerPlusPeaks,
			curr_corner,
			corner_1,
			corner_2,
			workpieceParam,
			branchCornerAngle,
			angleTh, cutLineDeviationTh,
#if _OUTPUT_DEBUG_DATA
			a_branchDebug,
#endif
			a_branchInfo);
		if (true == foundBranch)
		{
			branches.push_back(a_branchInfo);
#if _OUTPUT_DEBUG_DATA
			branchDebugData.push_back(a_branchDebug);
#endif
		}
		else
		{
			//反向搜索
			tmpIdx = cornerIdx - 1;
			if (tmpIdx < 0)
				tmpIdx += size_peaks;
			corner_1 = cornerPlusPeaks[tmpIdx];
			tmpIdx = cornerIdx - 2;
			if(tmpIdx < 0)
				tmpIdx += size_peaks;
			corner_2 = cornerPlusPeaks[tmpIdx];
			//检查切角branch

			foundBranch = _fittingCutAngleBranch_2(
				polarPoints,
				cornerPlusPeaks,
				curr_corner,
				corner_1,
				corner_2,
				workpieceParam,
				branchCornerAngle,
				angleTh, cutLineDeviationTh,
#if _OUTPUT_DEBUG_DATA
				a_branchDebug,
#endif
				a_branchInfo);
			if (true == foundBranch)
			{
				branches.push_back(a_branchInfo);
#if _OUTPUT_DEBUG_DATA
				branchDebugData.push_back(a_branchDebug);
#endif
			}
		}
	}

	//排序
	std::sort(branches.begin(), branches.end(), compareByBranchAngle);
	return;
}

SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines_src,
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	const SSG_planeCalibPara groundCalibPara,
	const SSX_BQworkpiecePara workpieceParam,
#if _OUTPUT_DEBUG_DATA
	std::vector<SSX_debugInfo>& debug_contours,
#endif
	int* errCode)
{
	*errCode = 0;
	SSX_BQworkpieceResult workpieceCorners;
	memset(&workpieceCorners, 0, sizeof(SSX_BQworkpieceResult));
	int lineNum = (int)scanLines_src.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return workpieceCorners;
	}

	//以数据进行平滑处理（填孔）,减小边缘上点的波动导致的角度计算波动
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	int fillingHoleSize = 3;//填孔大小:小于3个的孔洞
	wd_pointCloudHoleFilling(scanLines_src, scanLines, fillingHoleSize);  //填孔大小

	//将开始和结束的空白扫描线去除，获得扫描边界
	int validStartLine = -1;
	for (int i = 0; i < lineNum; i++)
	{
		int linePtNum = _counterLinePtNum(scanLines[i]);
		if (linePtNum > 0)
		{
			validStartLine = i;
			break;
		}
	}
	int validEndLine = -1;
	for (int i = lineNum - 1; i >= 0; i--)
	{
		int linePtNum = _counterLinePtNum(scanLines[i]);
		if (linePtNum > 0)
		{
			validEndLine = i;
			break;
		}
	}
	if ( (validStartLine < 0) || (validEndLine < 0))
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return workpieceCorners;
	}

	int linePtNum = (int)scanLines[0].size();
	bool isGridData = true;

	//自适应各种旋转角度
	{
		std::vector<SVzNL3DPosition> featurePoints;
		//生成全局Flag
		std::vector<std::vector<int>> globalFlags;
		globalFlags.resize(lineNum);
		for (int i = 0; i < lineNum; i++)
		{
			globalFlags[i].resize(linePtNum);
			std::fill(globalFlags[i].begin(), globalFlags[i].end(), 0);
		}

		//垂直跳变特征提取
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
		for (int line = 0; line < lineNum; line++)
		{
			if (line == 250)
				int kkk = 1;

			std::vector<SVzNL3DPosition>& lineData = scanLines[line];
			if (linePtNum != (int)lineData.size())
				isGridData = false;

			//滤波，滤除异常点
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);

			std::vector<SSG_basicFeature1D> line_features;
			int dataSize = (int)lineData.size();
			sg_getLineCornerFeature_BQ(
				&lineData[0],
				dataSize,
				line,
				groundCalibPara.planeHeight,
				cornerPara, //scale通常取bagH的1/4
				line_features);
			jumpFeatures_v_raw.push_back(line_features);

			int featureNum = (int)line_features.size();
			for (int j = 0; j < featureNum; j++)
			{
				int idx_line = line_features[j].jumpPos2D.x;
				int idx_pt = line_features[j].jumpPos2D.y;

				SVzNL3DPosition a_featurePt = lineData[idx_pt];
				if (globalFlags[idx_line][idx_pt] == 0)
				{
					globalFlags[idx_line][idx_pt] = 1;
					a_featurePt.nPointIdx = (idx_line << 16) | (idx_pt & 0x0000FFFF);
					featurePoints.push_back(a_featurePt);
				}
			}
		}

		if (false == isGridData)//数据不是网格格式
		{
			*errCode = SG_ERR_NOT_GRID_FORMAT;
			return workpieceCorners;
		}

		//生成水平扫描
		std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
		hLines_raw.resize(linePtNum);
		for (int i = 0; i < linePtNum; i++)
			hLines_raw[i].resize(lineNum);
		for (int line = 0; line < lineNum; line++)
		{
			for (int j = 0; j < linePtNum; j++)
			{
				scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
				hLines_raw[j][line] = scanLines[line][j];
				hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
				hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
			}
		}

		//水平arc特征提取
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h_raw;
		int lineNum_h_raw = (int)hLines_raw.size();
		for (int line = 0; line < lineNum_h_raw; line++)
		{
			if (line == 416)
				int kkk = 1;
			std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
			//滤波，滤除异常点
			int ptNum = (int)lineData.size();
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);

			std::vector<SSG_basicFeature1D> line_features;
			int dataSize = (int)lineData.size();
			sg_getLineCornerFeature_BQ(
				&hLines_raw[line][0],
				dataSize,
				line,
				groundCalibPara.planeHeight,
				cornerPara, //scale通常取bagH的1/4
				line_features);
			jumpFeatures_h_raw.push_back(line_features);

			int featureNum = (int)line_features.size();
			for (int j = 0; j < featureNum; j++)
			{
				int idx_line = line_features[j].jumpPos2D.y;
				int idx_pt = line_features[j].jumpPos2D.x;

				SVzNL3DPosition a_featurePt = scanLines[idx_line][idx_pt];
				if (globalFlags[idx_line][idx_pt] == 0)
				{
					globalFlags[idx_line][idx_pt] = 1;
					a_featurePt.nPointIdx = (idx_line << 16) | (idx_pt & 0x0000FFFF);
					featurePoints.push_back(a_featurePt);
				}
			}
		}
#if 0
		//特征生长，用于滤除噪点
		//垂直方向特征生长（激光线方向）
		std::vector<SSG_featureTree> v_trees;
		for (int line = 0; line < lineNum; line++)
		{
			bool isLastLine = false;
			if (line == lineNum - 1)
				isLastLine = true;
			std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
			if (a_lineJumpFeature.size() > 0)
				int kkk = 1;
			if (line == 202)
				int kkk = 1;
			sg_lineFeaturesGrowing(
				line,
				isLastLine,
				a_lineJumpFeature,
				v_trees,
				growParam);
		}

		//水平方向特征生长（扫描运动方向）
		std::vector<SSG_featureTree> h_trees;
		for (int line = 0; line < lineNum_h_raw; line++)
		{
			if (line == 650)
				int kkk = 1;
			bool isLastLine = false;
			if (line == lineNum_h_raw - 1)
				isLastLine = true;
			std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
			sg_lineFeaturesGrowing(
				line,
				isLastLine,
				a_lineJumpFeature,
				h_trees,
				growParam);
		}

		std::vector<SWD_polarPt> polarPoints;
		for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
		{
			SSG_featureTree* a_vTree = &v_trees[i];
			//在原始点云上标记，同时有Mask上标记
			for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
			{
				int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x;
				int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y;
				if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
				{
					SWD_polarPt a_polarPt;
					a_polarPt.lineIdx = lineIdx;
					a_polarPt.ptIdx = ptIdx;
					a_polarPt.R = 0;
					a_polarPt.angle = 0;
					a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
					a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
					a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
					polarPoints.push_back(a_polarPt);
					scanLines[lineIdx][ptIdx].nPointIdx = -1;
				}
			}
		}
		for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
		{
			SSG_featureTree* a_hTree = &h_trees[i];
			//在原始点云上标记，同时有Mask上标记
			for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
			{
				int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y;
				int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x;
				if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
				{
					SWD_polarPt a_polarPt;
					a_polarPt.lineIdx = lineIdx;
					a_polarPt.ptIdx = ptIdx;
					a_polarPt.cptIndex = -1;
					a_polarPt.R = 0;
					a_polarPt.angle = 0;
					a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
					a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
					a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
					polarPoints.push_back(a_polarPt);
					scanLines[lineIdx][ptIdx].nPointIdx = -1;
				}
			}
		}
#else
		//聚类，提取工件轮廓
		int clusterCheckWin = 8;  //加速搜索窗口
		std::vector<std::vector< SVzNL3DPosition>> objClusters;
		double clusterDist = sqrt(pow(growParam.yDeviation_max,2) + pow(growParam.maxSkipDistance, 2));
		wd_pointClustering_speedUp(
			featurePoints,
			lineNum, linePtNum, clusterCheckWin, //搜索窗口
			clusterDist,
			objClusters  //result
		);
		//取最大的聚类作为轮廓
		int contourID = 0;
		int contourPtNum = 0;
		for (int i = 0; i < (int)objClusters.size(); i++)
		{
			if (contourPtNum < objClusters[i].size())
			{
				contourPtNum = (int)objClusters[i].size();
				contourID = i;
			}
		}
		std::vector<SWD_polarPt> polarPoints;
		for (int i = 0, i_max = (int)objClusters[contourID].size(); i < i_max; i++)
		{
			int lineIdx = objClusters[contourID][i].nPointIdx >> 16;
			int ptIdx = objClusters[contourID][i].nPointIdx & 0x0000FFFF;
			SWD_polarPt a_polarPt;
			a_polarPt.lineIdx = lineIdx;
			a_polarPt.ptIdx = ptIdx;
			a_polarPt.R = 0;
			a_polarPt.angle = 0;
			a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
			a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
			a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
			polarPoints.push_back(a_polarPt);
			scanLines[lineIdx][ptIdx].nPointIdx = -1;
		}
#endif
		//计算几何中心
		int contourPtSize = (int)polarPoints.size();
		if (contourPtSize == 0)
		{
			*errCode = SX_ERR_ZERO_CONTOUR_PT;
			return workpieceCorners;
		}

		double center_x = 0;
		double center_y = 0;
		double center_z = 0;
#if 1  //重心计算
		//工件表面点标记，用于进行重心计算
		for (int line = 0; line < lineNum; line++)
		{
			int start = -1;
			for (int j = 0; j < linePtNum; j++)
			{
				if (scanLines[line][j].nPointIdx == -1)
				{
					if (start < 0)
						start = j;
					else
					{
						for (int m = start+1; m < j; m++)
							scanLines[line][m].nPointIdx = -2;
						start = -1;
					}
				}
			}
		}
		for (int j = 0; j < linePtNum; j++)
		{
			int start = -1;
			for (int line = 0; line < lineNum; line++)
			{
				if (scanLines[line][j].nPointIdx == -1)
				{
					if (start < 0)
						start = line;
					else
					{
						for (int m = start+1; m < line; m++)
							scanLines[m][j].nPointIdx = -2;
						start = -1;
					}
				}
			}
		}
		int testNum = 0;
		for (int line = 0; line < lineNum; line++)
		{
			for (int j = 0; j < linePtNum; j++)
			{
				if ( (scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].nPointIdx == -2))
				{
					center_x += scanLines[line][j].pt3D.x;
					center_y += scanLines[line][j].pt3D.y;
					center_z += scanLines[line][j].pt3D.z;
					testNum++;
				}
			}
		}
		center_x = center_x / (double)testNum;
		center_y = center_y / (double)testNum;
		center_z = center_z / (double)testNum;
#else  //几何中心
		for (int pi = 0; pi < contourPtSize; pi++)
		{
			center_x += polarPoints[pi].x;
			center_y += polarPoints[pi].y;
		}
		center_x = center_x / (double)contourPtSize;
		center_y = center_y / (double)contourPtSize;
#endif
		//计算极坐标的R和Theta
		for (int pi = 0; pi < contourPtSize; pi++)
		{
			double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x);
			angle = (angle / PI) * 180 +180.0;
			double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2));
			polarPoints[pi].R = R;
			polarPoints[pi].angle = angle;
		}
		//按角度大小排序
		std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
		for (int pi = 0; pi < contourPtSize; pi++)
			polarPoints[pi].cptIndex = pi;  // index
		
		//计算有序边缘点的角度变化 2026.03.06
		std::vector< SSG_dirCornerAngle> dirCornerAngles;
		_computeClosedPntListDirCorners(polarPoints, workpieceParam.dirAngleScale, dirCornerAngles);
		//提取方向角拐点(正）极值
		std::vector< SSG_dirCornerAngle> cornerPlusPeaks;
		_searchPlusCornerPeaks(dirCornerAngles, workpieceParam.minCutAngleTh, cornerPlusPeaks);

		//提取R极值点
		double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
		int minRPos = -1;
		std::vector<SWD_polarPt> polarRPeakPts;
		int winSize = contourPtSize / 36;  //+-10度范围
		if (winSize < 5)
			winSize = 5;
		for (int pi = 0; pi < contourPtSize; pi++)
		{
			double currR = polarPoints[pi].R;
			if (minR < 0)
			{
				minR = currR;
				maxR = currR;
				minRPos = pi;
			}
			else
			{
				minRPos = minR > currR ? pi : minRPos;
				minR = minR > currR ? currR : minR;
				maxR = maxR < currR ? currR : maxR;
			}
			bool isPeak = true;
			for (int k = -winSize; k <= winSize; k++)
			{
				int idx = (pi + k + contourPtSize) % contourPtSize; //筒形结构
				if (polarPoints[idx].R > currR)
				{
					isPeak = false;
					break;
				}
			}
			if (true == isPeak)
				polarRPeakPts.push_back(polarPoints[pi]);
		}

		std::vector<SWD_polarPt> validPolarRPeakPts;
		std::vector<SWD_polarPeakInfo> polarPeakInfo;
		int pkId = 0;
		//过滤圆弧段的极值：由于重心偏移，圆弧段也会形成极值。根据极值两边L=直线段长度构成的张角判断
		double arcAngleChkLen = 100; //检测圆弧张角的尺度
		double validPolarPeakZRange = 50;
		for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++)
		{
			int ptidx = polarRPeakPts[i].cptIndex;
			double px, py, pz;
			px = polarRPeakPts[i].x;
			py = polarRPeakPts[i].y;
			pz = polarRPeakPts[i].z;
			double z_diff = abs(center_z - pz);
			if (z_diff > validPolarPeakZRange) //大于50mm，判断为无效
				continue;

			int LL1 = -1;
			int halfLL1 = -1;
			for (int j = ptidx - 1; j > -contourPtSize; j--)
			{
				int idx = (j + contourPtSize) % contourPtSize; //筒形结构
				double cx = polarPoints[idx].x;
				double cy = polarPoints[idx].y;
				double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
				if (len < arcAngleChkLen)
					halfLL1 = idx;
				if (len > (workpieceParam.lineLen))
				{
					LL1 = idx;
					break;
				}
			}

			int LL2 = -1;
			int halfLL2 = -1;
			for (int j = ptidx + 1; j < contourPtSize*2; j++)
			{
				int idx = j % contourPtSize; //筒形结构
				double cx = polarPoints[idx].x;
				double cy = polarPoints[idx].y;
				double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
				if (len < arcAngleChkLen)
					halfLL2 = idx;
				if (len > (workpieceParam.lineLen))
				{
					LL2 = idx;
					break;
				}
			}

			if ((LL1 >= 0) && (LL2 >= 0))
			{
				double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2));
				double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2));
				double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) +
									pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2));
				double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2);
				double theta = acos(cosTheta) * 180.0 / PI;
				if (theta < 150)
				{
					SWD_polarPeakInfo a_pkInfo;
					a_pkInfo.cptIndex = ptidx;
					a_pkInfo.L1_ptIndex = LL1;
					a_pkInfo.L2_ptIndex = LL2;
					a_pkInfo.cornerAngle = theta;
					a_pkInfo.cornerDir = 0;
					a_pkInfo.point = polarRPeakPts[i];
					a_pkInfo.flag = 0;
					polarRPeakPts[i].cptIndex = ptidx;
					polarRPeakPts[i].pkId = pkId;
					pkId++;
					validPolarRPeakPts.push_back(polarRPeakPts[i]);
					polarPeakInfo.push_back(a_pkInfo);
				}
			}
		}

		//搜索branch
		double branchCornerAngle = 90;
		double angleTh = 10;
		std::vector<SWD_branchInfo> branches;
		_computeBranchesFromCornerPeaks(
			polarPoints,
			polarPeakInfo,
			cornerPlusPeaks,
			workpieceParam,
			branches,
#if _OUTPUT_DEBUG_DATA
			debug_contours,
#endif
			branchCornerAngle, angleTh, workpieceParam.lineDeviation);

		int workpieceType = -1;
		bool partialScan = false; //当相机视野不能覆盖工件全部时，partialScan为true
		std::vector< SWD_branchInfo> branchInfo;
		if (branches.size() > 0)  //有分支（branch)
		{
			int branchNum = (int)branches.size();
			if (branchNum == 2)
				workpieceType = 3;  //节点3
			else if (branchNum == 3)
				workpieceType = 2;  //节点2
			else if (branchNum == 4)
				workpieceType = 1;  //节点1
			else
				workpieceType = 0;
			branchInfo.insert(branchInfo.end(), branches.begin(), branches.end());
		
			workpieceCorners.workpieceType = workpieceType;
			if (workpieceType == 1) //4个branch
			{
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_1[m] = branchInfo[0].corner[m];
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_2[m] = branchInfo[1].corner[m];
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_3[m] = branchInfo[2].corner[m];
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_4[m] = branchInfo[3].corner[m];
				//计算剩余信息
				double line1_a, line1_b, line1_c;
				compute2ptLine(
					workpieceCorners.corner_1[1], workpieceCorners.corner_3[1],
					&line1_a, &line1_b, &line1_c);
				double line2_a, line2_b, line2_c;
				compute2ptLine(
					workpieceCorners.corner_2[1], workpieceCorners.corner_4[1],
					&line2_a, &line2_b, &line2_c);
				workpieceCorners.center = computeLineCrossPt_abs(
					line1_a, line1_b, line1_c,
					line2_a, line2_b, line2_c);
				workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z +
					workpieceCorners.corner_3[1].z + workpieceCorners.corner_4[1].z) / 4;
				//line1旋转45度方向
				double r45_line1_a, r45_line1_b, r45_line1_c;
				rotateLine45Deg(
					line1_a, line1_b, line1_c,
					workpieceCorners.center.x, workpieceCorners.center.y,
					&r45_line1_a, &r45_line1_b, &r45_line1_c);

				double r45_line2_a, r45_line2_b, r45_line2_c;
				rotateLine45Deg(
					line2_a, line2_b, line2_c,
					workpieceCorners.center.x, workpieceCorners.center.y,
					&r45_line2_a, &r45_line2_b, &r45_line2_c);

				double angle1 = getLineAngle(r45_line1_a, r45_line1_b, r45_line1_c);
				double angle2 = getLineAngle(r45_line2_a, r45_line2_b, r45_line2_c);
				if (angle1 < angle2)
				{
					SVzNL3DPoint a_cross = computeEdgeCross(angle1, polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
					workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross((angle1+180), polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
					workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross(angle2, polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
					workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross((angle2+180), polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
					workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.center, a_cross);
				}
				else
				{
					SVzNL3DPoint a_cross = computeEdgeCross(angle2, polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
					workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross((angle2 + 180), polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
					workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross(angle1, polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
					workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross((angle1 + 180), polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
					workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.center, a_cross);
				}
			}
			else if (workpieceType == 2) //3 个branch
			{
				int startIdx = 0;
				for (int i = 0; i < 3; i++)
				{
					int nxtIdx = (i + 1) % 3;
					double diff = computeAngleDiff(branchInfo[i].angle, branchInfo[nxtIdx].angle);
					if (diff > 135)
					{
						startIdx = nxtIdx;
						break;
					}
				}

				double crossDir;
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_1[m] = branchInfo[startIdx].corner[m];
				startIdx = (startIdx + 1) % 3;
				crossDir = branchInfo[startIdx].angle;
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_2[m] = branchInfo[startIdx].corner[m];
				startIdx = (startIdx + 1) % 3;
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_3[m] = branchInfo[startIdx].corner[m];

				//计算剩余信息
				workpieceCorners.center.x = (workpieceCorners.corner_1[1].x + workpieceCorners.corner_3[1].x) / 2;
				workpieceCorners.center.y = (workpieceCorners.corner_1[1].y + workpieceCorners.corner_3[1].y) / 2;
				workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_3[1].z) / 2;

				double line_a, line_b, line_c;
				compute2ptLine(
					workpieceCorners.corner_2[1], workpieceCorners.center,
					&line_a, &line_b, &line_c);

				workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center);
				workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center);
				workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.corner_3[1], workpieceCorners.center);
				SVzNL3DPoint a_cross = computeEdgeCross((crossDir+180), polarPoints, line_a, line_b, line_c);
				workpieceCorners.len315_B2 = compute2DLen(a_cross, workpieceCorners.center);
			}
			else
			{
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_1[m] = branchInfo[0].corner[m];
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_2[m] = branchInfo[1].corner[m];
				SVzNL3DPoint center = computeLineCrossPt_abs(
					branchInfo[0].line_a, branchInfo[0].line_b, branchInfo[0].line_c,
					branchInfo[1].line_a, branchInfo[1].line_b, branchInfo[1].line_c);
				workpieceCorners.center.x = center.x;
				workpieceCorners.center.y = center.y;
				workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z) / 2;
				workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center);
				workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center);
				SVzNL3DPoint a_cross = computeEdgeCross(
					(branchInfo[0].angle + 180), polarPoints, 
					branchInfo[0].line_a, branchInfo[0].line_b, branchInfo[0].line_c);
				workpieceCorners.len225_A2 = compute2DLen(a_cross, workpieceCorners.center);
				a_cross = computeEdgeCross(
					(branchInfo[1].angle + 180), polarPoints,
					branchInfo[1].line_a, branchInfo[1].line_b, branchInfo[1].line_c);
				workpieceCorners.len315_B2 = compute2DLen(a_cross, workpieceCorners.center);
			}
		}
		else
		{
			workpieceType = 4;  //节点4
			partialScan = true;
			//检查没有在边界的Corner
			int polarPkNum = (int)validPolarRPeakPts.size();
			for (int pki = 0; pki < polarPkNum; pki++)
			{
				//检查是否在扫描边界
				int validStartLineWin = validStartLine + 3;
				int validEndLineWin = validEndLine - 3;
				if (validEndLineWin < 0)
					validEndLineWin = 0;
				bool isSide = false;
				int cptIdx = validPolarRPeakPts[pki].cptIndex;
				SVzNL3DPoint cpt = { validPolarRPeakPts[pki].x, validPolarRPeakPts[pki].y, validPolarRPeakPts[pki].z };
				std::vector<SVzNL3DPoint> edgePt_1;
				for (int i = 0; i < contourPtSize; i++)
				{
					int idx = (i + cptIdx)%contourPtSize;
					SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
					double len = sqrt(pow(cpt.x - a_pt.x, 2) + pow(cpt.y - a_pt.y, 2));
					if (len > workpieceParam.lineLen)
						break;
					else
					{
						if ( (partialScan == true) &&
							((polarPoints[idx].lineIdx <= validStartLineWin) ||
							(polarPoints[idx].lineIdx >= validEndLineWin)))
							isSide = true;
						else
							edgePt_1.push_back(a_pt);
					}
				}

				std::vector<SVzNL3DPoint> edgePt_2;
				for (int i = 0; i < contourPtSize; i++)
				{
					int idx = cptIdx - i;
					if (idx < 0)
						idx += contourPtSize;

					SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
					double len = sqrt(pow(cpt.x - a_pt.x, 2) + pow(cpt.y - a_pt.y, 2));
					if (len > workpieceParam.lineLen)
						break;
					else
					{
						if ((partialScan == true) &&
							((polarPoints[idx].lineIdx <= validStartLineWin) ||
							(polarPoints[idx].lineIdx >= validEndLineWin)))
							isSide = true;
						else
							edgePt_2.push_back(a_pt);
					}
				}
				if( (edgePt_1.size() < 10) || (edgePt_2.size() < 10))
				{
					*errCode = SX_ERR_ZERO_CONTOUR_PT;
					return workpieceCorners;
				}

				if (true == isSide)
				{
					//拟合计算交点，修正
					double edge1_a = 0, edge1_b = 0, edge1_c = 0;
					lineFitting_abc(edgePt_1, &edge1_a, &edge1_b, &edge1_c);
					double edge2_a = 0, edge2_b = 0, edge2_c = 0;
					lineFitting_abc(edgePt_2, &edge2_a, &edge2_b, &edge2_c);
					//计算交点
					SVzNL3DPoint crossPt = computeLineCrossPt_abs(
						edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c);
					validPolarRPeakPts[pki].x = crossPt.x;
					validPolarRPeakPts[pki].y = crossPt.y;
				}
			}
			if (polarPkNum != 8)
			{
				*errCode = SX_ERR_INVLID_RPEAK_NUM;
				return workpieceCorners;
			}
			//取互相垂直的两对
			//（1）取相邻corner距离最小的两段
			std::vector<double> sideLens;
			for (int i = 0; i < polarPkNum; i++)
			{
				int nxtIdx = (i + 1) % polarPkNum;
				double len = sqrt(pow(validPolarRPeakPts[i].x - validPolarRPeakPts[nxtIdx].x, 2) +
					pow(validPolarRPeakPts[i].y - validPolarRPeakPts[nxtIdx].y, 2));
				sideLens.push_back(len);
			}
			double minLen1 = -1;
			int minLen1_idx = -1;
			for (int i = 0; i < polarPkNum; i++)
			{
				if (minLen1 < 0)
				{
					minLen1 = sideLens[i];
					minLen1_idx = i;
				}
				else
				{
					if(minLen1 > sideLens[i])
					{
						minLen1 = sideLens[i];
						minLen1_idx = i;
					}
				}
			}
			double minLen2 = -1;
			int minLen2_idx = -1;
			for (int i = 0; i < polarPkNum; i++)
			{
				if (i == minLen1_idx)
					continue;

				if (minLen2 < 0)
				{
					minLen2 = sideLens[i];
					minLen2_idx = i;
				}
				else
				{
					if (minLen2 > sideLens[i])
					{
						minLen2 = sideLens[i];
						minLen2_idx = i;
					}
				}
			}
			if( (minLen1_idx < 0) || (minLen2_idx < 0))
			{
				*errCode = SX_ERR_INVLID_RPEAK_NUM;
				return workpieceCorners;
			}

			workpieceCorners.workpieceType = workpieceType;
			//计算工件信息
			int tmpIdx = (minLen1_idx + 1) % polarPkNum;
			workpieceCorners.corner_1[0] = { validPolarRPeakPts[minLen1_idx].x, validPolarRPeakPts[minLen1_idx].y, validPolarRPeakPts[minLen1_idx].z };
			workpieceCorners.corner_1[2] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
			workpieceCorners.corner_1[1].x = (workpieceCorners.corner_1[0].x + workpieceCorners.corner_1[2].x) / 2;
			workpieceCorners.corner_1[1].y = (workpieceCorners.corner_1[0].y + workpieceCorners.corner_1[2].y) / 2;
			workpieceCorners.corner_1[1].z = (workpieceCorners.corner_1[0].z + workpieceCorners.corner_1[2].z) / 2;
			//顺序的两个corner为下一段
			tmpIdx = (tmpIdx +1) % polarPkNum;
			int tmpIdx1 = (tmpIdx + 1) % polarPkNum;
			workpieceCorners.corner_2[0] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
			workpieceCorners.corner_2[2] = { validPolarRPeakPts[tmpIdx1].x, validPolarRPeakPts[tmpIdx1].y, validPolarRPeakPts[tmpIdx1].z };
			workpieceCorners.corner_2[1].x = (workpieceCorners.corner_2[0].x + workpieceCorners.corner_2[2].x) / 2;
			workpieceCorners.corner_2[1].y = (workpieceCorners.corner_2[0].y + workpieceCorners.corner_2[2].y) / 2;
			workpieceCorners.corner_2[1].z = (workpieceCorners.corner_2[0].z + workpieceCorners.corner_2[2].z) / 2;
			//第二个最短的两个corner为第3个corner
			tmpIdx = (minLen2_idx + 1) % polarPkNum;
			workpieceCorners.corner_3[0] = { validPolarRPeakPts[minLen2_idx].x, validPolarRPeakPts[minLen2_idx].y, validPolarRPeakPts[minLen2_idx].z };
			workpieceCorners.corner_3[2] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
			workpieceCorners.corner_3[1].x = (workpieceCorners.corner_3[0].x + workpieceCorners.corner_3[2].x) / 2;
			workpieceCorners.corner_3[1].y = (workpieceCorners.corner_3[0].y + workpieceCorners.corner_3[2].y) / 2;
			workpieceCorners.corner_3[1].z = (workpieceCorners.corner_3[0].z + workpieceCorners.corner_3[2].z) / 2;
			//顺序的两个corner为下一段
			tmpIdx = (tmpIdx + 1) % polarPkNum;
			tmpIdx1 = (tmpIdx + 1) % polarPkNum;
			workpieceCorners.corner_4[0] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
			workpieceCorners.corner_4[2] = { validPolarRPeakPts[tmpIdx1].x, validPolarRPeakPts[tmpIdx1].y, validPolarRPeakPts[tmpIdx1].z };
			workpieceCorners.corner_4[1].x = (workpieceCorners.corner_4[0].x + workpieceCorners.corner_4[2].x) / 2;
			workpieceCorners.corner_4[1].y = (workpieceCorners.corner_4[0].y + workpieceCorners.corner_4[2].y) / 2;
			workpieceCorners.corner_4[1].z = (workpieceCorners.corner_4[0].z + workpieceCorners.corner_4[2].z) / 2;
			//计算剩余信息
			double line1_a, line1_b, line1_c;
			compute2ptLine(
				workpieceCorners.corner_1[1], workpieceCorners.corner_3[1], 
				&line1_a, &line1_b, &line1_c);
			double line2_a, line2_b, line2_c;
			compute2ptLine(
				workpieceCorners.corner_2[1], workpieceCorners.corner_4[1], 
				&line2_a, &line2_b, &line2_c);
			workpieceCorners.center = computeLineCrossPt_abs(
				line1_a, line1_b, line1_c,
				line2_a, line2_b, line2_c);
			workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z +
				workpieceCorners.corner_3[1].z + workpieceCorners.corner_4[1].z) / 4;
			workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center);
			workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center);
			workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.corner_3[1], workpieceCorners.center);
			workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_4[1], workpieceCorners.center);
		}
	}

#if 1
	//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
	for (int i = 0; i < lineNum; i++)
		sx_BQ_lineDataR(scanLines_src[i], groundCalibPara.invRMatrix, -1);
	//将检测结果重新投射回原来的坐标系
	SVzNL3DPoint rawObj;
	for (int i = 0; i < 3; i++)
	{
		rawObj = _translatePoint(workpieceCorners.corner_1[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_1[i] = rawObj;
		rawObj = _translatePoint(workpieceCorners.corner_2[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_2[i] = rawObj;
		rawObj = _translatePoint(workpieceCorners.corner_3[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_3[i] = rawObj;
		rawObj = _translatePoint(workpieceCorners.corner_4[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_4[i] = rawObj;
	}
	rawObj = _translatePoint(workpieceCorners.center, groundCalibPara.invRMatrix);
	workpieceCorners.center = rawObj;
#endif

#if _OUTPUT_DEBUG_DATA
	for (int i = 0; i < (int)debug_contours.size(); i++)
	{
		SSX_debugInfo& a_branchDebug = debug_contours[i];
		for (int m = 0; m < a_branchDebug.edge_size; m++)
		{
			rawObj = _translatePoint(a_branchDebug.edge[m], groundCalibPara.invRMatrix);
			a_branchDebug.edge[m] = rawObj;
		}
		for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
		{
			rawObj = _translatePoint(a_branchDebug.edgeLink_1[m], groundCalibPara.invRMatrix);
			a_branchDebug.edgeLink_1[m] = rawObj;
		}
		for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
		{
			rawObj = _translatePoint(a_branchDebug.edgeLink_2[m], groundCalibPara.invRMatrix);
			a_branchDebug.edgeLink_2[m] = rawObj;
		}
	}
#endif
	return workpieceCorners;
}