Adding ability to curve a jagged path

curve_path.py

+import numpy as np
+import matplotlib.pyplot as plt
+
+# Example points
+points = np.array([[0, 0], [1, 0], [1, 1], [2, 1], [2, 2], [3, 2]])
+
+# Parameters
+control_point_distance = 0.3  # Distance of control points from the middle point
+
+# Function to calculate the Bézier curve points
+def bezier_curve(t, control_points):
+    P0, P1, P2 = control_points
+    return (1 - t)**2 * P0 + 2 * (1 - t) * t * P1 + t**2 * P2
+
+# List to store the points along the smoothed curve
+smoothed_curve = []
+
+# Connect the first point to the first control point
+control_point_start = points[0] + (points[1] - points[0]) * control_point_distance
+smoothed_curve.append(points[0])
+smoothed_curve.append(control_point_start)
+
+# Iterate through each set of three consecutive points
+for i in range(len(points) - 2):
+    # Extract the three consecutive points
+    P0 = points[i]
+    P1 = points[i + 1]
+    P2 = points[i + 2]
+    
+    # Calculate the tangent directions at the start and end points
+    tangent_start = (P1 - P0) / np.linalg.norm(P1 - P0)
+    tangent_end = (P2 - P1) / np.linalg.norm(P2 - P1)
+    
+    # Calculate the control points
+    control_point_start = P1 - tangent_start * control_point_distance
+    control_point_end = P1 + tangent_end * control_point_distance
+    
+    # Construct the Bézier curve for the current set of points
+    control_points = [control_point_start, P1, control_point_end]
+    t_values = np.linspace(0, 1, 100)
+    curve_points = np.array([bezier_curve(t, control_points) for t in t_values])
+    
+    # Append the points along the curve to the smoothed curve list
+    smoothed_curve.extend(curve_points[1:])
+
+# Connect the last control point to the last point
+control_point_end = points[-1] - (points[-1] - points[-2]) * control_point_distance
+smoothed_curve.append(control_point_end)
+smoothed_curve.append(points[-1])
+
+# Convert smoothed curve points to a numpy array
+smoothed_curve = np.array(smoothed_curve)
+
+# Plot the original points and the smoothed curve
+plt.plot(points[:, 0], points[:, 1], 'bo-', label='original path')
+plt.plot(smoothed_curve[:, 0], smoothed_curve[:, 1], 'r-', label='curved path')
+plt.xlabel('X')
+plt.ylabel('Y')
+# plt.title('Smoothed Curve using Bézier Curves')
+plt.legend()
+plt.grid(True)
+plt.axis('equal')
+plt.show()