4 changed files with 300 additions and 9 deletions
@ -0,0 +1,30 @@ |
|||||
|
#pragma once |
||||
|
#include <vector> |
||||
|
#include <primitive_descriptor.h> |
||||
|
|
||||
|
namespace internal |
||||
|
{ |
||||
|
|
||||
|
struct fillet_config_t { |
||||
|
// 圆角切除量占相邻两段中较短段弦长的比例（默认 5%）
|
||||
|
// 越小则圆角越微小，棱角感越强。
|
||||
|
double segment_ratio = 0.05; |
||||
|
// 低于此角度（度）的衔接处视为共线，跳过不处理
|
||||
|
double min_turn_angle_deg = 0.5; |
||||
|
}; |
||||
|
|
||||
|
// 对 2D profile 多段线的衔接顶点插入微小圆角弧，实现 G1 连续。
|
||||
|
//
|
||||
|
// 闭合多边形：处理全部 n 个顶点，输出隐式首尾相连。
|
||||
|
// 不闭合多段线：仅处理内部顶点 [1, n-2]，首尾端点原样保留。
|
||||
|
//
|
||||
|
// @param profile 原始 profile descriptor（通过 is_closed 区分开闭）
|
||||
|
// @param out_points 输出点列表
|
||||
|
// @param out_bulges 输出 bulge 列表（与 out_points 等长）
|
||||
|
// @param cfg 圆角参数
|
||||
|
void insert_profile_corner_fillets(const polyline_descriptor_t& profile, |
||||
|
std::vector<vector3d>& out_points, |
||||
|
std::vector<double>& out_bulges, |
||||
|
const fillet_config_t& cfg = {}); |
||||
|
|
||||
|
} // namespace internal
|
||||
@ -0,0 +1,215 @@ |
|||||
|
#include <subface/geometry/polyline_fillet.hpp> |
||||
|
#include <math/math_defs.hpp> |
||||
|
//#include <algorithm>
|
||||
|
|
||||
|
namespace internal |
||||
|
{ |
||||
|
namespace |
||||
|
{ |
||||
|
|
||||
|
// 将 2D 向量 v 旋转 angle 弧度（逆时针为正）
|
||||
|
inline Eigen::Vector2d rotate2d(Eigen::Vector2d v, double angle) |
||||
|
{ |
||||
|
const double c = std::cos(angle), s = std::sin(angle); |
||||
|
return {c * v.x() - s * v.y(), s * v.x() + c * v.y()}; |
||||
|
} |
||||
|
|
||||
|
// 圆弧段 A→B（bulge b）在终点 B 处的切线方向
|
||||
|
Eigen::Vector2d arc_end_tangent(Eigen::Vector2d A, Eigen::Vector2d B, double bulge) |
||||
|
{ |
||||
|
Eigen::Vector2d d = B - A; |
||||
|
if (d.norm() < 1e-12) return Eigen::Vector2d::UnitX(); |
||||
|
d.normalize(); |
||||
|
if (std::abs(bulge) < 1e-12) return d; // 直线段
|
||||
|
|
||||
|
const double sign = bulge > 0 ? 1.0 : -1.0; |
||||
|
const double theta = 4.0 * std::atan(std::abs(bulge)); |
||||
|
return rotate2d(d, sign * theta / 2.0); |
||||
|
} |
||||
|
|
||||
|
// 圆弧段 A→B（bulge b）在起点 A 处的切线方向
|
||||
|
Eigen::Vector2d arc_start_tangent(Eigen::Vector2d A, Eigen::Vector2d B, double bulge) |
||||
|
{ |
||||
|
Eigen::Vector2d d = B - A; |
||||
|
if (d.norm() < 1e-12) return Eigen::Vector2d::UnitX(); |
||||
|
d.normalize(); |
||||
|
if (std::abs(bulge) < 1e-12) return d; |
||||
|
|
||||
|
const double sign = bulge > 0 ? 1.0 : -1.0; |
||||
|
const double theta = 4.0 * std::atan(std::abs(bulge)); |
||||
|
return rotate2d(d, -sign * theta / 2.0); |
||||
|
} |
||||
|
|
||||
|
|
||||
|
struct Corner { |
||||
|
bool active = false; |
||||
|
Eigen::Vector2d trim_in; // 圆弧起点
|
||||
|
Eigen::Vector2d trim_out; // 圆弧终点
|
||||
|
double fillet_bulge = 0; // 圆弧的 bulge
|
||||
|
}; |
||||
|
|
||||
|
// 计算顶点 i 的圆角参数
|
||||
|
// @param pts 所有顶点
|
||||
|
// @param bulges 所有段的 bulge（bulges[i] 对应段 i→i+1，bulges[prev] 对应入射段）
|
||||
|
// @param i 当前顶点索引
|
||||
|
// @param prev 入射段起点索引
|
||||
|
// @param next 出射段终点索引
|
||||
|
Corner compute_corner(const std::vector<Eigen::Vector2d>& pts, |
||||
|
const std::vector<double>& bulges, |
||||
|
uint32_t i, |
||||
|
uint32_t prev, |
||||
|
uint32_t next, |
||||
|
const fillet_config_t& cfg) |
||||
|
{ |
||||
|
Corner c{}; |
||||
|
|
||||
|
const Eigen::Vector2d T_in = arc_end_tangent(pts[prev], pts[i], bulges[prev]); |
||||
|
const Eigen::Vector2d T_out = arc_start_tangent(pts[i], pts[next], bulges[i]); |
||||
|
|
||||
|
const double cos_a = std::clamp(T_in.dot(T_out), -1.0, 1.0); |
||||
|
const double turn_angle = std::acos(cos_a); |
||||
|
|
||||
|
const double min_turn = cfg.min_turn_angle_deg * (pi / 180.0); |
||||
|
if (turn_angle < min_turn) 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}); |
||||
|
|
||||
|
if (trim < 1e-12) return c; |
||||
|
|
||||
|
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; |
||||
|
|
||||
|
c.active = true; |
||||
|
c.fillet_bulge = fillet_bulge; |
||||
|
return c; |
||||
|
} |
||||
|
|
||||
|
|
||||
|
inline void push_point(std::vector<vector3d>& out_points, |
||||
|
std::vector<double>& out_bulges, |
||||
|
const Eigen::Vector2d& p, |
||||
|
double z, |
||||
|
double bulge) |
||||
|
{ |
||||
|
out_points.push_back({p.x(), p.y(), z}); |
||||
|
out_bulges.push_back(bulge); |
||||
|
} |
||||
|
|
||||
|
} // anonymous namespace
|
||||
|
|
||||
|
void insert_profile_corner_fillets(const polyline_descriptor_t& profile, |
||||
|
std::vector<vector3d>& out_points, |
||||
|
std::vector<double>& out_bulges, |
||||
|
const fillet_config_t& cfg) |
||||
|
{ |
||||
|
out_points.clear(); |
||||
|
out_bulges.clear(); |
||||
|
|
||||
|
const uint32_t n = profile.point_number; |
||||
|
const bool closed = profile.is_close; |
||||
|
|
||||
|
// ── 退化情况：点数过少，无内部衔接顶点可处理 ────────────────────────
|
||||
|
// 闭合：至少需要 3 点才有顶点（含衔接）
|
||||
|
// 不闭合：至少需要 3 点才有内部顶点（至少 1 个 interior vertex）
|
||||
|
if (n < 3) { |
||||
|
for (uint32_t i = 0; i < n; ++i) { |
||||
|
out_points.push_back(profile.points[i]); |
||||
|
out_bulges.push_back(profile.bulges ? profile.bulges[i] : 0.0); |
||||
|
} |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
std::vector<Eigen::Vector2d> pts(n); |
||||
|
std::vector<double> bulges(n); |
||||
|
for (uint32_t i = 0; i < n; ++i) { |
||||
|
pts[i] = {profile.points[i].x, profile.points[i].y}; |
||||
|
bulges[i] = profile.bulges ? profile.bulges[i] : 0.0; |
||||
|
} |
||||
|
|
||||
|
// ── 逐顶点计算圆角 ────────────────────────────────────────────────────
|
||||
|
// 闭合：处理全部 n 个顶点（首尾环形相连）
|
||||
|
// 不闭合：仅处理内部顶点 [1, n-2]（首尾为端点，无入/出射配对）
|
||||
|
std::vector<Corner> corners(n); |
||||
|
|
||||
|
if (closed) { |
||||
|
for (uint32_t i = 0; i < n; ++i) { |
||||
|
const uint32_t prev = (i + n - 1) % n; |
||||
|
const uint32_t next = (i + 1) % n; |
||||
|
corners[i] = compute_corner(pts, bulges, i, prev, next, cfg); |
||||
|
} |
||||
|
} else { |
||||
|
// corners[0] 和 corners[n-1] 保持默认 inactive
|
||||
|
for (uint32_t i = 1; i <= n - 2; ++i) { corners[i] = compute_corner(pts, bulges, i, i - 1, i + 1, cfg); } |
||||
|
} |
||||
|
|
||||
|
out_points.reserve(n * 2); |
||||
|
out_bulges.reserve(n * 2); |
||||
|
|
||||
|
if (closed) { |
||||
|
// 闭合：环形迭代，输出隐式首尾相连
|
||||
|
//
|
||||
|
// 对每段 i（pts[i]→pts[next]，bulge=bulges[i]）：
|
||||
|
// 1.段起点：corners[i].active ? trim_out[i] : pts[i]
|
||||
|
// 2.若 corners[next].active：插入 trim_in[next]（附 fillet_bulge[next]）
|
||||
|
// 下一迭代自然从 trim_out[next] 开始
|
||||
|
for (uint32_t i = 0; i < n; ++i) { |
||||
|
const uint32_t next = (i + 1) % n; |
||||
|
const double z_i = profile.points[i].z; |
||||
|
const double z_nxt = profile.points[next].z; |
||||
|
|
||||
|
const Eigen::Vector2d& seg_start = corners[i].active ? corners[i].trim_out : pts[i]; |
||||
|
|
||||
|
push_point(out_points, out_bulges, seg_start, z_i, bulges[i]); |
||||
|
|
||||
|
if (corners[next].active) { |
||||
|
push_point(out_points, out_bulges, corners[next].trim_in, z_nxt, corners[next].fillet_bulge); |
||||
|
} |
||||
|
} |
||||
|
} else { |
||||
|
// 不闭合：线性迭代，明确保留首尾端点
|
||||
|
//
|
||||
|
// 对每段 i（0..n-2），输出段起点；若 next 顶点有圆角（且非末点），
|
||||
|
// 则额外插入 trim_in[next]（附 fillet_bulge[next]）作为本段实际终点，
|
||||
|
// 下一迭代从 trim_out[next] 开始。
|
||||
|
|
||||
|
for (uint32_t i = 0; i < n - 1; ++i) { |
||||
|
const uint32_t next = i + 1; |
||||
|
const double z_i = profile.points[i].z; |
||||
|
const double z_nxt = profile.points[next].z; |
||||
|
|
||||
|
// 段起点：
|
||||
|
// i==0 → 始终为 pts[0]（端点，没有圆角）
|
||||
|
// corners[i].active → trim_out[i]（由上一迭代的圆角偏移得到）
|
||||
|
// 否则 → pts[i]
|
||||
|
const Eigen::Vector2d& seg_start = (i == 0) ? pts[0] : corners[i].active ? corners[i].trim_out : pts[i]; |
||||
|
|
||||
|
// 若 next 有圆角（且 next 不是末点，末点无圆角）：
|
||||
|
// 本段实际终点为 trim_in[next]，之后插入圆弧，
|
||||
|
// 下一迭代从 trim_out[next] 开始
|
||||
|
if (next < n - 1 && corners[next].active) { |
||||
|
push_point(out_points, out_bulges, seg_start, z_i, bulges[i]); |
||||
|
push_point(out_points, out_bulges, corners[next].trim_in, z_nxt, corners[next].fillet_bulge); |
||||
|
} else { |
||||
|
// next 无圆角（或 next 是末点）：正常输出段起点
|
||||
|
push_point(out_points, out_bulges, seg_start, z_i, bulges[i]); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// 末点：始终为 pts[n-1]（端点，没有圆角）
|
||||
|
// bulge 对最后一点无意义（无后续段），填 0.0
|
||||
|
out_points.push_back(profile.points[n - 1]); |
||||
|
out_bulges.push_back(0.0); |
||||
|
} |
||||
|
|
||||
|
assert(out_points.size() == out_bulges.size()); |
||||
|
} |
||||
|
|
||||
|
} // namespace internal
|
||||
Loading…
Reference in new issue