622 lines
24 KiB
C++
622 lines
24 KiB
C++
|
#include "vde/core/cam_mesh.h"
|
||
|
|
#include "vde/curves/nurbs_curve.h"
|
||
|
|
#include "vde/core/aabb.h"
|
||
|
|
#include <cmath>
|
||
|
|
#include <cstdio>
|
||
|
|
#include <algorithm>
|
||
|
|
#include <limits>
|
||
|
|
#include <map>
|
||
|
|
|
||
|
|
#ifndef M_PI
|
||
|
|
#define M_PI 3.14159265358979323846
|
||
|
|
#endif
|
||
|
|
|
||
|
|
namespace vde::core {
|
||
|
|
|
||
|
|
// ── Internal helpers ─────────────────────────────────────────────────────
|
||
|
|
|
||
|
|
namespace {
|
||
|
|
|
||
|
|
/// Format a double to 4 decimal places
|
||
|
|
std::string fmt(double v) {
|
||
|
|
char buf[32];
|
||
|
|
std::snprintf(buf, sizeof(buf), "%.4f", v);
|
||
|
|
return buf;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// 2D cross product
|
||
|
|
inline double cross2d(const Vector2D& a, const Vector2D& b) {
|
||
|
|
return a.x() * b.y() - a.y() * b.x();
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Segment-segment intersection (2D)
|
||
|
|
bool segment_intersect(const Point2D& a1, const Point2D& a2,
|
||
|
|
const Point2D& b1, const Point2D& b2, Point2D& out) {
|
||
|
|
Vector2D da = a2 - a1;
|
||
|
|
Vector2D db = b2 - b1;
|
||
|
|
double cross = cross2d(da, db);
|
||
|
|
if (std::abs(cross) < 1e-12) return false;
|
||
|
|
Vector2D dc = b1 - a1;
|
||
|
|
double t = cross2d(dc, db) / cross;
|
||
|
|
double u = cross2d(dc, da) / cross;
|
||
|
|
if (t >= -1e-12 && t <= 1.0 + 1e-12 && u >= -1e-12 && u <= 1.0 + 1e-12) {
|
||
|
|
out = a1 + t * da;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Ray-casting point-in-polygon (2D)
|
||
|
|
bool point_in_polygon(const std::vector<Point2D>& poly, const Point2D& p) {
|
||
|
|
int n = static_cast<int>(poly.size());
|
||
|
|
if (n < 3) return false;
|
||
|
|
bool inside = false;
|
||
|
|
for (int i = 0, j = n - 1; i < n; j = i++) {
|
||
|
|
const auto& pi = poly[i];
|
||
|
|
const auto& pj = poly[j];
|
||
|
|
if (((pi.y() > p.y()) != (pj.y() > p.y())) &&
|
||
|
|
(p.x() < (pj.x() - pi.x()) * (p.y() - pi.y()) / (pj.y() - pi.y()) + pi.x())) {
|
||
|
|
inside = !inside;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return inside;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Fit a clamped NURBS curve through given control points
|
||
|
|
curves::NurbsCurve fit_clamped(const std::vector<Point3D>& pts) {
|
||
|
|
int n = static_cast<int>(pts.size());
|
||
|
|
if (n < 2) {
|
||
|
|
return curves::NurbsCurve({pts[0], pts[0]}, {0,0,1,1}, {1,1}, 1);
|
||
|
|
}
|
||
|
|
int p = (n <= 2) ? 1 : std::min(3, n - 1);
|
||
|
|
std::vector<double> weights(n, 1.0);
|
||
|
|
std::vector<double> knots(n + p + 1);
|
||
|
|
for (int i = 0; i <= p; ++i) knots[i] = 0.0;
|
||
|
|
for (int i = n; i < n + p + 1; ++i) knots[i] = 1.0;
|
||
|
|
int interior = n - p - 1;
|
||
|
|
if (interior > 0) {
|
||
|
|
for (int i = 0; i <= interior; ++i) {
|
||
|
|
knots[p + i] = static_cast<double>(i) / static_cast<double>(interior + 1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return curves::NurbsCurve(pts, std::move(knots), std::move(weights), p);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Sample a NURBS curve at `count` evenly-spaced parameter values
|
||
|
|
std::vector<Point3D> sample_curve(const curves::NurbsCurve& curve, int count) {
|
||
|
|
std::vector<Point3D> pts;
|
||
|
|
pts.reserve(count);
|
||
|
|
auto [t0, t1] = curve.domain();
|
||
|
|
for (int i = 0; i < count; ++i) {
|
||
|
|
double t = t0 + (t1 - t0) * static_cast<double>(i) / static_cast<double>(count - 1);
|
||
|
|
pts.push_back(curve.evaluate(t));
|
||
|
|
}
|
||
|
|
return pts;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Rotate a 2D point around origin by angle (radians)
|
||
|
|
Point2D rotate_2d(const Point2D& p, double angle_rad) {
|
||
|
|
double c = std::cos(angle_rad);
|
||
|
|
double s = std::sin(angle_rad);
|
||
|
|
return Point2D(c * p.x() - s * p.y(), s * p.x() + c * p.y());
|
||
|
|
}
|
||
|
|
|
||
|
|
} // anonymous namespace
|
||
|
|
|
||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
||
|
|
// extract_contour_from_mesh — 截面轮廓提取
|
||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
||
|
|
|
||
|
|
std::vector<curves::NurbsCurve> extract_contour_from_mesh(
|
||
|
|
const mesh::HalfedgeMesh& mesh,
|
||
|
|
double z_height)
|
||
|
|
{
|
||
|
|
const double kEps = 1e-10;
|
||
|
|
|
||
|
|
// ── Step 1: Collect intersection segments ────────────────────────
|
||
|
|
// Each (p0, p1) represents a segment where a triangular face intersects
|
||
|
|
// the z-plane. A triangle intersecting the plane yields exactly 2 edge-hit
|
||
|
|
// points (general position).
|
||
|
|
struct Seg {
|
||
|
|
Point2D p0, p1;
|
||
|
|
};
|
||
|
|
std::vector<Seg> segs;
|
||
|
|
|
||
|
|
for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
|
||
|
|
auto fv = mesh.face_vertices(static_cast<int>(fi));
|
||
|
|
if (fv.size() < 3) continue;
|
||
|
|
|
||
|
|
std::vector<Point2D> hits; // 2D intersection points on this face
|
||
|
|
|
||
|
|
for (size_t j = 0; j < fv.size(); ++j) {
|
||
|
|
size_t k = (j + 1) % fv.size();
|
||
|
|
Point3D p0 = mesh.vertex(static_cast<size_t>(fv[j]));
|
||
|
|
Point3D p1 = mesh.vertex(static_cast<size_t>(fv[k]));
|
||
|
|
|
||
|
|
double z0 = p0.z();
|
||
|
|
double z1 = p1.z();
|
||
|
|
|
||
|
|
// Check if edge straddles the z plane
|
||
|
|
if ((z0 > z_height && z1 < z_height) ||
|
||
|
|
(z0 < z_height && z1 > z_height)) {
|
||
|
|
double t = (z_height - z0) / (z1 - z0);
|
||
|
|
if (t >= 0.0 && t <= 1.0) {
|
||
|
|
double x = p0.x() + t * (p1.x() - p0.x());
|
||
|
|
double y = p0.y() + t * (p1.y() - p0.y());
|
||
|
|
hits.emplace_back(x, y);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
// Edge lying exactly on the plane — take the whole edge
|
||
|
|
else if (std::abs(z0 - z_height) < kEps && std::abs(z1 - z_height) < kEps) {
|
||
|
|
hits.emplace_back(p0.x(), p0.y());
|
||
|
|
hits.emplace_back(p1.x(), p1.y());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// A triangle intersecting the plane should give exactly 2 points
|
||
|
|
// (unless the plane passes through a vertex or edge — those are degenerate)
|
||
|
|
if (hits.size() == 2) {
|
||
|
|
segs.push_back({hits[0], hits[1]});
|
||
|
|
} else if (hits.size() > 2) {
|
||
|
|
// Multiple hits: pair them up to form segments
|
||
|
|
// (e.g. plane passes through a vertex)
|
||
|
|
for (size_t a = 0; a + 1 < hits.size(); a += 2) {
|
||
|
|
segs.push_back({hits[a], hits[a + 1]});
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (segs.size() < 3) return {};
|
||
|
|
|
||
|
|
// ── Step 2: Chain segments into closed loops ─────────────────────
|
||
|
|
const double kMergeTol = 1e-8;
|
||
|
|
std::vector<std::vector<Point2D>> loops;
|
||
|
|
|
||
|
|
while (!segs.empty()) {
|
||
|
|
std::vector<Point2D> loop;
|
||
|
|
Seg seed = segs.back();
|
||
|
|
segs.pop_back();
|
||
|
|
|
||
|
|
loop.push_back(seed.p0);
|
||
|
|
Point2D target = seed.p1;
|
||
|
|
|
||
|
|
bool growing = true;
|
||
|
|
while (growing && !segs.empty()) {
|
||
|
|
// Check if we've closed the loop
|
||
|
|
if ((target - loop.front()).norm() < kMergeTol * 10.0) {
|
||
|
|
growing = false;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
growing = false;
|
||
|
|
for (auto it = segs.begin(); it != segs.end(); ++it) {
|
||
|
|
if ((target - it->p0).norm() < kMergeTol) {
|
||
|
|
loop.push_back(it->p0);
|
||
|
|
target = it->p1;
|
||
|
|
segs.erase(it);
|
||
|
|
growing = true;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
if ((target - it->p1).norm() < kMergeTol) {
|
||
|
|
loop.push_back(it->p1);
|
||
|
|
target = it->p0;
|
||
|
|
segs.erase(it);
|
||
|
|
growing = true;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Close the loop
|
||
|
|
loop.push_back(loop.front());
|
||
|
|
|
||
|
|
if (loop.size() >= 4) { // at least 3 unique points + closure
|
||
|
|
loops.push_back(std::move(loop));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// ── Step 3: Fit NURBS curves ─────────────────────────────────────
|
||
|
|
std::vector<curves::NurbsCurve> result;
|
||
|
|
result.reserve(loops.size());
|
||
|
|
|
||
|
|
for (const auto& loop : loops) {
|
||
|
|
std::vector<Point3D> pts3d;
|
||
|
|
pts3d.reserve(loop.size());
|
||
|
|
for (const auto& p : loop) {
|
||
|
|
pts3d.emplace_back(p.x(), p.y(), z_height);
|
||
|
|
}
|
||
|
|
result.push_back(fit_clamped(pts3d));
|
||
|
|
}
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
||
|
|
// contour_toolpath (mesh) — 轮廓加工刀路
|
||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
||
|
|
|
||
|
|
Toolpath contour_toolpath(
|
||
|
|
const mesh::HalfedgeMesh& mesh,
|
||
|
|
double z_height,
|
||
|
|
const Tool& /*tool*/,
|
||
|
|
const ContourParams& params)
|
||
|
|
{
|
||
|
|
Toolpath tp;
|
||
|
|
tp.name = "Contour_Mesh";
|
||
|
|
tp.safe_z = params.safe_z;
|
||
|
|
tp.cut_z = z_height;
|
||
|
|
tp.step_down = params.step_down;
|
||
|
|
|
||
|
|
// Offset the cutting plane by stock_to_leave (cut slightly outside)
|
||
|
|
double effective_z = z_height + params.stock_to_leave;
|
||
|
|
|
||
|
|
auto contours = extract_contour_from_mesh(mesh, effective_z);
|
||
|
|
if (contours.empty()) return tp;
|
||
|
|
|
||
|
|
double effective_step = params.step_down;
|
||
|
|
if (effective_step <= 0.0) effective_step = 1.0;
|
||
|
|
|
||
|
|
// Compute Z range for multi-pass
|
||
|
|
double z_min = std::numeric_limits<double>::max();
|
||
|
|
double z_max = std::numeric_limits<double>::lowest();
|
||
|
|
for (size_t vi = 0; vi < mesh.num_vertices(); ++vi) {
|
||
|
|
auto v = mesh.vertex(static_cast<int>(vi));
|
||
|
|
z_min = std::min(z_min, v.z());
|
||
|
|
z_max = std::max(z_max, v.z());
|
||
|
|
}
|
||
|
|
|
||
|
|
// Cut from safe_z level down to z_height in step_down increments
|
||
|
|
double start_z = z_max;
|
||
|
|
int pass_count = 0;
|
||
|
|
|
||
|
|
// Rapid to safe position
|
||
|
|
tp.segments.push_back({Point3D(0, 0, params.safe_z),
|
||
|
|
Point3D(0, 0, params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
|
||
|
|
// For contouring, we cut at the specified z_height (can be multi-pass)
|
||
|
|
while (start_z > z_height + 1e-9) {
|
||
|
|
double cut_z = std::max(z_height, start_z - effective_step);
|
||
|
|
|
||
|
|
for (size_t ci = 0; ci < contours.size(); ++ci) {
|
||
|
|
auto sampled = sample_curve(contours[ci], 200);
|
||
|
|
if (sampled.size() < 2) continue;
|
||
|
|
|
||
|
|
// First contour of first pass: rapid approach
|
||
|
|
if (pass_count == 0 && ci == 0) {
|
||
|
|
tp.segments.push_back({Point3D(0, 0, params.safe_z),
|
||
|
|
Point3D(sampled[0].x(), sampled[0].y(), params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
|
||
|
|
// Plunge to first cut depth
|
||
|
|
tp.segments.push_back({Point3D(sampled[0].x(), sampled[0].y(), params.safe_z),
|
||
|
|
Point3D(sampled[0].x(), sampled[0].y(), cut_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Linear,
|
||
|
|
params.feed_rate * 0.5, cut_z});
|
||
|
|
} else {
|
||
|
|
// Move to start of contour at safe Z then plunge
|
||
|
|
tp.segments.push_back({tp.segments.back().end,
|
||
|
|
Point3D(sampled[0].x(), sampled[0].y(), params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
|
||
|
|
tp.segments.push_back({Point3D(sampled[0].x(), sampled[0].y(), params.safe_z),
|
||
|
|
Point3D(sampled[0].x(), sampled[0].y(), cut_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Linear,
|
||
|
|
params.feed_rate * 0.5, cut_z});
|
||
|
|
}
|
||
|
|
|
||
|
|
// Follow the contour at this Z
|
||
|
|
for (size_t i = 1; i < sampled.size(); ++i) {
|
||
|
|
tp.segments.push_back({sampled[i-1], sampled[i],
|
||
|
|
Point3D::Zero(), PathSegmentType::Linear,
|
||
|
|
params.feed_rate, cut_z});
|
||
|
|
}
|
||
|
|
|
||
|
|
// Retract to safe Z between contours
|
||
|
|
tp.segments.push_back({tp.segments.back().end,
|
||
|
|
Point3D(tp.segments.back().end.x(),
|
||
|
|
tp.segments.back().end.y(), params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
}
|
||
|
|
|
||
|
|
start_z = cut_z;
|
||
|
|
pass_count++;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Final retract
|
||
|
|
if (!tp.segments.empty()) {
|
||
|
|
tp.segments.push_back({tp.segments.back().end,
|
||
|
|
Point3D(0, 0, params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
}
|
||
|
|
|
||
|
|
return tp;
|
||
|
|
}
|
||
|
|
|
||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
||
|
|
// pocket_toolpath (mesh) — 口袋加工刀路
|
||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
||
|
|
|
||
|
|
Toolpath pocket_toolpath(
|
||
|
|
const mesh::HalfedgeMesh& mesh,
|
||
|
|
const std::vector<mesh::HalfedgeMesh>& islands,
|
||
|
|
double z_height,
|
||
|
|
const Tool& tool,
|
||
|
|
const PocketParams& params)
|
||
|
|
{
|
||
|
|
Toolpath tp;
|
||
|
|
tp.name = "Pocket_Mesh";
|
||
|
|
tp.safe_z = params.safe_z;
|
||
|
|
tp.cut_z = z_height;
|
||
|
|
tp.step_down = params.step_down;
|
||
|
|
|
||
|
|
// Extract outer boundary as 2D polygon
|
||
|
|
auto boundary_curves = extract_contour_from_mesh(mesh, z_height);
|
||
|
|
if (boundary_curves.empty()) return tp;
|
||
|
|
|
||
|
|
// Use the longest contour as the outer boundary
|
||
|
|
std::vector<Point2D> boundary_poly;
|
||
|
|
{
|
||
|
|
size_t best = 0;
|
||
|
|
double best_len = 0;
|
||
|
|
for (size_t i = 0; i < boundary_curves.size(); ++i) {
|
||
|
|
auto sampled = sample_curve(boundary_curves[i], 200);
|
||
|
|
double len = 0;
|
||
|
|
for (size_t j = 1; j < sampled.size(); ++j) {
|
||
|
|
len += (Vector3D(sampled[j].x() - sampled[j-1].x(),
|
||
|
|
sampled[j].y() - sampled[j-1].y(),
|
||
|
|
sampled[j].z() - sampled[j-1].z())).norm();
|
||
|
|
}
|
||
|
|
if (len > best_len) {
|
||
|
|
best_len = len;
|
||
|
|
best = i;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
auto sampled = sample_curve(boundary_curves[best], 300);
|
||
|
|
boundary_poly.reserve(sampled.size());
|
||
|
|
for (auto& p : sampled) {
|
||
|
|
boundary_poly.emplace_back(p.x(), p.y());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (boundary_poly.size() < 3) return tp;
|
||
|
|
|
||
|
|
// Extract island polygons
|
||
|
|
std::vector<std::vector<Point2D>> island_polys;
|
||
|
|
for (const auto& island_mesh : islands) {
|
||
|
|
auto island_curves = extract_contour_from_mesh(island_mesh, z_height);
|
||
|
|
for (auto& ic : island_curves) {
|
||
|
|
auto sampled = sample_curve(ic, 200);
|
||
|
|
std::vector<Point2D> ipoly;
|
||
|
|
ipoly.reserve(sampled.size());
|
||
|
|
for (auto& p : sampled) {
|
||
|
|
ipoly.emplace_back(p.x(), p.y());
|
||
|
|
}
|
||
|
|
if (ipoly.size() >= 3) {
|
||
|
|
island_polys.push_back(std::move(ipoly));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Compute spacing
|
||
|
|
double spacing = params.step_over;
|
||
|
|
if (spacing <= 0.0) spacing = tool.diameter * 0.5;
|
||
|
|
|
||
|
|
// Compute bounding box
|
||
|
|
double min_x = std::numeric_limits<double>::max();
|
||
|
|
double max_x = std::numeric_limits<double>::lowest();
|
||
|
|
double min_y = std::numeric_limits<double>::max();
|
||
|
|
double max_y = std::numeric_limits<double>::lowest();
|
||
|
|
for (const auto& p : boundary_poly) {
|
||
|
|
min_x = std::min(min_x, p.x());
|
||
|
|
max_x = std::max(max_x, p.x());
|
||
|
|
min_y = std::min(min_y, p.y());
|
||
|
|
max_y = std::max(max_y, p.y());
|
||
|
|
}
|
||
|
|
|
||
|
|
double expand = std::max(max_x - min_x, max_y - min_y) * 0.1;
|
||
|
|
min_x -= expand;
|
||
|
|
max_x += expand;
|
||
|
|
min_y -= expand;
|
||
|
|
max_y += expand;
|
||
|
|
|
||
|
|
// Rotation angle for zigzag
|
||
|
|
double angle_rad = params.cut_angle * M_PI / 180.0;
|
||
|
|
|
||
|
|
// Determine the scanning direction based on the rotated bounding box
|
||
|
|
// We generate lines perpendicular to the scan direction
|
||
|
|
// For cut_angle=0: horizontal lines (vary Y), for cut_angle=90: vertical lines (vary X)
|
||
|
|
|
||
|
|
// Generate scan lines
|
||
|
|
// Rotate boundary to align with scan direction
|
||
|
|
std::vector<Point2D> rot_boundary;
|
||
|
|
rot_boundary.reserve(boundary_poly.size());
|
||
|
|
for (const auto& p : boundary_poly) {
|
||
|
|
rot_boundary.push_back(rotate_2d(p, -angle_rad));
|
||
|
|
}
|
||
|
|
|
||
|
|
// Also rotate islands
|
||
|
|
std::vector<std::vector<Point2D>> rot_islands;
|
||
|
|
for (const auto& ip : island_polys) {
|
||
|
|
std::vector<Point2D> rip;
|
||
|
|
rip.reserve(ip.size());
|
||
|
|
for (const auto& p : ip) {
|
||
|
|
rip.push_back(rotate_2d(p, -angle_rad));
|
||
|
|
}
|
||
|
|
rot_islands.push_back(std::move(rip));
|
||
|
|
}
|
||
|
|
|
||
|
|
// Compute rotated bounding box
|
||
|
|
double r_min_x = std::numeric_limits<double>::max();
|
||
|
|
double r_max_x = std::numeric_limits<double>::lowest();
|
||
|
|
double r_min_y = std::numeric_limits<double>::max();
|
||
|
|
double r_max_y = std::numeric_limits<double>::lowest();
|
||
|
|
for (const auto& p : rot_boundary) {
|
||
|
|
r_min_x = std::min(r_min_x, p.x());
|
||
|
|
r_max_x = std::max(r_max_x, p.x());
|
||
|
|
r_min_y = std::min(r_min_y, p.y());
|
||
|
|
r_max_y = std::max(r_max_y, p.y());
|
||
|
|
}
|
||
|
|
double r_expand = (r_max_y - r_min_y) * 0.05;
|
||
|
|
r_min_y -= r_expand;
|
||
|
|
r_max_y += r_expand;
|
||
|
|
|
||
|
|
// ── Generate horizontal scan lines in rotated space ─────────────
|
||
|
|
struct CutSeg { Point2D a, b; };
|
||
|
|
std::vector<CutSeg> cut_segs;
|
||
|
|
|
||
|
|
int ns = static_cast<int>(rot_boundary.size());
|
||
|
|
double line_start_x = r_min_x - spacing;
|
||
|
|
double line_end_x = r_max_x + spacing;
|
||
|
|
|
||
|
|
for (double y = r_min_y; y <= r_max_y + spacing * 0.5; y += spacing) {
|
||
|
|
Point2D l_start(line_start_x, y);
|
||
|
|
Point2D l_end(line_end_x, y);
|
||
|
|
|
||
|
|
// Find intersections with rotated boundary
|
||
|
|
std::vector<double> t_hits;
|
||
|
|
for (int i = 0; i < ns; ++i) {
|
||
|
|
int j = (i + 1) % ns;
|
||
|
|
Point2D out;
|
||
|
|
if (segment_intersect(l_start, l_end, rot_boundary[i], rot_boundary[j], out)) {
|
||
|
|
Vector2D dir = l_end - l_start;
|
||
|
|
double dlen = dir.squaredNorm();
|
||
|
|
if (dlen > 1e-12) {
|
||
|
|
double t = ((out - l_start).dot(dir)) / dlen;
|
||
|
|
t_hits.push_back(t);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
std::sort(t_hits.begin(), t_hits.end());
|
||
|
|
t_hits.erase(std::unique(t_hits.begin(), t_hits.end(),
|
||
|
|
[](double a, double b) { return std::abs(a - b) < 1e-9; }),
|
||
|
|
t_hits.end());
|
||
|
|
|
||
|
|
// For each pair of intersections, check if midpoint is inside boundary
|
||
|
|
// and outside all islands
|
||
|
|
for (size_t k = 0; k + 1 < t_hits.size(); k += 2) {
|
||
|
|
double ta = t_hits[k];
|
||
|
|
double tb = t_hits[k + 1];
|
||
|
|
Point2D mid = l_start + (ta + tb) * 0.5 * (l_end - l_start);
|
||
|
|
|
||
|
|
// Must be inside boundary
|
||
|
|
if (!point_in_polygon(rot_boundary, mid)) continue;
|
||
|
|
|
||
|
|
// Must be outside all islands
|
||
|
|
bool inside_island = false;
|
||
|
|
for (const auto& rip : rot_islands) {
|
||
|
|
if (point_in_polygon(rip, mid)) {
|
||
|
|
inside_island = true;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (inside_island) continue;
|
||
|
|
|
||
|
|
Point2D pa = l_start + ta * (l_end - l_start);
|
||
|
|
Point2D pb = l_start + tb * (l_end - l_start);
|
||
|
|
|
||
|
|
// Rotate back to original space
|
||
|
|
Point2D orig_a = rotate_2d(pa, angle_rad);
|
||
|
|
Point2D orig_b = rotate_2d(pb, angle_rad);
|
||
|
|
cut_segs.push_back({orig_a, orig_b});
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// ── Sort segments for efficient movement ───────────────────────
|
||
|
|
// Greedy nearest-neighbour sort
|
||
|
|
if (!cut_segs.empty()) {
|
||
|
|
std::vector<CutSeg> sorted;
|
||
|
|
sorted.reserve(cut_segs.size());
|
||
|
|
sorted.push_back(cut_segs[0]);
|
||
|
|
cut_segs.erase(cut_segs.begin());
|
||
|
|
|
||
|
|
while (!cut_segs.empty()) {
|
||
|
|
const Point2D& last = sorted.back().b;
|
||
|
|
size_t best_i = 0;
|
||
|
|
double best_dist = std::numeric_limits<double>::max();
|
||
|
|
for (size_t i = 0; i < cut_segs.size(); ++i) {
|
||
|
|
double d = (cut_segs[i].a - last).norm();
|
||
|
|
if (d < best_dist) {
|
||
|
|
best_dist = d;
|
||
|
|
best_i = i;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
sorted.push_back(cut_segs[best_i]);
|
||
|
|
cut_segs.erase(cut_segs.begin() + best_i);
|
||
|
|
}
|
||
|
|
cut_segs = std::move(sorted);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (cut_segs.empty()) {
|
||
|
|
tp.segments.push_back({Point3D(0, 0, params.safe_z),
|
||
|
|
Point3D(0, 0, params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
return tp;
|
||
|
|
}
|
||
|
|
|
||
|
|
// ── Build toolpath ─────────────────────────────────────────────
|
||
|
|
// Rapid approach to first point
|
||
|
|
tp.segments.push_back({Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), params.safe_z),
|
||
|
|
Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
|
||
|
|
// Plunge
|
||
|
|
tp.segments.push_back({Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), params.safe_z),
|
||
|
|
Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), z_height),
|
||
|
|
Point3D::Zero(), PathSegmentType::Linear,
|
||
|
|
params.feed_rate * 0.5, z_height});
|
||
|
|
|
||
|
|
for (size_t i = 0; i < cut_segs.size(); ++i) {
|
||
|
|
const auto& seg = cut_segs[i];
|
||
|
|
|
||
|
|
// Cut the segment
|
||
|
|
tp.segments.push_back({Point3D(seg.a.x(), seg.a.y(), z_height),
|
||
|
|
Point3D(seg.b.x(), seg.b.y(), z_height),
|
||
|
|
Point3D::Zero(), PathSegmentType::Linear,
|
||
|
|
params.feed_rate, z_height});
|
||
|
|
|
||
|
|
// Link to next segment
|
||
|
|
if (i + 1 < cut_segs.size()) {
|
||
|
|
const Point2D& next = cut_segs[i + 1].a;
|
||
|
|
double dist = (next - seg.b).norm();
|
||
|
|
if (dist < 5.0 * spacing) {
|
||
|
|
// Close enough: direct cut move
|
||
|
|
tp.segments.push_back({Point3D(seg.b.x(), seg.b.y(), z_height),
|
||
|
|
Point3D(next.x(), next.y(), z_height),
|
||
|
|
Point3D::Zero(), PathSegmentType::Linear,
|
||
|
|
params.feed_rate * 1.5, z_height});
|
||
|
|
} else {
|
||
|
|
// Too far: rapid retract + reposition + plunge
|
||
|
|
tp.segments.push_back({Point3D(seg.b.x(), seg.b.y(), z_height),
|
||
|
|
Point3D(seg.b.x(), seg.b.y(), params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
|
||
|
|
tp.segments.push_back({Point3D(seg.b.x(), seg.b.y(), params.safe_z),
|
||
|
|
Point3D(next.x(), next.y(), params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
|
||
|
|
tp.segments.push_back({Point3D(next.x(), next.y(), params.safe_z),
|
||
|
|
Point3D(next.x(), next.y(), z_height),
|
||
|
|
Point3D::Zero(), PathSegmentType::Linear,
|
||
|
|
params.feed_rate * 0.5, z_height});
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Retract
|
||
|
|
tp.segments.push_back({tp.segments.back().end,
|
||
|
|
Point3D(tp.segments.back().end.x(),
|
||
|
|
tp.segments.back().end.y(), params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
|
||
|
|
tp.segments.push_back({Point3D(tp.segments.back().end.x(),
|
||
|
|
tp.segments.back().end.y(), params.safe_z),
|
||
|
|
Point3D(0, 0, params.safe_z),
|
||
|
|
Point3D::Zero(), PathSegmentType::Rapid});
|
||
|
|
|
||
|
|
return tp;
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace vde::core
|