algoLib/sourceCode/BQ_workpieceCornerExtraction.cpp

#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 : <20>㷨<EFBFBD><E3B7A8>ͬһbrach<63>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˾<EFBFBD><CBBE><EFBFBD><EFBFBD>ж<EFBFBD>
//version 1.4.0 : ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ս<EFBFBD><D5BD>ж<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//version 1.4.1 : (1)ʹ<>þ<EFBFBD><C3BE>෽<EFBFBD><E0B7BD>Ѱ<EFBFBD><D1B0>׼ȷ<D7BC><C8B7><EFBFBD><EFBFBD><EFBFBD>㣨2<E3A3A8><32>ʹ<EFBFBD>ùսǺ<D5BD>R<EFBFBD><52>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::string	m_strVersion = "1.4.1";
const char* wd_BQWorkpieceCornerVersion(void)
{
	return m_strVersion.c_str();
}

//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara sx_BQ_getBaseCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getPlaneCalibPara2(scanLines);
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}

//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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; //<2F>ߵĴ<DFB5><C4B4>߷<EFBFBD><DFB7><EFBFBD> ax+by+c = 0
}SWD_branchInfo;

//<2F><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>תʱ <20><> > 0 <20><>˳ʱ<CBB3><CAB1><EFBFBD><EFBFBD>תʱ <20><> < 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;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֧<EFBFBD><D6A7>Ϣ
int _getBranchInfo(
	int validStartLine,   //<2F><>ʼɨ<CABC><C9A8><EFBFBD>߽<EFBFBD>
	int validEndLine, //<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
	bool partialScan, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ұ<EFBFBD><D2B0><EFBFBD>ܸ<EFBFBD><DCB8>ǹ<EFBFBD><C7B9><EFBFBD>ȫ<EFBFBD><C8AB>ʱ<EFBFBD><CAB1>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)//<2F><>ʱ<EFBFBD><CAB1>
	{
		startIdx = branchCorner.cptIndex - cornerWin;  //<2F>˴<EFBFBD><CBB4><EFBFBD>corner<65><72>Χ<EFBFBD>ĵ㲻<C4B5><E3B2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		if (startIdx < 0)
			startIdx += contourPtSize;
		LineDir = -1;
		contourDir = 1;
		endingIdx = branchCornerInfo.L1_ptIndex;
	}
	else  //˳ʱ<CBB3><CAB1>
	{
		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;

	//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽类<DFBD>г<EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ϱߺ<CFB1>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ<C4BD><C7B5><EFBFBD><EFBFBD>ܲ<EFBFBD><DCB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ㣬<C7B5><E3A3AC>Ҫ<EFBFBD><D2AA><EFBFBD>м<EFBFBD><D0BC><EFBFBD>
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ㣺ȡcorner<65><72><EFBFBD><EFBFBD>5<EFBFBD><35><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>б߽磬<DFBD><E7A3AC><EFBFBD><EFBFBD>
	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);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ<C4BD>
		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;
			}
		}
		//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
		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);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ<C4BD>
		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)//<2F><>ʱ<EFBFBD><CAB1>
		{
			startIdx = branchCorner.cptIndex - cornerWin;  //<2F>˴<EFBFBD><CBB4><EFBFBD>corner<65><72>Χ<EFBFBD>ĵ㲻<C4B5><E3B2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			if (startIdx < 0)
				startIdx += contourPtSize;
		}
		else  //˳ʱ<CBB3><CAB1>
			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;
	}

	//<2F><><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>: ax+by+c = 0
	double _a = 0, _b = 0, _c = 0;
	lineFitting_abc(LinePts, &_a, &_b, &_c);
	SVzNL3DPoint endingPt0 = LinePts.back();
	//<2F><><EFBFBD><EFBFBD><EFBFBD>˵㴹<CBB5><E3B4B9>
	SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c);
	//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	double minDist = -1;
	for (int i = cornerWin; i < contourPtSize; i++)  //<2F>ܿ<EFBFBD><DCBF>ǵ<EFBFBD><C7B5><EFBFBD>Χ
	{
		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);
	}
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>б߽<D0B1><DFBD>㡣<EFBFBD><E3A1A3><EFBFBD><EFBFBD><EFBFBD>б߽<D0B1><DFBD>㣬<EFBFBD><E3A3AC>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	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;
		}
	}
	//<2F>ֿ<EFBFBD>branch<63><68><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	if (false == hasSidePt)
	{
		double maxDist = -1;
		int maxPos = -1;
		//<2F>ҵ<EFBFBD><D2B5>յ<EFBFBD>:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>ľ<EFBFBD><C4BE>룬<EFBFBD><EBA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>Ϊ<EFBFBD>յ<EFBFBD>
		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;
				}
			}
		}
		//<2F>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>
		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);
		}

		//<2F><><EFBFBD><EFBFBD><EFBFBD>󽻵<EFBFBD>
		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);

		//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
		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
	{
		//<2F><><EFBFBD>߽<EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD>ʣ<EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD>ν<EFBFBD><CEBD>д<EFBFBD><D0B4><EFBFBD>
		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);
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>ϵĴ<CFB5><C4B4>㣬ȡ<E3A3AC><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ġ<EFBFBD><C4A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵĴ<C4B5><C4B4><EFBFBD>
		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;
		//<2F><><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>ϵĴ<CFB5><C4B4>㣬ȡ<E3A3AC><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ġ<EFBFBD><C4A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵĴ<C4B5><C4B4><EFBFBD>
		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;
		//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
		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)  //<2F><><EFBFBD><EFBFBD>60<36>ȷ<EFBFBD>Χ
		{
			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);
	//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
	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;

}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD>β<EFBFBD><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>XOYƽ<59><C6BD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>Ǻͺ<C7BA><CDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC>սǡ<D5BD> ǰ<><C7B0><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD>+<2B><> <20><><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD>-
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];
		//ǰ<><C7B0>Ѱ<EFBFBD><D1B0>
		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;
			}
		}
		//<2F><><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0>
		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;
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD>ս<EFBFBD>
		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;
			//<2F><><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>ڼ<EFBFBD><DABC>Ǻͷ<C7BA><CDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD>corner<65><72><EFBFBD><EFBFBD>
			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))  //<2F><><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>뼫<EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>µIJ<C2B5><C4B2><EFBFBD>
			{
				if (dirCornerAngles[i].corner > 160)  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȴ<C7B6><C8B4><EFBFBD>
				{
					dirCornerAngles[i].flag = -1;
				}
			}
			
			dirCornerAngles[i].corner = corner;  //ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵy<CFB5><79><EFBFBD><EFBFBD><EFBFBD>෴<EFBFBD><E0B7B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>-<2D><><EFBFBD><EFBFBD>
			dirCornerAngles[i].pntIdx = i;
			dirCornerAngles[i].forward_pntIdx = plus_i;
			dirCornerAngles[i].backward_pntIdx = minus_i;
			dirCornerAngles[i].point = polarPoints[i];
		}
	}
}

