From 2757c09682312bb9ae521b99007c6de1e473c3ca Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 27 Apr 2026 14:09:06 +0800 Subject: [PATCH] =?UTF-8?q?workpieceHolePositioning=20version=201.5.0=20:?= =?UTF-8?q?=20=E4=BF=AE=E6=AD=A3=E5=B7=A5=E4=BB=B6Y=E6=96=B9=E5=90=91?= =?UTF-8?q?=E7=9F=A2=E9=87=8F=E8=AE=A1=E7=AE=97=E7=9A=84=E4=B8=80=E4=B8=AA?= =?UTF-8?q?Bug=EF=BC=88=E4=B8=8D=E5=9C=A8=E5=B7=A5=E4=BB=B6=E8=A1=A8?= =?UTF-8?q?=E9=9D=A2=E5=B9=B3=E9=9D=A2=E5=86=85=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/workpieceHolePositioning.cpp | 31 ++++++++++++------- .../workpieceHolePositioning_test.cpp | 4 +-- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index ddb7f7e..c558b84 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/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; - 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 }; - 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 }; + y_dir = vec3_normalize(y_dir); + 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 }; a_workpiece.z_dir = normalDir; - allWorkpiece.push_back(a_workpiece); } diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index 180b865..efa24f2 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/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; //初始化成单位阵