workpieceHolePositioning version 1.5.0 :

修正工件Y方向矢量计算的一个Bug（不在工件表面平面内）

sourceCode/workpieceHolePositioning.cpp

@@ -20,7 +20,8 @@
 //version 1.4.7 : 修正问题：在进行异物检测时，计算ZSliceTh时。添加Z计算的保护，防止异常值混入
 //version 1.4.8 : 将工件法向调整为垂直于工件表面
 //version 1.4.9 : 修正工件法向计算的一个Bug
-std::string	m_strVersion = "1.4.9";
+//version 1.5.0 : 修正工件Y方向矢量计算的一个Bug（不在工件表面平面内）
+std::string	m_strVersion = "1.5.0";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -895,7 +896,7 @@ void wd_workpieceHolePositioning(
 		if (idx1 < 0)
 			continue;
 
-		SVzNLRangeD angleRange = { 85, 95 }; //垂直，5度范围
+		SVzNLRangeD angleRange = { 80, 100 }; //垂直，5度范围
 		SWD_HoleInfo& p1 = holes[idx1];
 		//搜索最接近distance且角度为angle的目标, 以角度为优先
 		int idx2 = angleConditionDistanceSearch(
@@ -965,6 +966,7 @@ void wd_workpieceHolePositioning(
 		//计算工件法向
 		SVzNL3DPoint normalDir;
 		SVzNL3DPoint vec_norm;
+		SVzNL3DPoint y_dir;
 		if ((int)(Points3ds.size() > size_1) && (size_1 > 0))
 		{
 			//计算面参数: z = Ax + By + C
@@ -975,11 +977,25 @@ void wd_workpieceHolePositioning(
 			vec_norm = vec3_normalize(vec_1);
 			vec_norm = vec3_multiply(vec_norm, 20.0);
 			normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z };
+
+			SVzNL3DPoint vector2 = { 0, 0, 1 };
+			SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2);
+			SVzNL3DPoint rotate_p0 = _ptRotate(p0.center, adjustPara.planeCalib);
+			SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib);
+			if (rotate_p0.x < rotate_p1.x)
+				y_dir = { rotate_p1.x - rotate_p0.x, rotate_p1.y - rotate_p0.y, 0 };
+			else
+				y_dir = { rotate_p0.x - rotate_p1.x, rotate_p0.y - rotate_p1.y, 0 };
+			y_dir = _ptRotate(y_dir, adjustPara.invRMatrix);
 		}
 		else
 		{
 			normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 };
 			vec_norm = { 0, 0, 20 };
+			if (p0.center.x < p1.center.x)
+				y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
+			else
+				y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
 		}
 
 		WD_workpieceInfo a_workpiece;
@@ -997,16 +1013,9 @@ void wd_workpieceHolePositioning(
 		}
 		a_workpiece.center = centerPoint;
 
-		SVzNL3DPoint y_dir;
+		y_dir = vec3_normalize(y_dir);
-		if (p0.center.x < p1.center.x)
+		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z  + y_dir.z * 20 };
-			y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
-		else
-			y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
-		double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2));
-		y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 };
-		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
 		a_workpiece.z_dir = normalDir;
-
 		allWorkpiece.push_back(a_workpiece);
 	}
 

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void)
 	};
 
 	SVzNLRange fileIdx[TPF_TEST_GROUP] = {
-		{6,6}, {7, 7}, {1,17}
+		{6,6}, {1, 16}, {18,18}
 	};
 
 	const char* ver = wd_workpieceHolePositioningVersion();
@@ -734,7 +734,7 @@ void TuoPuFa_holePosition_test(void)
 #endif
 
 #if TEST_COMPUTE_HOLE
-	for (int grp = 1; grp <= 1; grp++)
+	for (int grp = 2; grp <= 2; grp++)
 	{
 		SSG_planeCalibPara groundCalibPara;
 		//初始化成单位阵