//<2F><>ȡcorner<65><72>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD>ֺ<EFBFBD><D6BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD>ô˺<C3B4><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD>
void _searchPlusCornerPeaks(
	std::vector< SSG_dirCornerAngle>& corners,
	double cutAngleTh,
	std::vector< SSG_dirCornerAngle>& cornerPlusPeaks
)
{
	std::vector<SSG_dirCornerAngle> peakCorners;
	int cornerSize = (int)corners.size();
	//<2F><><EFBFBD><EFBFBD><EFBFBD>սǼ<D5BD>ֵ
	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:  //<2F><>̬
			if (cornerDiff < 0) //<2F>½<EFBFBD>
			{
				_state = 2;
			}
			else if (cornerDiff > 0) //<2F><><EFBFBD><EFBFBD>
			{
				_state = 1;
			}
			break;
		case 1: //<2F><><EFBFBD><EFBFBD>
			if (cornerDiff < 0)   //<2F>½<EFBFBD>
			{
				if (pre_data->corner > cutAngleTh)  //<2F>ؽ<EFBFBD><D8BD><EFBFBD><EFBFBD>ޣ<EFBFBD><DEA3>˳<EFBFBD><CBB3>㲨<EFBFBD><E3B2A8><EFBFBD><EFBFBD><EFBFBD>µĽǶȱ仯
					peakCorners.push_back(*pre_data);
				_state = 2;
			}
			break;
		case 2: //<2F>½<EFBFBD>
			if (cornerDiff > 0)   //  <20><><EFBFBD><EFBFBD>
				_state = 1;
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//ע<>⣺<EFBFBD><E2A3BA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>

	//<2F><>Сֵ<D0A1>㣨<EFBFBD>嶥<EFBFBD><E5B6A5>
	//<2F><>ֵ<EFBFBD>Ƚϣ<C8BD><CFA3>ڳ߶ȴ<DFB6><C8B4><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD>Ҿֲ<D2BE><D6B2><EFBFBD>ֵ<EFBFBD><D6B5>
	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<75><73><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0>
		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<75><73><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0>
		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))  //<2F><><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>뼫<EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>µIJ<C2B5><C4B2><EFBFBD>
				{
					if (a_dirAngle.corner < 160)  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȴ<C7B6><C8B4><EFBFBD>
					{
						a_dirAngle.flag = 0;
						cornerPlusPeaks.push_back(a_dirAngle);
					}
				}
				else
				{
					a_dirAngle.flag = 0;
					cornerPlusPeaks.push_back(a_dirAngle);
				}
			}
		}
	}
}

