From bf7cc795305882e9b4b8a29ae900200719204317 Mon Sep 17 00:00:00 2001 From: linchforever <2766643922@qq.com> Date: Wed, 15 Apr 2026 17:28:28 +0800 Subject: [PATCH] fix: adjust the PMC function and add skip conditions for corner fillets --- .../subface/geometry/polyline_fillet.hpp | 7 +- .../src/subface/geometry/geometry_data.cpp | 94 ++++++++++------ .../src/subface/geometry/polyline_fillet.cpp | 106 ++++++++++++++++-- 3 files changed, 158 insertions(+), 49 deletions(-) diff --git a/primitive_process/interface/subface/geometry/polyline_fillet.hpp b/primitive_process/interface/subface/geometry/polyline_fillet.hpp index c6dca3d..44eba28 100644 --- a/primitive_process/interface/subface/geometry/polyline_fillet.hpp +++ b/primitive_process/interface/subface/geometry/polyline_fillet.hpp @@ -8,12 +8,9 @@ namespace internal struct fillet_config_t { double segment_ratio = 0.05 ;// 圆角切除量占相邻两段中较短段弦长的比例 double min_turn_angle_deg = 0.5; // 低于此角度（度）的衔接处视为共线，跳过 - - // 当锐角过尖（turn_angle > max_fillet_angle_deg）， - // 即使达到 min_fillet_radius 也无法避免自相交时的处理策略: - // true → 跳过该顶点（保留原始尖角，不插入圆弧） - // false → 用最大允许 trim 插入圆弧（会有轻微自相交，但尽力修复） bool is_axis = false; + double max_fillet_turn_angle_deg = 135.0; // 超过此转角不做圆角 + double min_fillet_radius = 1e-3; // 圆角半径下限 }; // 对 2D profile / axis 多段线的衔接顶点插入微小圆角弧，实现 G1 连续。 diff --git a/primitive_process/src/subface/geometry/geometry_data.cpp b/primitive_process/src/subface/geometry/geometry_data.cpp index c994183..0886082 100644 --- a/primitive_process/src/subface/geometry/geometry_data.cpp +++ b/primitive_process/src/subface/geometry/geometry_data.cpp @@ -452,48 +452,76 @@ void polyline_geometry_data::build_as_axis(polyline_descriptor_t&& const Eigen::Vector2d vec = p - closest_param.point.topRows<2>(); if (closest_param.is_peak_value) { - return vec.dot(this->calculate_normal(closest_param.t)) < 0; + return vec.dot(this->calculate_normal(closest_param.t)) < 0; } - constexpr double kEps = 1e-9; - const auto N = static_cast(this->thetas.size()); - const auto n = static_cast(std::round(closest_param.t)); - - // 出射段法向量 - const Eigen::Vector2d n_out = this->calculate_normal(static_cast(n) + kEps); - - // 入射段法向量 - Eigen::Vector2d n_in; - if (n == 0) { - const bool is_closed = (this->vertices.back() - this->vertices.front()).squaredNorm() < EPSILON; - if (is_closed && N > 1) { - n_in = this->calculate_normal(static_cast(N - 1) + 1.0 - kEps); - } else { - n_in = n_out; + + constexpr double kChordEps = 1e-12; // 弦长退化阈值 + constexpr double kCrossThresh = 1e-9; // 叉积共线阈值 + const auto N = static_cast(this->thetas.size()); + auto n = static_cast(std::round(closest_param.t)); + const bool is_closed = (this->vertices.back() - this->vertices.front()).squaredNorm() < EPSILON; + + // 对闭合曲线，顶点索引 N 等同于顶点 0 + if (is_closed && n >= N) n = 0u; + + // 顶点 i（0 ≤ i < N）的 2D 坐标存储在 vertices[start_indices[i]]。 + // i == N 时（开放曲线末端），使用 vertices.back()。 + auto get_vertex_2d = [&](uint32_t idx) -> Eigen::Vector2d { + return (idx < N) ? this->vertices[this->start_indices[idx]] : this->vertices.back(); + }; + + const Eigen::Vector2d v_curr = get_vertex_2d(n); + + Eigen::Vector2d v_next = v_curr; + { + const uint32_t n1 = n + 1; + if (n1 <= N) { + v_next = get_vertex_2d(n1); + } else if (is_closed) { + v_next = get_vertex_2d(0); } - } else { - n_in = this->calculate_normal(static_cast(n - 1) + 1.0 - kEps); + // else: 开放曲线 n == N，无后继，v_next 保持 = v_curr + } + + Eigen::Vector2d v_prev = v_curr; + if (n > 0u) { + v_prev = get_vertex_2d(n - 1u); + } else if (is_closed && N > 1u) { + v_prev = get_vertex_2d(N - 1u); + } + + const Eigen::Vector2d chord_out_raw = v_next - v_curr; + const Eigen::Vector2d chord_in_raw = v_curr - v_prev; + const double len_out = chord_out_raw.norm(); + const double len_in = chord_in_raw.norm(); + + // 顶点重合,回退到弧/线法线 + if (len_out < kChordEps && len_in < kChordEps) { + const double t_fallback = std::min(static_cast(n) + 1e-9, static_cast(N) - 1e-9); + return vec.dot(this->calculate_normal(t_fallback)) < 0; } - const double dot_in = vec.dot(n_in); - const double dot_out = vec.dot(n_out); + // 归一化（单边退化时用另一边代替） + const Eigen::Vector2d chord_out = (len_out >= kChordEps) ? (chord_out_raw / len_out) : (chord_in_raw / len_in); + const Eigen::Vector2d chord_in = (len_in >= kChordEps) ? (chord_in_raw / len_in) : (chord_out_raw / len_out); + + const Eigen::Vector2d n_in_chord(-chord_in.y(), chord_in.x()); + const Eigen::Vector2d n_out_chord(-chord_out.y(), chord_out.x()); + + const double dot_in = vec.dot(n_in_chord); + const double dot_out = vec.dot(n_out_chord); - const Eigen::Vector2d t_out = this->calculate_tangent(static_cast(n) + kEps); - const Eigen::Vector2d t_in = (n == 0 && (this->vertices.back() - this->vertices.front()).squaredNorm() < EPSILON) - ? this->calculate_tangent(static_cast(N - 1) + 1.0 - kEps) - : (n > 0 ? this->calculate_tangent(static_cast(n - 1) + 1.0 - kEps) : t_out); - const double cross = t_in.x() * t_out.y() - t_in.y() * t_out.x(); + const double cross = chord_in.x() * chord_out.y() - chord_in.y() * chord_out.x(); - constexpr double kCrossThreshold = 1e-9; - if (cross > kCrossThreshold) { - // 凸角：INSIDE = 在两个法向量内侧的交集 - // OUTSIDE = NOT(两者均 ≥ 0) = 至少有一个 < 0 → OR + if (cross > kCrossThresh) { + // 凸角 return dot_in < 0.0 || dot_out < 0.0; - } else if (cross < -kCrossThreshold) { - // 凹角：INSIDE = 在两个法向量内侧的并集 - // OUTSIDE = 在两者均反侧 → AND + } else if (cross < -kCrossThresh) { + // 凹角 return dot_in < 0.0 && dot_out < 0.0; } else { - // 平角（共线切向）：保持加权平均 + // 平角（切向共线，光滑连接） + // 两半平面方向近似相同，取双法线加权平均 return (dot_in + dot_out) < 0.0; } } diff --git a/primitive_process/src/subface/geometry/polyline_fillet.cpp b/primitive_process/src/subface/geometry/polyline_fillet.cpp index 9bdc6cb..c3db227 100644 --- a/primitive_process/src/subface/geometry/polyline_fillet.cpp +++ b/primitive_process/src/subface/geometry/polyline_fillet.cpp @@ -71,11 +71,27 @@ Corner compute_corner(const std::vector& pts, const double min_turn = cfg.min_turn_angle_deg * (pi / 180.0); if (turn_angle < min_turn) return c; // 视为共线，不处理 - // 切除距离：取相邻两段弦长最小值的比例，并硬限幅 + // 转角接近 180° 时，fillet 弧两端点几乎重合，弧半径趋近于零。 + // 此时 fillet 无法在当前网格分辨率下被正确表达，应跳过以避免几何崩溃。 + const double max_fillet_turn_angle_deg = cfg.max_fillet_turn_angle_deg; + if (turn_angle > max_fillet_turn_angle_deg * (pi / 180.0)) { + std::cout << "转角 " << turn_angle * 180.0 / pi << "° 超过阈值，跳过圆角（避免退化弧）" << std::endl; + return c; + } + const double chord_in = (pts[i] - pts[prev]).norm(); const double chord_out = (pts[next] - pts[i]).norm(); const double trim = std::min({std::min(chord_in, chord_out) * cfg.segment_ratio, chord_in * 0.45, chord_out * 0.45}); + // 计算实际 fillet 圆弧半径并与最小可分辨阈值比较 + // R_fillet = trim / tan(turn_angle/2) + const double half_turn = turn_angle / 2.0; + const double R_fillet = (std::tan(half_turn) > 1e-9) ? trim / std::tan(half_turn) : 0.0; + if (R_fillet < cfg.min_fillet_radius) { + std::cout << "圆角半径 " << R_fillet << " 低于最小阈值 " << cfg.min_fillet_radius << "，跳过圆角" << std::endl; + return c; + } + if (trim < 1e-12) return c; c.trim_in = pts[i] - trim * T_in; @@ -91,6 +107,61 @@ Corner compute_corner(const std::vector& pts, return c; } +/** + * @brief 将圆弧 A→B（bulge b）裁切为子弧 A_new→B_new 后，重新计算子弧的 bulge。 + * + * Bulge 依赖弦长和包含角，裁切端点后两者均变化，必须重算。 + * 对直线段（b≈0）直接返回 0，不做计算。 + * + * @param A 原弧起点 + * @param B 原弧终点 + * @param b 原弧 bulge + * @param A_new 裁切后的新起点（在同一圆上） + * @param B_new 裁切后的新终点（在同一圆上） + * @return 子弧的 bulge + */ +static double recompute_subarc_bulge(const Eigen::Vector2d& A, + const Eigen::Vector2d& B, + double b, + const Eigen::Vector2d& A_new, + const Eigen::Vector2d& B_new) +{ + // 直线段：裁切后仍是直线 + if (std::abs(b) < 1e-10) return 0.0; + + // 原弧包含角 θ = 4·atan(|b|)，半角 = 2·atan(|b|) + const double half_theta = 2.0 * std::atan(std::abs(b)); + const double sign_b = (b > 0.0) ? 1.0 : -1.0; + + // 原弧弦 + const Eigen::Vector2d chord = B - A; + const double chord_len = chord.norm(); + if (chord_len < 1e-12) return b; + + // 圆半径与圆心 + const double R = chord_len / (2.0 * std::sin(half_theta)); + // 弦中垂线方向（左手正交），对 CCW 弧（b>0）圆心在弦左侧 + const Eigen::Vector2d perp_unit(-chord.y() / chord_len, chord.x() / chord_len); + const double d = R * std::cos(half_theta); // 弦中点到圆心的距离 + const Eigen::Vector2d center = 0.5 * (A + B) + sign_b * d * perp_unit; + + // 新起终点在圆上的角度 + const double ang_A = std::atan2(A_new.y() - center.y(), A_new.x() - center.x()); + const double ang_B = std::atan2(B_new.y() - center.y(), B_new.x() - center.x()); + + double delta = ang_B - ang_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; + } else { + if (delta > 0.0) delta -= 2.0 * pi; + if (delta < -2.0 * pi) delta += 2.0 * pi; + } + + return std::tan(delta / 4.0); +} inline void push_point(std::vector& out_points, std::vector& out_bulges, @@ -122,11 +193,13 @@ void insert_polyline_corner_fillets(const polyline_descriptor_t& desc, if (cfg.is_axis) { // axis：XZ 平面 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) + 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) } else { // profile：XY 平面 - to_2d = [](const vector3d& p) -> Eigen::Vector2d {return {p.x, p.y};}; - to_3d = [](const Eigen::Vector2d& q, double fixed_z) -> vector3d {return {q.x(), q.y(), fixed_z};}; + to_2d = [](const vector3d& p) -> Eigen::Vector2d { return {p.x, p.y}; }; + to_3d = [](const Eigen::Vector2d& q, double fixed_z) -> vector3d { return {q.x(), q.y(), fixed_z}; }; } // 退化情况 @@ -168,20 +241,31 @@ void insert_polyline_corner_fillets(const polyline_descriptor_t& desc, if (closed) { for (uint32_t i = 0; i < n; ++i) { - const uint32_t next = (i + 1) % n; - const auto& seg_start = corners[i].active ? corners[i].trim_out : pts[i]; - push_pt(seg_start, fixed_coords[i], bulges[i]); + const uint32_t next = (i + 1) % n; + const Eigen::Vector2d seg_start = corners[i].active ? corners[i].trim_out : pts[i]; + const Eigen::Vector2d seg_end = corners[next].active ? corners[next].trim_in : pts[next]; + + // 子弧端点已改变，必须重算 bulge + const double seg_bulge = recompute_subarc_bulge(pts[i], pts[next], bulges[i], seg_start, seg_end); + + push_pt(seg_start, fixed_coords[i], seg_bulge); // 原来是 bulges[i] if (corners[next].active) push_pt(corners[next].trim_in, fixed_coords[next], corners[next].fillet_bulge); } } else { for (uint32_t i = 0; i < n - 1; ++i) { - const uint32_t next = i + 1; - const auto& seg_start = (i == 0) ? pts[0] : corners[i].active ? corners[i].trim_out : pts[i]; - push_pt(seg_start, fixed_coords[i], bulges[i]); + const uint32_t next = i + 1; + + const Eigen::Vector2d seg_start = (i == 0) ? pts[0] : (corners[i].active ? corners[i].trim_out : pts[i]); + const Eigen::Vector2d seg_end = (next < n - 1 && corners[next].active) ? corners[next].trim_in : pts[next]; + + + const double seg_bulge = recompute_subarc_bulge(pts[i], pts[next], bulges[i], seg_start, seg_end); + + push_pt(seg_start, fixed_coords[i], seg_bulge); if (next < n - 1 && corners[next].active) push_pt(corners[next].trim_in, fixed_coords[next], corners[next].fillet_bulge); } - out_points.push_back(desc.points[n - 1]); // 末点原样保留 + out_points.push_back(desc.points[n - 1]); } if (closed)