diff --git a/primitive_process/interface/subface/geometry/polyline_fillet.hpp b/primitive_process/interface/subface/geometry/polyline_fillet.hpp index 44eba28..47fadd2 100644 --- a/primitive_process/interface/subface/geometry/polyline_fillet.hpp +++ b/primitive_process/interface/subface/geometry/polyline_fillet.hpp @@ -19,4 +19,14 @@ void insert_polyline_corner_fillets(const polyline_descriptor_t& profile, std::vector& out_bulges, const fillet_config_t& cfg = {}); +/** + * @brief 检测并修复 polyline 中负 bulge 圆弧与相邻边的自相交。 + * 将自相交段的 bulge 置 0（退化为直线），消除 PMC 歧义区域。 + */ +void sanitize_negative_bulge_self_intersections(const std::vector& pts_3d, + std::vector& bulges, + bool is_axis, + bool closed); + + } // namespace internal \ No newline at end of file diff --git a/primitive_process/src/subface/geometry/polyline_fillet.cpp b/primitive_process/src/subface/geometry/polyline_fillet.cpp index c3db227..201fdcd 100644 --- a/primitive_process/src/subface/geometry/polyline_fillet.cpp +++ b/primitive_process/src/subface/geometry/polyline_fillet.cpp @@ -39,7 +39,6 @@ Eigen::Vector2d arc_start_tangent(Eigen::Vector2d A, Eigen::Vector2d B, double b return rotate2d(d, -sign * theta / 2.0); } - struct Corner { bool active = false; Eigen::Vector2d trim_in; // 圆弧起点 @@ -97,7 +96,6 @@ Corner compute_corner(const std::vector& pts, c.trim_in = pts[i] - trim * T_in; c.trim_out = pts[i] + trim * T_out; - // bulge 符号：cross>0 → 左转(CCW多边形凸角) → CW弧 → 负 bulge double fillet_bulge = std::tan(turn_angle / 4.0); const double cross = T_in.x() * T_out.y() - T_in.y() * T_out.x(); if (cross < 0.0) fillet_bulge = -fillet_bulge; @@ -153,11 +151,19 @@ static double recompute_subarc_bulge(const Eigen::Vector2d& A, // 规范化：子弧行进方向与原弧一致（CCW 为正，CW 为负） if (sign_b > 0.0) { - if (delta < 0.0) delta += 2.0 * pi; - if (delta > 2.0 * pi) delta -= 2.0 * pi; + // 强制 delta 落入 (0, 2π] + delta = std::fmod(delta, 2.0 * pi); + if (delta <= 0.0) delta += 2.0 * pi; } else { - if (delta > 0.0) delta -= 2.0 * pi; - if (delta < -2.0 * pi) delta += 2.0 * pi; + // 强制 delta 落入 [-2π, 0) + delta = std::fmod(delta, 2.0 * pi); + if (delta >= 0.0) delta -= 2.0 * pi; + } + + double result = std::tan(delta / 4.0); + if ((b > 0) != (result > 0) && std::abs(result) > 1e-9) { + std::cerr << "[BULGE_SIGN_FLIP] original_b=" << b << " recomputed_bulge=" << result << " delta=" << delta + << " ang_A=" << ang_A << " ang_B=" << ang_B << "\n"; } return std::tan(delta / 4.0); @@ -173,8 +179,114 @@ inline void push_point(std::vector& out_points, out_bulges.push_back(bulge); } +/** + * @brief 计算点 p 到线段 [a, b] 的最短距离（2D）。 + * @details 将参数 t 限制在 [0,1] 以保证只测量到线段本身，而非无限延长线， + * 从而避免把圆弧圆心到相邻线段延长线的误距离当作相交依据。 + */ +static double point_to_segment_dist_2d(const Eigen::Vector2d& p, const Eigen::Vector2d& a, const Eigen::Vector2d& b) +{ + const Eigen::Vector2d ab = b - a; + const double len2 = ab.squaredNorm(); + if (len2 < 1e-24) return (p - a).norm(); + const double t = std::clamp((p - a).dot(ab) / len2, 0.0, 1.0); + return (p - (a + t * ab)).norm(); +} + } // anonymous namespace +/** + * @brief 检测并修复 polyline 中"负 bulge 圆弧与相邻边发生自相交"的问题。 + * + * @details + * 对于负 bulge（CW 弧，在 CCW profile 中表现为内凹弧），若其圆心到相邻线段的 + * 距离 < 圆弧半径 R，则该弧与相邻边相交。自相交区域内，PMC 的最近点查询会产生 + * 歧义，导致 SDF 符号局部翻转，进而在错误位置生成多余等值面（即图中异常轮廓）。 + * + * 修复策略：将发生自相交的段的 bulge 置 0，退化为直线段，并输出诊断日志。 + * + * 几何原理（以 CW 弧为例）： + * 弦 AB，半角 half_theta = 2*atan(|b|) + * R = |AB| / (2 * sin(half_theta)) + * 圆心 C = midpoint(A,B) + R*cos(half_theta) * perp_right(AB) + * 若 dist(C, 相邻段) < R → 圆与相邻段相交 → 弧自相交 + * + * @param pts_3d polyline 顶点（3D，原地读取，不修改） + * @param bulges 各段 bulge 值（若检测到自相交则原地置 0） + * @param is_axis true = XZ 平面（Axis），false = XY 平面（Profile） + * @param closed 是否为闭合多段线 + */ +void sanitize_negative_bulge_self_intersections(const std::vector& pts_3d, + std::vector& bulges, + bool is_axis, + bool closed) +{ + const auto n = static_cast(pts_3d.size()); + if (n < 2) return; + + // 将顶点投影到对应的 2D 工作平面 + std::vector pts(n); + for (uint32_t i = 0; i < n; ++i) { + if (is_axis) + pts[i] = {pts_3d[i].z, pts_3d[i].x}; // Axis: XZ 平面 → (Z, X) + else + pts[i] = {pts_3d[i].x, pts_3d[i].y}; // Profile: XY 平面 → (X, Y) + } + + const uint32_t seg_count = closed ? n : (n - 1u); + + for (uint32_t i = 0; i < seg_count; ++i) { + if (bulges[i] >= 0.0) continue; // 仅检查负 bulge 的内凹弧 + + const uint32_t i1 = (i + 1u) % n; + const Eigen::Vector2d& A = pts[i]; + const Eigen::Vector2d& B = pts[i1]; + const double b = bulges[i]; + const double chord_len = (B - A).norm(); + if (chord_len < 1e-12) continue; + + // 计算 CW 弧的圆心与半径 + // half_theta = theta/2，其中 theta = 4*atan(|b|) 为弧所对圆心角 + const double abs_b = std::abs(b); + const double half_theta = 2.0 * std::atan(abs_b); + const double R = chord_len / (2.0 * std::sin(half_theta)); + const Eigen::Vector2d chord_dir = (B - A) / chord_len; + // CW 弧（b < 0）的圆心在弦 AB 的右侧 + const Eigen::Vector2d perp_right = {chord_dir.y(), -chord_dir.x()}; + const double d_center = R * std::cos(half_theta); // 弦中点到圆心的距离 + const Eigen::Vector2d center = 0.5 * (A + B) + d_center * perp_right; + + bool self_intersects = false; + + // ---- 检测与前一段 [pts[i-1], A] 的自相交 ---- + if (closed || i > 0u) { + const uint32_t i0 = (i + n - 1u) % n; + const double dist = point_to_segment_dist_2d(center, pts[i0], A); + if (dist < R * (1.0 - 1e-6)) { + self_intersects = true; + std::cout << "[SANITIZE_ARC] 段[" << i << "] 负bulge圆弧" + << "（R=" << R << ", bulge=" << b << "）" + << "圆心到前段距离=" << dist << " < R，" + << "检测到与前段自相交，bulge → 0（退化直线）\n"; + } + } + + // ---- 检测与后一段 [B, pts[i+2]] 的自相交 ---- + if (!self_intersects && (closed || i + 2u < n)) { + const uint32_t i2 = (i + 2u) % n; + const double dist = point_to_segment_dist_2d(center, B, pts[i2]); + if (dist < R * (1.0 - 1e-6)) { + self_intersects = true; + std::cout << "[SANITIZE_ARC] 段[" << i << "] 负bulge圆弧" + << "（R=" << R << ", bulge=" << b << "）" + << "圆心到后段距离=" << dist << " < R，" + << "检测到与后段自相交，bulge → 0（退化直线）\n"; + } + } + + if (self_intersects) { bulges[i] = 0.0; } + } +} void insert_polyline_corner_fillets(const polyline_descriptor_t& desc, std::vector& out_points, @@ -195,7 +307,7 @@ void insert_polyline_corner_fillets(const polyline_descriptor_t& desc, to_2d = [](const vector3d& p) -> Eigen::Vector2d { return {p.z, p.x}; }; // 3D -> 2D: (Z, X) to_3d = [](const Eigen::Vector2d& q, double fixed_y) -> vector3d { return {q.y(), fixed_y, q.x()}; - }; // 2D -> 3D: (X, Y, Z) = (q.y, fixed_y, q.x) + }; // 2D -> 3D: (X, Y, Z) = (q.y, fixed_y, q.x) } else { // profile：XY 平面 to_2d = [](const vector3d& p) -> Eigen::Vector2d { return {p.x, p.y}; };