C2 curvature matching and path deviation for Bezier blends

bezier9Init() accepts curvature parameters (kappa, normal) at both
endpoints. For arc-adjacent blends, P2/P3 are offset in the curvature
normal direction by delta = 5*alpha^2*kappa/4 to match adjacent segment
curvature, eliminating centripetal acceleration discontinuity at junctions.
For line segments (kappa=0) the offset is zero and behavior is unchanged.

Add bezier9PathDeviation() which measures maximum distance to the
two-segment programmed path (P_start -> corner -> P_end), correct for
both symmetric and asymmetric blends. Add bezier9MaxDeviation() for
multi-sample diagnostic verification.

Author: grandixximo

src/emc/motion_planning/bezier9.cc

@@ -291,6 +291,10 @@ int bezier9Init(Bezier9 * const b,
                 PmCartesian const * const u_end_abc,
                 PmCartesian const * const u_start_uvw,
                 PmCartesian const * const u_end_uvw,
+                double kappa_start,
+                PmCartesian const * const n_start,
+                double kappa_end,
+                PmCartesian const * const n_end,
                 double alpha)
 {
     // Validate inputs
@@ -314,14 +318,34 @@ int bezier9Init(Bezier9 * const b,
     PmCartesian start_uvw = {start->u, start->v, start->w};
     PmCartesian end_uvw   = {end->u,   end->v,   end->w};
 
-    // Set quintic control points for each subspace (G2 continuity)
+    // Set quintic control points for each subspace (zero-curvature baseline)
     set_quintic_control_points(b->P, &start_xyz, &end_xyz,
                                u_start_xyz, u_end_xyz, alpha);
     set_quintic_control_points(b->A, &start_abc, &end_abc,
                                u_start_abc, u_end_abc, alpha);
     set_quintic_control_points(b->U, &start_uvw, &end_uvw,
                                u_start_uvw, u_end_uvw, alpha);
 
+    // C2 curvature matching (XYZ only): offset P[2] and P[3] in the
+    // curvature normal direction to match adjacent segment curvature.
+    //
+    // For a quintic Bezier with P0,P1,P2 collinear along tangent u:
+    //   B'(0) = 5α·u,  B''(0) = 20·δ·n  (after offset)
+    //   κ(0) = |B'×B''| / |B'|³ = 4δ / (5α²)
+    // Solving for target curvature: δ = 5α²·κ / 4
+    if (kappa_start > BEZIER9_CURVATURE_EPSILON && n_start) {
+        double delta = 5.0 * alpha * alpha * kappa_start / 4.0;
+        b->P[2].x += delta * n_start->x;
+        b->P[2].y += delta * n_start->y;
+        b->P[2].z += delta * n_start->z;
+    }
+    if (kappa_end > BEZIER9_CURVATURE_EPSILON && n_end) {
+        double delta = 5.0 * alpha * alpha * kappa_end / 4.0;
+        b->P[3].x += delta * n_end->x;
+        b->P[3].y += delta * n_end->y;
+        b->P[3].z += delta * n_end->z;
+    }
+
     // Build arc-length parameterization table
     build_arc_length_table(b);
 
@@ -614,3 +638,84 @@ double bezier9Deviation(Bezier9 const * const b,
 
     return sqrt(dx * dx + dy * dy + dz * dz);
 }
+
+double bezier9MaxDeviation(Bezier9 const * const b,
+                           PmCartesian const * const intersection_point,
+                           int n_samples)
+{
+    if (!b || !intersection_point || n_samples < 1) {
+        return 0.0;
+    }
+
+    double max_dev = 0.0;
+    for (int i = 1; i <= n_samples; i++) {
+        double t = (double)i / (double)(n_samples + 1);
+        PmCartesian pt;
+        bezier5_eval(b->P, t, &pt);
+        double dx = pt.x - intersection_point->x;
+        double dy = pt.y - intersection_point->y;
+        double dz = pt.z - intersection_point->z;
+        double dev = sqrt(dx * dx + dy * dy + dz * dz);
+        if (dev > max_dev) max_dev = dev;
+    }
+    return max_dev;
+}
+
+/**
+ * Point-to-line-segment distance in 3D.
+ * Returns the minimum Euclidean distance from point p to the
+ * line segment from a to b.
+ */
+static double point_to_segment_dist(PmCartesian const * const p,
+                                    PmCartesian const * const a,
+                                    PmCartesian const * const b)
+{
+    double abx = b->x - a->x;
+    double aby = b->y - a->y;
+    double abz = b->z - a->z;
+    double apx = p->x - a->x;
+    double apy = p->y - a->y;
+    double apz = p->z - a->z;
+
+    double ab_sq = abx * abx + aby * aby + abz * abz;
+    if (ab_sq < 1e-30) {
+        /* Degenerate segment (a == b): distance to the point */
+        return sqrt(apx * apx + apy * apy + apz * apz);
+    }
+
+    double t = (apx * abx + apy * aby + apz * abz) / ab_sq;
+    if (t < 0.0) t = 0.0;
+    if (t > 1.0) t = 1.0;
+
+    double cx = a->x + t * abx - p->x;
+    double cy = a->y + t * aby - p->y;
+    double cz = a->z + t * abz - p->z;
+
+    return sqrt(cx * cx + cy * cy + cz * cz);
+}
+
+double bezier9PathDeviation(Bezier9 const * const b,
+                            PmCartesian const * const seg1_start,
+                            PmCartesian const * const corner,
+                            PmCartesian const * const seg2_end,
+                            int n_samples)
+{
+    if (!b || !seg1_start || !corner || !seg2_end || n_samples < 1) {
+        return 0.0;
+    }
+
+    double max_dev = 0.0;
+    for (int i = 1; i <= n_samples; i++) {
+        double t = (double)i / (double)(n_samples + 1);
+        PmCartesian pt;
+        bezier5_eval(b->P, t, &pt);
+
+        /* Distance to programmed path = min of distances to both segments */
+        double d1 = point_to_segment_dist(&pt, seg1_start, corner);
+        double d2 = point_to_segment_dist(&pt, corner, seg2_end);
+        double dev = fmin(d1, d2);
+
+        if (dev > max_dev) max_dev = dev;
+    }
+    return max_dev;
+}

src/emc/motion_planning/bezier9.h

@@ -26,10 +26,12 @@ extern "C" {
  * xyz (translation), abc (rotation), and uvw (auxiliary) coordinate spaces.
  *
  * G2 continuity is achieved by placing P0,P1,P2 collinear along the
- * incoming tangent and P3,P4,P5 collinear along the outgoing tangent.
- * This gives B''(0) = B''(1) = 0, so curvature is zero at both endpoints
- * — matching the zero curvature of adjacent line segments and eliminating
- * the centripetal acceleration discontinuity that cubic blends suffer from.
+ * incoming tangent and P3,P4,P5 collinear along the outgoing tangent,
+ * then offsetting P2 and P3 in the curvature normal direction to match
+ * the curvature of adjacent segments.  For line segments (κ=0) the
+ * offset is zero and P0,P1,P2 remain collinear (B''(0)=0).  For arcs
+ * (κ=1/R) the offset δ = 5α²κ/4 gives κ_bezier(endpoint) = κ_arc,
+ * eliminating the centripetal acceleration discontinuity at junctions.
  *
  * Arc-length parameterization is precomputed during initialization to enable
  * constant-velocity traversal. Curvature analysis is performed on the xyz
@@ -78,6 +80,10 @@ typedef struct {
  * @param u_end_abc Unit tangent vector at end (abc)
  * @param u_start_uvw Unit tangent vector at start (uvw)
  * @param u_end_uvw Unit tangent vector at end (uvw)
+ * @param kappa_start Curvature of adjacent segment at start (0 for lines)
+ * @param n_start Unit curvature normal at start (toward center), NULL if kappa=0
+ * @param kappa_end Curvature of adjacent segment at end (0 for lines)
+ * @param n_end Unit curvature normal at end (toward center), NULL if kappa=0
  * @param alpha Shape parameter controlling control point placement (> 0)
  * @return TP_ERR_OK on success, error code otherwise
  */
@@ -90,6 +96,10 @@ int bezier9Init(Bezier9 * const b,
                 PmCartesian const * const u_end_abc,
                 PmCartesian const * const u_start_uvw,
                 PmCartesian const * const u_end_uvw,
+                double kappa_start,
+                PmCartesian const * const n_start,
+                double kappa_end,
+                PmCartesian const * const n_end,
                 double alpha);
 
 /**
@@ -192,6 +202,43 @@ double bezier9AccLimit(Bezier9 const * const b,
 double bezier9Deviation(Bezier9 const * const b,
                         PmCartesian const * const intersection_point);
 
+/**
+ * bezier9MaxDeviation - Compute maximum deviation over N sample points
+ *
+ * Samples the xyz Bezier at N equally-spaced t values in (0,1) and returns
+ * the maximum distance to the intersection point.  Used for diagnostic
+ * verification of the t=0.5 single-point estimate.
+ *
+ * @param b Input Bezier9 curve
+ * @param intersection_point Original corner intersection point (xyz only)
+ * @param n_samples Number of sample points (e.g. 19 for t = 0.05, 0.10, ... 0.95)
+ * @return Maximum deviation distance (always >= 0)
+ */
+double bezier9MaxDeviation(Bezier9 const * const b,
+                           PmCartesian const * const intersection_point,
+                           int n_samples);
+
+/**
+ * bezier9PathDeviation - Max deviation from two-segment programmed path
+ *
+ * Measures the maximum distance from the Bezier curve to the nearest point
+ * on the two-segment polyline (seg1_start → corner → seg2_end). Unlike
+ * bezier9Deviation (which measures distance to the corner only), this gives
+ * the correct path deviation for both symmetric and asymmetric blends.
+ *
+ * @param b Input Bezier9 curve
+ * @param seg1_start Start of first segment (xyz, = blend P_start)
+ * @param corner Junction point between segments (xyz, = intersection_point)
+ * @param seg2_end End of second segment (xyz, = blend P_end)
+ * @param n_samples Number of sample points (9 is sufficient)
+ * @return Maximum path deviation (always >= 0)
+ */
+double bezier9PathDeviation(Bezier9 const * const b,
+                            PmCartesian const * const seg1_start,
+                            PmCartesian const * const corner,
+                            PmCartesian const * const seg2_end,
+                            int n_samples);
+
 #ifdef __cplusplus
 }
 #endif