//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
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)  //<2F>޷<EFBFBD><DEB7>ܿ<EFBFBD>һ<EFBFBD><D2BB><EFBFBD>߶<EFBFBD>
	{
		idx1 = pt1.pntIdx; 
		idx2 = pt2.pntIdx; 
	}
	else
	{
		idx1 = pt1.forward_pntIdx;  //<2F>ܿ<EFBFBD>һ<EFBFBD><D2BB><EFBFBD>߶<EFBFBD>
		idx2 = pt2.backward_pntIdx; //<2F>ܿ<EFBFBD>һ<EFBFBD><D2BB><EFBFBD>߶<EFBFBD>
	}
	//ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	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)  //<2F>ж<EFBFBD>Ϊֱ<CEAA><D6B1>
		return true;
	else
		return false;
}

//<2F><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ㣬<D5B5><E3A3AC><EFBFBD><EFBFBD>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();
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>
	// ֱ<><D6B1>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>
	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);
	//ֱ<><D6B1>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);
	//ֱ<><D6B1>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());
	//<2F><><EFBFBD><EFBFBD>branch<63><68>Ϣ
	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;
}

//<2F>нDz<D0BD>ȫ<EFBFBD><C8AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB><EFBFBD>Ĺսǵ<D5BD>
SVzNL3DPoint _fittingCutAngle(
	SSG_dirCornerAngle& corner_1, SSG_dirCornerAngle& corner_2,
	std::vector<SWD_polarPt>& polarPoints,
	const SSX_BQworkpiecePara& workpieceParam
)
{
	int ringBuffSize = (int)polarPoints.size();
	//<2F><><EFBFBD>ϼ<EFBFBD><CFBC>㽻<EFBFBD><E3BDBB>
	//ֱ<><D6B1>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);
	//ֱ<><D6B1>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);
	//<2F>󽻵<EFBFBD>
	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;
}

