Compare commits
5 Commits
c4d9968e9f
...
557f1e49d9
| Author | SHA1 | Date |
|---|---|---|
 linchforever
|
557f1e49d9 | 4 months ago |
|
linchforever
|
16f32e18cc | 4 months ago |
|
linchforever
|
da3efa945c | 4 months ago |
|
linchforever
|
57c7a7a865 | 4 months ago |
|
linchforever
|
717b151b0a | 4 months ago |
7 changed files with 338 additions and 183 deletions
@ -0,0 +1,25 @@ |
|||||
|
#pragma once |
||||
|
#include <vector> |
||||
|
#include <primitive_descriptor.h> |
||||
|
|
||||
|
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; |
||||
|
}; |
||||
|
|
||||
|
// 对 2D profile / axis 多段线的衔接顶点插入微小圆角弧，实现 G1 连续。
|
||||
|
void insert_polyline_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,192 @@ |
|||||
|
#include <subface/geometry/polyline_fillet.hpp> |
||||
|
#include <math/math_defs.hpp> |
||||
|
|
||||
|
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_polyline_corner_fillets(const polyline_descriptor_t& desc, |
||||
|
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 = desc.point_number; |
||||
|
const bool closed = desc.is_close; |
||||
|
|
||||
|
std::function<Eigen::Vector2d(const vector3d&)> to_2d; |
||||
|
std::function<vector3d(const Eigen::Vector2d&, double)> to_3d; |
||||
|
|
||||
|
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)
|
||||
|
} 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};}; |
||||
|
} |
||||
|
|
||||
|
// 退化情况
|
||||
|
if (n < 3) { |
||||
|
for (uint32_t i = 0; i < n; ++i) { |
||||
|
out_points.push_back(desc.points[i]); |
||||
|
if (i < n - 1) out_bulges.push_back(desc.bulges ? desc.bulges[i] : 0.0); |
||||
|
} |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
std::vector<Eigen::Vector2d> pts(n); |
||||
|
std::vector<double> bulges(n); |
||||
|
// fixed_coord：axis→y分量（恒=0），profile→z分量（恒=0）
|
||||
|
std::vector<double> fixed_coords(n); |
||||
|
|
||||
|
for (uint32_t i = 0; i < n; ++i) { |
||||
|
pts[i] = to_2d(desc.points[i]); |
||||
|
bulges[i] = desc.bulges ? desc.bulges[i] : 0.0; |
||||
|
fixed_coords[i] = cfg.is_axis ? desc.points[i].y // axis：保留 y（=0）
|
||||
|
: desc.points[i].z; // profile：保留 z（=0）
|
||||
|
} |
||||
|
|
||||
|
// 计算每个顶点的圆角
|
||||
|
std::vector<Corner> corners(n); |
||||
|
if (closed) { |
||||
|
for (uint32_t i = 0; i < n; ++i) corners[i] = compute_corner(pts, bulges, i, (i + n - 1) % n, (i + 1) % n, cfg); |
||||
|
} else { |
||||
|
for (uint32_t i = 1; i <= n - 2; ++i) corners[i] = compute_corner(pts, bulges, i, i - 1, i + 1, cfg); |
||||
|
} |
||||
|
|
||||
|
auto push_pt = [&](const Eigen::Vector2d& p2d, double fixed, double bulge) { |
||||
|
out_points.push_back(to_3d(p2d, fixed)); |
||||
|
out_bulges.push_back(bulge); |
||||
|
}; |
||||
|
|
||||
|
out_points.reserve(n * 2); |
||||
|
out_bulges.reserve(n * 2); |
||||
|
|
||||
|
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]); |
||||
|
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]); |
||||
|
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]); // 末点原样保留
|
||||
|
} |
||||
|
|
||||
|
if (closed) |
||||
|
assert(out_points.size() == out_bulges.size()); |
||||
|
else |
||||
|
assert(out_bulges.size() + 1 == out_points.size()); |
||||
|
} |
||||
|
} // namespace internal
|
||||
Loading…
Reference in new issue