title: How could I segment a long Bezier cruve and add some control points to the path that I have found to make sure this curve never hit the obstacles?
body: <p>I'm generating a grid 2D map with 50*50 size with <code>random</code> distribution to the obstacles. These obstacles have different heights according to my <code>drone_flight height = 60</code>. </p>
  
  <p>I'm working on improving a trajectory for multirotor UAV and I need to get a continuous trajectory by using <code>Bezier curves</code>. It means I need to get one <code>Bezier curve</code>. I have applied the <code>Bezier python code</code> and <code>B-spline codes</code> that I have found here in the links below:</p>
  
  <p><a href="https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/BSplinePath/bspline_path.py" rel="nofollow noreferrer">https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/BSplinePath/bspline_path.py</a></p>
  
  <p><a href="https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/BezierPath/bezier_path.py" rel="nofollow noreferrer">https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/BezierPath/bezier_path.py</a></p>
  
  <p>and they work well, but the <code>Bezier curve</code> sometimes enters the <code>safe_zone</code> around the obstacles that I made. For me, I tested here both of them (<code>Bezier and B-spline</code>), but I need to work just with <code>Bezier curves</code>. </p>
  
  <p>I'm planning to segment the long <code>Bezier curve</code> that I get to multiple segments until to add some control points to the segment of the curve that enters the <code>safe_zone</code> of obstacles to pull it out towards the path. I attached below a picture for my path with <code>B-spline</code> and <code>Bezier curve</code>. The Bezier curve with green color and how it entered the yellow safe_zone.</p>
  
  <p><a href="https://i.stack.imgur.com/jRIhb.jpg" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/jRIhb.jpg" alt="PathPlanning"></a></p>
  
  <p>Could I get assistance on how to <code>segment</code> the <code>Bezeir curve</code> and how to add control points to a specific segment?</p>
  
  <p>P.S. My map is generated randomly and the obstacles have height are also random. So, at every running will be new values and shape.</p>
  
  <p>Bezier Python Code:</p>
  
  <pre><code>import matplotlib.pyplot as plt
  import numpy as np
  import scipy.special
  
  show_animation = True
  
  
  def calc_4points_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset):
      """
      Compute control points and path given start and end position.
      :param sx: (float) x-coordinate of the starting point
      :param sy: (float) y-coordinate of the starting point
      :param syaw: (float) yaw angle at start
      :param ex: (float) x-coordinate of the ending point
      :param ey: (float) y-coordinate of the ending point
      :param eyaw: (float) yaw angle at the end
      :param offset: (float)
      :return: (numpy array, numpy array)
      """
      dist = np.sqrt((sx - ex) ** 2 + (sy - ey) ** 2) / offset
      control_points = np.array(
          [[sx, sy],
           [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],
           [ex - dist * np.cos(eyaw), ey - dist * np.sin(eyaw)],
           [ex, ey]])
  
      path = calc_bezier_path(control_points, n_points=100)
  
      return path, control_points
  
  
  def calc_bezier_path(control_points, n_points=100):
      """
      Compute bezier path (trajectory) given control points.
      :param control_points: (numpy array)
      :param n_points: (int) number of points in the trajectory
      :return: (numpy array)
      """
      traj = []
      for t in np.linspace(0, 1, n_points):
          traj.append(bezier(t, control_points))
  
      return np.array(traj)
  
  
  def bernstein_poly(n, i, t):
      """
      Bernstein polynom.
      :param n: (int) polynom degree
      :param i: (int)
      :param t: (float)
      :return: (float)
      """
      return scipy.special.comb(n, i) * t ** i * (1 - t) ** (n - i)
  
  
  def bezier(t, control_points):
      """
      Return one point on the bezier curve.
      :param t: (float) number in [0, 1]
      :param control_points: (numpy array)
      :return: (numpy array) Coordinates of the point
      """
      n = len(control_points) - 1
      return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)
  
  
  def bezier_derivatives_control_points(control_points, n_derivatives):
      """
      Compute control points of the successive derivatives of a given bezier curve.
      A derivative of a bezier curve is a bezier curve.
      See https://pomax.github.io/bezierinfo/#derivatives
      for detailed explanations
      :param control_points: (numpy array)
      :param n_derivatives: (int)
      e.g., n_derivatives=2 -&gt; compute control points for first and second derivatives
      :return: ([numpy array])
      """
      w = {0: control_points}
      for i in range(n_derivatives):
          n = len(w[i])
          w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j])
                               for j in range(n - 1)])
      return w
  
  
  def curvature(dx, dy, ddx, ddy):
      """
      Compute curvature at one point given first and second derivatives.
      :param dx: (float) First derivative along x axis
      :param dy: (float)
      :param ddx: (float) Second derivative along x axis
      :param ddy: (float)
      :return: (float)
      """
      return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2)
  
  
  def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):  # pragma: no cover
      """Plot arrow."""
      if not isinstance(x, float):
          for (ix, iy, iyaw) in zip(x, y, yaw):
              plot_arrow(ix, iy, iyaw)
      else:
          plt.arrow(x, y, length * np.cos(yaw), length * np.sin(yaw),
                    fc=fc, ec=ec, head_width=width, head_length=width)
          plt.plot(x, y)
  
  
  def main():
      """Plot an example bezier curve."""
      start_x = 10.0  # [m]
      start_y = 1.0  # [m]
      start_yaw = np.radians(180.0)  # [rad]
  
      end_x = -0.0  # [m]
      end_y = -3.0  # [m]
      end_yaw = np.radians(-45.0)  # [rad]
      offset = 3.0
  
      path, control_points = calc_4points_bezier_path(
          start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)
  
      # Note: alternatively, instead of specifying start and end position
      # you can directly define n control points and compute the path:
      # control_points = np.array([[5., 1.], [-2.78, 1.], [-11.5, -4.5], [-6., -8.]])
      # path = calc_bezier_path(control_points, n_points=100)
  
      # Display the tangent, normal and radius of cruvature at a given point
      t = 0.86  # Number in [0, 1]
      x_target, y_target = bezier(t, control_points)
      derivatives_cp = bezier_derivatives_control_points(control_points, 2)
      point = bezier(t, control_points)
      dt = bezier(t, derivatives_cp[1])
      ddt = bezier(t, derivatives_cp[2])
      # Radius of curvature
      radius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1])
      # Normalize derivative
      dt /= np.linalg.norm(dt, 2)
      tangent = np.array([point, point + dt])
      normal = np.array([point, point + [- dt[1], dt[0]]])
      curvature_center = point + np.array([- dt[1], dt[0]]) * radius
      circle = plt.Circle(tuple(curvature_center), radius,
                          color=(0, 0.8, 0.8), fill=False, linewidth=1)
  
      assert path.T[0][0] == start_x, "path is invalid"
      assert path.T[1][0] == start_y, "path is invalid"
      assert path.T[0][-1] == end_x, "path is invalid"
      assert path.T[1][-1] == end_y, "path is invalid"
  
      if show_animation:  # pragma: no cover
          fig, ax = plt.subplots()
          ax.plot(path.T[0], path.T[1], label="Bezier Path")
          ax.plot(control_points.T[0], control_points.T[1],
                  '--o', label="Control Points")
          ax.plot(x_target, y_target)
          ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent")
          ax.plot(normal[:, 0], normal[:, 1], label="Normal")
          ax.add_artist(circle)
          plot_arrow(start_x, start_y, start_yaw)
          plot_arrow(end_x, end_y, end_yaw)
          ax.legend()
          ax.axis("equal")
          ax.grid(True)
          plt.show()
  
  
  def main2():
      """Show the effect of the offset."""
      start_x = 10.0  # [m]
      start_y = 1.0  # [m]
      start_yaw = np.radians(180.0)  # [rad]
  
      end_x = -0.0  # [m]
      end_y = -3.0  # [m]
      end_yaw = np.radians(-45.0)  # [rad]
  
      for offset in np.arange(1.0, 5.0, 1.0):
          path, control_points = calc_4points_bezier_path(
              start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)
          assert path.T[0][0] == start_x, "path is invalid"
          assert path.T[1][0] == start_y, "path is invalid"
          assert path.T[0][-1] == end_x, "path is invalid"
          assert path.T[1][-1] == end_y, "path is invalid"
  
          if show_animation:  # pragma: no cover
              plt.plot(path.T[0], path.T[1], label="Offset=" + str(offset))
  
      if show_animation:  # pragma: no cover
          plot_arrow(start_x, start_y, start_yaw)
          plot_arrow(end_x, end_y, end_yaw)
          plt.legend()
          plt.axis("equal")
          plt.grid(True)
          plt.show()
  
  
  if __name__ == '__main__':
      main()
      #  main2()
  </code></pre>