//<2F><>curr_corner<65><72><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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();
	//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD>branch<63><68>ǰһcorner<65><72>forward<72><64><EFBFBD><EFBFBD>һcorner<65><72>backward<72><64><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĹսǺ<D5BD><C7BA><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	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))
	{
		//ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
		//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
		double maxDeviation = 0;
		bool isLine = checkIsLine(corner_1, corner_2, polarPoints, cutLineDeviationTh, &maxDeviation);
		if (true == isLine)  //<2F>ж<EFBFBD>Ϊ<EFBFBD>ؽ<EFBFBD>
		{
			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;
			//<2F><><EFBFBD><EFBFBD>corner
			corner_1.corner = corner_1.corner + corner_2.corner;
			corner_1.forward_pntIdx = corner_2.forward_pntIdx;

			//<2F><><EFBFBD><EFBFBD>branchInfo
			//<2F><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ㣬<D5B5><E3A3AC><EFBFBD><EFBFBD>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)//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD>branch<63><68><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ںܽ<DABA><DCBD><EFBFBD>ȱ<EFBFBD><C8B1>һ<EFBFBD><D2BB>corner<65><72>
	{
		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))
		{
			//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
			double maxDeviation = 0;
			bool isLine = checkIsLine( curr_corner, corner_1, polarPoints, cutLineDeviationTh*2, &maxDeviation);
			if (true == isLine)  //<2F>ж<EFBFBD>Ϊ<EFBFBD>ؽ<EFBFBD>
			{
				//ֱ<>Ӽ<EFBFBD><D3BC><EFBFBD>brachInfo<66><6F><EFBFBD><EFBFBD>curr_corner<65><72>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>׼
				// ֱ<><D6B1>1<EFBFBD><31><EFBFBD><EFBFBD>ӦcurrCorner<65><72>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);
				//ֱ<><D6B1>2: <20><>ӦcurrCorner<65><72>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);
				//ֱ<><D6B1>1<EFBFBD><31> <20><>curr_corner<65>Ĺյ<C4B9><D5B5><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>0<EFBFBD><30>ֱ
				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
				//<2F><><EFBFBD><EFBFBD>branch<63><68>Ϣ
				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;
}

//<2F><>curr_corner<65>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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();
	//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD>branch<63><68>ǰһcorner<65><72>forward<72><64><EFBFBD><EFBFBD>һcorner<65><72>backward<72><64><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĹսǺ<D5BD><C7BA><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	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))
	{
		//ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
		//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
		double maxDeviation = 0;
		bool isLine = checkIsLine(corner_2, corner_1, polarPoints, cutLineDeviationTh, &maxDeviation);
		if (true == isLine)  //<2F>ж<EFBFBD>Ϊ<EFBFBD>ؽ<EFBFBD>
		{
			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;
			//<2F><><EFBFBD><EFBFBD>corner
			corner_1.corner = corner_1.corner + corner_2.corner;
			corner_1.backward_pntIdx = corner_2.backward_pntIdx;

			//<2F><><EFBFBD><EFBFBD>branchInfo
			//<2F><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ㣬<D5B5><E3A3AC><EFBFBD><EFBFBD>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)//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD>branch<63><68><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ںܽ<DABA><DCBD><EFBFBD>ȱ<EFBFBD><C8B1>һ<EFBFBD><D2BB>corner<65><72>
	{
		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))
		{
			//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
			double maxDeviation = 0;
			bool isLine = checkIsLine(corner_1, curr_corner, polarPoints, cutLineDeviationTh * 2, &maxDeviation);
			if (true == isLine)  //<2F>ж<EFBFBD>Ϊ<EFBFBD>ؽ<EFBFBD>
			{
				//ֱ<>Ӽ<EFBFBD><D3BC><EFBFBD>brachInfo<66><6F><EFBFBD><EFBFBD>curr_corner<65><72>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>׼
				// ֱ<><D6B1>1<EFBFBD><31><EFBFBD><EFBFBD>ӦcurrCorner<65><72>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);
				//ֱ<><D6B1>2: <20><>ӦcurrCorner<65><72>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);
				//ֱ<><D6B1>1<EFBFBD><31> <20><>curr_corner<65>Ĺյ<C4B9><D5B5><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>0<EFBFBD><30>ֱ
				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
				//<2F><><EFBFBD><EFBFBD>branch<63><68>Ϣ
				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;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ<C4BD><C7B5><EFBFBD><EFBFBD><EFBFBD>Branch<63><68> <20><><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ȱ<EFBFBD>ǡ<EFBFBD>ȱ<EFBFBD><C8B1>ʱͨ<CAB1><CDA8><EFBFBD><EFBFBD><EFBFBD>Ϸ<EFBFBD>ʽ<EFBFBD><CABD>ȫ
// std::vector<SWD_polarPt>& polarPoints: <20><><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// polarRPeakInfo: R<><52>ֵ<EFBFBD><D6B5>
// cornerPlusPeaks<6B><73> corner<65><72>ֵ<EFBFBD>㡣<EFBFBD><E3A1A3>R<EFBFBD><52>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>corner<65><72>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB>ʱ<EFBFBD><CAB1>Ϊ<EFBFBD><CEAA>Ч<EFBFBD>Ĺյ<C4B9>
//branchCornerAngle: ֧<><D6A7>Ѱ<EFBFBD>Ҳ<EFBFBD>ͬ<EFBFBD>Ƕȵ<C7B6>Branch<63><68><EFBFBD><EFBFBD>branchCornerAngleָ<65><D6B8><EFBFBD><EFBFBD>90<39><30>Ϊֱ<CEAA>ǡ<EFBFBD>
//angleTh: <20>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>Χ<EEB7B6><CEA7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ90<39><30>ֱ<EFBFBD>ǣ<EFBFBD>ʵ<EFBFBD>ʼӹ<CABC><D3B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//cutLineDeviationTh:<3A>Խؽ<D4BD><D8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߣ<EFBFBD><DFA3>ؽ<EFBFBD><D8BD>ϵ㵽ֱ<E3B5BD>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD>롣<EFBFBD><EBA1A3>֤<EFBFBD>ؽ<EFBFBD><D8BD><EFBFBD>ֱ<EFBFBD><D6B1>
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();

	//<2F><>R<EFBFBD><52>ֵΪ<D6B5><CEAA><EFBFBD><EFBFBD><EFBFBD>жϹյ<CFB9><D5B5>Ƿ<EFBFBD><C7B7><EFBFBD>ȱ<EFBFBD><C8B1>
	//<2F><><EFBFBD>Ƚ<EFBFBD>R<EFBFBD><52>ֵ<EFBFBD><D6B5>Corner<65><72>ֵ<EFBFBD><D6B5>Ӧ<EFBFBD>ļ<EFBFBD>ֵ<EFBFBD><D6B5>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>Ϊ<EFBFBD>Ŷ<EFBFBD><C5B6><EFBFBD><EFBFBD>ߵĹյ<C4B9>
	std::vector<SSG_intPair> realCorners;
	double samePosDistTh = 20.0;  //R<><52>ֵ<EFBFBD><D6B5>corner<65><72>ֵλ<D6B5><CEBB>ͬһ<CDAC>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	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;

			//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>롣<EFBFBD><EBA1A3><EFBFBD><EFBFBD>С<EFBFBD><D0A1>5mm<6D><6D><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD>
			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);
		}
	}

	//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>н<EFBFBD>
	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];
		//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>н<EFBFBD>
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>corner<65>ȶԣ<C8B6><D4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>Ϊ<EFBFBD>н<EFBFBD>
		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))
		{
			//ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
			//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
			double maxDeviation = 0;
			bool isLine = checkIsLine(corner_1, curr_corner, polarPoints, cutLineDeviationTh, &maxDeviation);
			if (true == isLine)  //<2F>ж<EFBFBD>Ϊ<EFBFBD>ؽ<EFBFBD>
			{
				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;
				//<2F><><EFBFBD><EFBFBD>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))
		{
			//ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
			//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
			double maxDeviation = 0;
			bool isLine = checkIsLine(curr_corner, corner_2, polarPoints, cutLineDeviationTh, &maxDeviation);
			if (true == isLine)  //<2F>ж<EFBFBD>Ϊ<EFBFBD>ؽ<EFBFBD>
			{
				SVzNL3DPoint cutCornerPt = _fittingCutAngle(curr_corner, corner_2, polarPoints, workpieceParam);
				//<2F><><EFBFBD>ϼ<EFBFBD><CFBC>㽻<EFBFBD><E3BDBB>
				curr_corner.point.x = cutCornerPt.x;
				curr_corner.point.y = cutCornerPt.y;
				curr_corner.point.z = cutCornerPt.z;
				corner_2.pntIdx = -1;
				//<2F><><EFBFBD><EFBFBD>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;
			}
		}
	}

	//<2F><>û<EFBFBD>ж<EFBFBD><D0B6>ϵ<EFBFBD>R<EFBFBD><52>ֵ<EFBFBD><D6B5><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52>ֵΪ<D6B5><CEAA>׼<EFBFBD><D7BC>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>cornerΪ׼
	int size_peaks = (int)polarRPeakInfo.size();
	for (int i = 0; i < size_peaks; i++)
	{
		if (polarRPeakInfo[i].flag > 0)
			continue;

		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>corner
		int nearestCorner = _searchClosestCorner(polarRPeakInfo[i].cptIndex, cornerPlusPeaks);
		if (nearestCorner < 0)
			continue;

		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD>н<EFBFBD>
		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))
		{
			//ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
			//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
			double maxDeviation = 0;
			bool isLine = checkIsLine(corner_1, curr_corner, polarPoints, cutLineDeviationTh, &maxDeviation);
			if (true == isLine)  //<2F>ж<EFBFBD>Ϊ<EFBFBD>ؽ<EFBFBD>
			{
				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;
				//<2F><><EFBFBD><EFBFBD>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))
		{
			//ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
			//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
			double maxDeviation = 0;
			bool isLine = checkIsLine(curr_corner, corner_2, polarPoints, cutLineDeviationTh, &maxDeviation);
			if (true == isLine)  //<2F>ж<EFBFBD>Ϊ<EFBFBD>ؽ<EFBFBD>
			{
				SVzNL3DPoint cutCornerPt = _fittingCutAngle(curr_corner, corner_2, polarPoints, workpieceParam);
				//<2F><><EFBFBD>ϼ<EFBFBD><CFBC>㽻<EFBFBD><E3BDBB>
				curr_corner.point.x = cutCornerPt.x;
				curr_corner.point.y = cutCornerPt.y;
				curr_corner.point.z = cutCornerPt.z;
				corner_2.pntIdx = -1;
				//<2F><><EFBFBD><EFBFBD>corner
				curr_corner.corner = sumCorner_2;
				curr_corner.forward_pntIdx = corner_2.forward_pntIdx;
				curr_corner.flag = 1;
			}
		}
	}

#if 0
	//<2F><><EFBFBD><EFBFBD>branch<63><68><EFBFBD><EFBFBD>,
	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];
		//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
		bool isBranch = checkIsLine(corner_1, corner_2, polarPoints, cutLineDeviationTh);
		if (true == isBranch)  //<2F>ж<EFBFBD>Ϊbranch
		{
			//<2F><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ㣬<D5B5><E3A3AC><EFBFBD><EFBFBD>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;
		}
	}

	//<2F><>ʣ<EFBFBD>µ<EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>ԵĹյ<C4B9><D5B5><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>


	//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD>ȱ<EFBFBD><C8B1>
	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)
		{
			//ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
			//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
			bool isLine = checkIsLine(	corner_1, corner_2, polarPoints, cutLineDeviationTh);
			if (true == isLine)  //<2F>ж<EFBFBD>Ϊ<EFBFBD>ؽ<EFBFBD>
			{
				//<2F><><EFBFBD>ϼ<EFBFBD><CFBC>㽻<EFBFBD><E3BDBB>
				//ֱ<><D6B1>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);
				//ֱ<><D6B1>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);
				//<2F>󽻵<EFBFBD>
				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;
				//<2F><><EFBFBD><EFBFBD>corner
				corner_1.corner = sumCorner;
				corner_1.forward_pntIdx = corner_2.forward_pntIdx;
			}
		}
	}

	//<2F>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Corner, <20><>ɾ<EFBFBD><C9BE>
	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
	//Ѱ<><D1B0>branch: branch<63><68><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE>Ϊֱ<CEAA><D6B1>
	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];
		//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>һ<EFBFBD><D2BB>ֱ<EFBFBD><D6B1>
		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)  //<2F>ж<EFBFBD>Ϊbranch
		{
			//<2F><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ㣬<D5B5><E3A3AC><EFBFBD><EFBFBD>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;
		}
	}
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD>γ<EFBFBD>branchInfo<66>ļ<EFBFBD>ֵ<EFBFBD><D6B5>
	//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD>R<EFBFBD><52>ֵ<EFBFBD>㣬<EFBFBD>Լ<EFBFBD>ֵ<EFBFBD><D6B5>Ϊ<EFBFBD><CEAA>׼<EFBFBD><D7BC>Ѱ<EFBFBD><D1B0><EFBFBD><EFBFBD><EFBFBD>ŵ<EFBFBD>ƽ<EFBFBD>бߣ<D0B1><DFA3><EFBFBD><EFBFBD><EFBFBD>corner<65>ĺ<EFBFBD><C4BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>෴ֵ<E0B7B4><D6B5><EFBFBD>ȣ<EFBFBD>
	for (int i = 0; i < validPeak_size; i++)
	{
		int cornerIdx = validCornerPeaks[i];
		if (cornerPlusPeaks[cornerIdx].flag == 0)
			continue;

		SSG_dirCornerAngle& curr_corner = cornerPlusPeaks[cornerIdx];
		//˳<><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		int tmpIdx = (cornerIdx + 1)% size_peaks;
		SSG_dirCornerAngle corner_1 = cornerPlusPeaks[tmpIdx];
		tmpIdx = (cornerIdx + 2) % size_peaks;
		SSG_dirCornerAngle corner_2 = cornerPlusPeaks[tmpIdx];
		//<2F><><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD>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
		{
			//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			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];
			//<2F><><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD>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
			}
		}
	}

	//<2F><><EFBFBD><EFBFBD>
	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;
	}

	//<2F><><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ף<EFBFBD>,<2C><>С<EFBFBD><D0A1>Ե<EFBFBD>ϵ<EFBFBD><CFB5>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>µĽǶȼ<C7B6><C8BC>㲨<EFBFBD><E3B2A8>
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	int fillingHoleSize = 3;//<2F><><EFBFBD>״<EFBFBD>С:С<><D0A1>3<EFBFBD><33><EFBFBD>Ŀ׶<C4BF>
	wd_pointCloudHoleFilling(scanLines_src, scanLines, fillingHoleSize);  //<2F><><EFBFBD>״<EFBFBD>С

	//<2F><><EFBFBD><EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>Ŀհ<C4BF>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
	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;

	//<2F><><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕ<EFBFBD>
	{
		std::vector<SVzNL3DPosition> featurePoints;
		//<2F><><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB>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);
		}

		//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
		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;

			//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
			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ͨ<65><CDA8>ȡbagH<67><48>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)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
		{
			*errCode = SG_ERR_NOT_GRID_FORMAT;
			return workpieceCorners;
		}

		//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
		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; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
				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<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
		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];
			//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
			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ͨ<65><CDA8>ȡbagH<67><48>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
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>
		//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD>
		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);
		}

		//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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];
			//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
			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];
			//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
			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
		//<2F><><EFBFBD>࣬<EFBFBD><E0A3AC>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		int clusterCheckWin = 8;  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			clusterDist,
			objClusters  //result
		);
		//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
		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
		//<2F><><EFBFBD>㼸<EFBFBD><E3BCB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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  //<2F><><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD>
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD>ڽ<EFBFBD><DABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD>
		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  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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
		//<2F><><EFBFBD>㼫<EFBFBD><E3BCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52>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;
		}
		//<2F><><EFBFBD>Ƕȴ<C7B6>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
		std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
		for (int pi = 0; pi < contourPtSize; pi++)
			polarPoints[pi].cptIndex = pi;  // index
		
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><D4B5><EFBFBD>ĽǶȱ仯 2026.03.06
		std::vector< SSG_dirCornerAngle> dirCornerAngles;
		_computeClosedPntListDirCorners(polarPoints, workpieceParam.dirAngleScale, dirCornerAngles);
		//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>ǹյ<C7B9>(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
		std::vector< SSG_dirCornerAngle> cornerPlusPeaks;
		_searchPlusCornerPeaks(dirCornerAngles, workpieceParam.minCutAngleTh, cornerPlusPeaks);

		//<2F><>ȡR<C8A1><52>ֵ<EFBFBD><D6B5>
		double minR = -1, maxR = -1; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>з<EFBFBD>֧<EFBFBD><D6A7>minR<6E><52>maxR<78><52><EFBFBD><EFBFBD>Сʱ<D0A1><CAB1>ΪԲ<CEAA>λ<EFBFBD>8<EFBFBD><38><EFBFBD>Σ<EFBFBD>û<EFBFBD>з<EFBFBD>֧
		int minRPos = -1;
		std::vector<SWD_polarPt> polarRPeakPts;
		int winSize = contourPtSize / 36;  //+-10<31>ȷ<EFBFBD>Χ
		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;
		//<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>εļ<CEB5>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD>ƣ<EFBFBD>Բ<EFBFBD><D4B2><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>γɼ<CEB3>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>L=ֱ<>߶γ<DFB6><CEB3>ȹ<EFBFBD><C8B9>ɵ<EFBFBD><C9B5>Ž<EFBFBD><C5BD>ж<EFBFBD>
		double arcAngleChkLen = 100; //<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>Žǵij߶<C4B3>
		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) //<2F><><EFBFBD><EFBFBD>50mm<6D><6D><EFBFBD>ж<EFBFBD>Ϊ<EFBFBD><CEAA>Ч
				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);
				}
			}
		}

		//<2F><><EFBFBD><EFBFBD>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; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ұ<EFBFBD><D2B0><EFBFBD>ܸ<EFBFBD><DCB8>ǹ<EFBFBD><C7B9><EFBFBD>ȫ<EFBFBD><C8AB>ʱ<EFBFBD><CAB1>partialScanΪtrue
		std::vector< SWD_branchInfo> branchInfo;
		if (branches.size() > 0)  //<2F>з<EFBFBD>֧<EFBFBD><D6A7>branch)
		{
			int branchNum = (int)branches.size();
			if (branchNum == 2)
				workpieceType = 3;  //<2F>ڵ<EFBFBD>3
			else if (branchNum == 3)
				workpieceType = 2;  //<2F>ڵ<EFBFBD>2
			else if (branchNum == 4)
				workpieceType = 1;  //<2F>ڵ<EFBFBD>1
			else
				workpieceType = 0;
			branchInfo.insert(branchInfo.end(), branches.begin(), branches.end());
		
			workpieceCorners.workpieceType = workpieceType;
			if (workpieceType == 1) //4<><34>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];
				//<2F><><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>Ϣ
				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<65><31>ת45<34>ȷ<EFBFBD><C8B7><EFBFBD>
				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 <20><>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];

				//<2F><><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>Ϣ
				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;  //<2F>ڵ<EFBFBD>4
			partialScan = true;
			//<2F><><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD>ڱ߽<DAB1><DFBD><EFBFBD>Corner
			int polarPkNum = (int)validPolarRPeakPts.size();
			for (int pki = 0; pki < polarPkNum; pki++)
			{
				//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
				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)
				{
					//<2F><><EFBFBD>ϼ<EFBFBD><CFBC>㽻<EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD>
					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);
					//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
					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;
			}
			//ȡ<><C8A1><EFBFBD>ഹֱ<E0B4B9><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			//<2F><>1<EFBFBD><31>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>corner<65><72><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			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;
			//<2F><><EFBFBD>㹤<EFBFBD><E3B9A4><EFBFBD><EFBFBD>Ϣ
			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;
			//˳<><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>cornerΪ<72><CEAA>һ<EFBFBD><D2BB>
			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;
			//<2F>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̵<EFBFBD><CCB5><EFBFBD><EFBFBD><EFBFBD>cornerΪ<72><CEAA>3<EFBFBD><33>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;
			//˳<><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>cornerΪ<72><CEAA>һ<EFBFBD><D2BB>
			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;
			//<2F><><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>Ϣ
			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
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۱궨<DBB1><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
	for (int i = 0; i < lineNum; i++)
		sx_BQ_lineDataR(scanLines_src[i], groundCalibPara.invRMatrix, -1);
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
	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;
}