feat(v3.4): CAM toolpath — contour + pocket + G-code

src/CMakeLists.txt

@@ -26,6 +26,7 @@ add_library(vde_core STATIC
     core/distance.cpp
     core/convex_hull.cpp
     core/icp.cpp
+    core/cam_toolpath.cpp
 )
 target_include_directories(vde_core
     PUBLIC  ${CMAKE_SOURCE_DIR}/include

src/core/cam_toolpath.cpp

@@ -0,0 +1,418 @@
+#include "vde/core/cam_toolpath.h"
+#include "vde/curves/nurbs_curve.h"
+#include "vde/core/aabb.h"
+#include <cmath>
+#include <cstdio>
+#include <algorithm>
+#include <limits>
+#include <utility>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace vde::core {
+
+// ── Internal helpers ─────────────────────────────────────────────────────
+
+namespace {
+
+/// Format a double to 4 decimal places for G-code
+std::string fmt(double v) {
+    char buf[32];
+    std::snprintf(buf, sizeof(buf), "%.4f", v);
+    return buf;
+}
+
+/// Sample a NURBS curve at `count` evenly-spaced parameter values.
+/// Returns the 3D points in order.
+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;
+}
+
+/// Create a clamped NURBS curve with given control points.
+/// Uses degree = 1 for ≤2 points, degree = 3 for more.
+curves::NurbsCurve fit_clamped(const std::vector<Point3D>& pts) {
+    int n = static_cast<int>(pts.size());
+    if (n < 2) {
+        // Degenerate — single-point curve
+        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;
+    for (int i = 0; i <= interior; ++i) {
+        knots[p + i] = static_cast<double>(i) / static_cast<double>(interior);
+    }
+    return curves::NurbsCurve(pts, std::move(knots), std::move(weights), p);
+}
+
+/// Return the 2D tangent direction at each sample point (XY plane only).
+std::vector<Vector2D> sample_tangents(const curves::NurbsCurve& curve, int count) {
+    std::vector<Vector2D> tangents;
+    tangents.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);
+        auto deriv = curve.derivative(t, 1);
+        Vector2D d(deriv.x(), deriv.y());
+        double len = d.norm();
+        if (len < 1e-12) {
+            // Fallback — use finite difference
+            double eps = 1e-6;
+            auto p_fwd = curve.evaluate(std::min(t1, t + eps));
+            auto p_bwd = curve.evaluate(std::max(t0, t - eps));
+            d = Vector2D(p_fwd.x() - p_bwd.x(), p_fwd.y() - p_bwd.y());
+            len = d.norm();
+        }
+        tangents.push_back(len > 1e-12 ? d / len : Vector2D(1, 0));
+    }
+    return tangents;
+}
+
+/// Return the CCW normal of a 2D tangent: rotate by -90° (right-hand outward)
+inline Vector2D normal_ccw(const Vector2D& tangent) {
+    return Vector2D(tangent.y(), -tangent.x());
+}
+
+/// 2D cross product (scalar): a.x * b.y - a.y * b.x
+inline double cross2d(const Vector2D& a, const Vector2D& b) {
+    return a.x() * b.y() - a.y() * b.x();
+}
+
+/// 2D point-in-polygon (ray-casting)
+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;
+}
+
+/// Line segment intersection (2D). Returns true + t parameter if segments intersect.
+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;
+}
+
+/// Build a closed 2D polygon from curve samples.
+std::vector<Point2D> curve_to_polygon(const curves::NurbsCurve& curve, int samples = 200) {
+    auto pts3d = sample_curve(curve, samples);
+    std::vector<Point2D> poly;
+    poly.reserve(pts3d.size());
+    for (auto& p : pts3d) poly.emplace_back(p.x(), p.y());
+    return poly;
+}
+
+/// Generate evenly spaced 2D points along a line from `start` to `end`.
+std::vector<Point2D> sample_line(const Point2D& start, const Point2D& end, double spacing) {
+    Vector2D dir = end - start;
+    double len = dir.norm();
+    if (len < 1e-12) return {start};
+    int steps = std::max(1, static_cast<int>(std::ceil(len / spacing)));
+    std::vector<Point2D> pts;
+    pts.reserve(steps + 1);
+    for (int i = 0; i <= steps; ++i) {
+        double t = static_cast<double>(i) / static_cast<double>(steps);
+        pts.push_back(start + t * dir);
+    }
+    return pts;
+}
+
+}  // anonymous namespace
+
+// ── Public API ────────────────────────────────────────────────────────────
+
+curves::NurbsCurve offset_contour(const curves::NurbsCurve& curve, double distance) {
+    constexpr int N = 200;  // sample count
+    auto pts = sample_curve(curve, N);
+    auto tangents = sample_tangents(curve, N);
+    std::vector<Point3D> offset_pts;
+    offset_pts.reserve(N);
+
+    for (int i = 0; i < N; ++i) {
+        Vector2D n = normal_ccw(tangents[i]);
+        offset_pts.emplace_back(
+            pts[i].x() + distance * n.x(),
+            pts[i].y() + distance * n.y(),
+            pts[i].z());
+    }
+
+    return fit_clamped(offset_pts);
+}
+
+Toolpath contour_toolpath(const curves::NurbsCurve& curve,
+                          double cut_depth, double safe_z, double step_down) {
+    Toolpath tp;
+    tp.name = "Contour";
+    tp.safe_z = safe_z;
+    tp.cut_z = cut_depth;
+    tp.step_down = step_down;
+
+    // Discretize contour
+    constexpr int N = 200;
+    auto pts = sample_curve(curve, N);
+
+    // Close the loop if open
+    if ((pts.front() - pts.back()).norm() > 1e-6) {
+        pts.push_back(pts.front());
+    }
+
+    // Depth passes from safe_z (or 0) stepping down to cut_depth
+    double current_z = 0.0;
+    bool first_pass = true;
+
+    // Move to safe Z and to start XY
+    tp.segments.push_back({Point3D(0, 0, safe_z), Point3D(pts[0].x(), pts[0].y(), safe_z),
+                           Point3D::Zero(), PathSegmentType::Rapid});
+
+    while (current_z > cut_depth + 1e-9) {
+        current_z = std::max(cut_depth, current_z - step_down);
+
+        // Plunge to depth
+        tp.segments.push_back({Point3D(pts[0].x(), pts[0].y(), safe_z),
+                               Point3D(pts[0].x(), pts[0].y(), current_z),
+                               Point3D::Zero(), PathSegmentType::Linear, 500.0, current_z});
+
+        // Follow contour
+        for (size_t i = 1; i < pts.size(); ++i) {
+            tp.segments.push_back({pts[i - 1], pts[i], Point3D::Zero(),
+                                   PathSegmentType::Linear, 1000.0, current_z});
+        }
+
+        // If more passes, rapid back to safe Z
+        if (current_z > cut_depth + 1e-9) {
+            tp.segments.push_back({pts.back(), Point3D(pts.back().x(), pts.back().y(), safe_z),
+                                   Point3D::Zero(), PathSegmentType::Rapid});
+        }
+
+        first_pass = false;
+    }
+
+    // Retract to safe Z
+    tp.segments.push_back({pts.back(), Point3D(pts.back().x(), pts.back().y(), safe_z),
+                           Point3D::Zero(), PathSegmentType::Rapid});
+
+    return tp;
+}
+
+Toolpath pocket_toolpath(const curves::NurbsCurve& boundary,
+                         const std::vector<curves::NurbsCurve>& islands,
+                         double cut_depth, double step_over, double angle) {
+    Toolpath tp;
+    tp.name = "Pocket";
+    tp.safe_z = 10.0;
+    tp.cut_z = cut_depth;
+    tp.step_down = 1.0;
+
+    // Discretize boundary
+    auto boundary_poly = curve_to_polygon(boundary);
+
+    // Compute boundary AABB (2D)
+    AABB3D aabb;
+    for (auto& p : boundary_poly) {
+        aabb.expand(Point3D(p.x(), p.y(), 0.0));
+    }
+
+    double min_x = aabb.min().x();
+    double max_x = aabb.max().x();
+    double min_y = aabb.min().y();
+    double max_y = aabb.max().y();
+
+    // Step-over: assume default 1mm tool diameter, step_over is fraction
+    double spacing = step_over;  // mm between zigzag lines
+
+    // Zigzag direction angle in radians
+    double rad = angle * M_PI / 180.0;
+    Vector2D zig_dir(std::cos(rad), std::sin(rad));           // primary direction
+    Vector2D step_dir(-std::sin(rad), std::cos(rad));         // perpendicular (step direction)
+
+    // Compute the sweep range along step_dir
+    double diag_min = std::numeric_limits<double>::max();
+    double diag_max = std::numeric_limits<double>::lowest();
+    std::vector<Point2D> corners = {
+        {min_x, min_y}, {max_x, min_y}, {max_x, max_y}, {min_x, max_y}
+    };
+    for (auto& c : corners) {
+        double proj = c.x() * step_dir.x() + c.y() * step_dir.y();
+        diag_min = std::min(diag_min, proj);
+        diag_max = std::max(diag_max, proj);
+    }
+
+    // Extend a bit to fully cover
+    diag_min -= spacing;
+    diag_max += spacing;
+
+    // Perpendicular span for zigzag lines
+    double zig_len_min = std::numeric_limits<double>::max();
+    double zig_len_max = std::numeric_limits<double>::lowest();
+    for (auto& c : corners) {
+        double proj = c.x() * zig_dir.x() + c.y() * zig_dir.y();
+        zig_len_min = std::min(zig_len_min, proj);
+        zig_len_max = std::max(zig_len_max, proj);
+    }
+    double extend = std::max(max_x - min_x, max_y - min_y) * 0.2;
+    double zig_start = zig_len_min - extend;
+    double zig_end   = zig_len_max + extend;
+
+    // Collect cut segments from zigzag lines
+    struct CutSeg { Point2D a, b; };
+    std::vector<CutSeg> cut_segs;
+
+    for (double d = diag_min; d <= diag_max + 1e-9; d += spacing) {
+        // Zigzag line: from zig_start to zig_end along zig_dir, offset by d along step_dir
+        Point2D line_start = zig_start * zig_dir + d * step_dir;
+        Point2D line_end   = zig_end   * zig_dir + d * step_dir;
+
+        // Find all intersections of this line with the boundary polygon edges
+        std::vector<double> t_intersects;
+        int nb = static_cast<int>(boundary_poly.size());
+        for (int i = 0; i < nb; ++i) {
+            int j = (i + 1) % nb;
+            Point2D out;
+            if (segment_intersect(line_start, line_end, boundary_poly[i], boundary_poly[j], out)) {
+                Vector2D dir = line_end - line_start;
+                double len = dir.squaredNorm();
+                if (len < 1e-12) continue;
+                double t = ((out - line_start).dot(dir)) / len;
+                t_intersects.push_back(t);
+            }
+        }
+
+        // Deduplicate and sort
+        std::sort(t_intersects.begin(), t_intersects.end());
+        t_intersects.erase(std::unique(t_intersects.begin(), t_intersects.end(),
+            [](double a, double b) { return std::abs(a - b) < 1e-9; }),
+            t_intersects.end());
+
+        // Pair intersections: pairs [t0,t1], [t2,t3], ... are inside
+        for (size_t k = 0; k + 1 < t_intersects.size(); k += 2) {
+            double ta = t_intersects[k];
+            double tb = t_intersects[k + 1];
+            // Midpoint check: is it inside?
+            Point2D mid = line_start + (ta + tb) * 0.5 * (line_end - line_start);
+            if (point_in_polygon(boundary_poly, mid)) {
+                cut_segs.push_back({line_start + ta * (line_end - line_start),
+                                    line_start + tb * (line_end - line_start)});
+            }
+        }
+    }
+
+    if (cut_segs.empty()) {
+        // Fallback: just follow the boundary as contour
+        return contour_toolpath(boundary, cut_depth, 10.0, 1.0);
+    }
+
+    // Build toolpath segments
+    // Move to safe Z
+    tp.segments.push_back({Point3D(0, 0, 10.0),
+                           Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), 10.0),
+                           Point3D::Zero(), PathSegmentType::Rapid});
+
+    // Plunge
+    tp.segments.push_back({Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), 10.0),
+                           Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), cut_depth),
+                           Point3D::Zero(), PathSegmentType::Linear, 500.0, cut_depth});
+
+    for (size_t i = 0; i < cut_segs.size(); ++i) {
+        const auto& seg = cut_segs[i];
+        tp.segments.push_back({Point3D(seg.a.x(), seg.a.y(), cut_depth),
+                               Point3D(seg.b.x(), seg.b.y(), cut_depth),
+                               Point3D::Zero(), PathSegmentType::Linear, 1000.0, cut_depth});
+
+        // Connect to next segment if close enough, otherwise rapid
+        if (i + 1 < cut_segs.size()) {
+            Point2D next = cut_segs[i + 1].a;
+            Point2D current = seg.b;
+            double dist = (next - current).norm();
+            if (dist < 5.0 * spacing) {
+                // Direct linking cut
+                tp.segments.push_back({Point3D(seg.b.x(), seg.b.y(), cut_depth),
+                                       Point3D(next.x(), next.y(), cut_depth),
+                                       Point3D::Zero(), PathSegmentType::Linear, 1500.0, cut_depth});
+            } else {
+                // Rapid retract, move, plunge
+                tp.segments.push_back({Point3D(seg.b.x(), seg.b.y(), cut_depth),
+                                       Point3D(seg.b.x(), seg.b.y(), 10.0),
+                                       Point3D::Zero(), PathSegmentType::Rapid});
+                tp.segments.push_back({Point3D(seg.b.x(), seg.b.y(), 10.0),
+                                       Point3D(next.x(), next.y(), 10.0),
+                                       Point3D::Zero(), PathSegmentType::Rapid});
+                tp.segments.push_back({Point3D(next.x(), next.y(), 10.0),
+                                       Point3D(next.x(), next.y(), cut_depth),
+                                       Point3D::Zero(), PathSegmentType::Linear, 500.0, cut_depth});
+            }
+        }
+    }
+
+    // Retract
+    tp.segments.push_back({cut_segs.empty() ? Point3D(0,0,cut_depth) :
+                           Point3D(cut_segs.back().b.x(), cut_segs.back().b.y(), cut_depth),
+                           cut_segs.empty() ? Point3D(0,0,10.0) :
+                           Point3D(cut_segs.back().b.x(), cut_segs.back().b.y(), 10.0),
+                           Point3D::Zero(), PathSegmentType::Rapid});
+
+    return tp;
+}
+
+std::string export_gcode(const Toolpath& tp) {
+    std::string gcode;
+    gcode += "(Generated by ViewDesignEngine CAM)\n";
+    gcode += "G90 G21 G17\n";  // absolute, mm, XY plane
+    gcode += "G0 Z" + fmt(tp.safe_z) + "\n";  // safe height
+
+    for (const auto& seg : tp.segments) {
+        switch (seg.type) {
+            case PathSegmentType::Rapid:
+                gcode += "G0 X" + fmt(seg.end.x()) + " Y" + fmt(seg.end.y()) + "\n";
+                break;
+            case PathSegmentType::Linear:
+                gcode += "G1 X" + fmt(seg.end.x()) + " Y" + fmt(seg.end.y()) +
+                         " Z" + fmt(seg.z_depth) + " F" + fmt(seg.feed_rate) + "\n";
+                break;
+            case PathSegmentType::ArcCW:
+                gcode += "G2 X" + fmt(seg.end.x()) + " Y" + fmt(seg.end.y()) +
+                         " I" + fmt(seg.center.x()) + " J" + fmt(seg.center.y()) + "\n";
+                break;
+            case PathSegmentType::ArcCCW:
+                gcode += "G3 X" + fmt(seg.end.x()) + " Y" + fmt(seg.end.y()) +
+                         " I" + fmt(seg.center.x()) + " J" + fmt(seg.center.y()) + "\n";
+                break;
+        }
+    }
+
+    gcode += "G0 Z" + fmt(tp.safe_z) + "\n";  // retract
+    gcode += "M30\n";  // program end
+    return gcode;
+}
+
+}  // namespace vde::core