"""
Path Planning with 4 point Beizer curve

Author: Atsushi Sakai(@Atsushi_twi)
From https://github.com/AtsushiSakai/PythonRobotics
"""
import scipy.special
import numpy as np

from constants import MAX_WIDTH, MAX_HEIGHT


def computeControlPoints(x, y, add_current_pos=True):
    """
    The image processing predicts (x, y) points belonging to the line
    :param x: (numpy array)
    :param y: (numpy array)
    :param add_current_pos: (bool)
    """
    control_points = np.concatenate((x[None].T, y[None].T), axis=1)

    if add_current_pos:
        current_position = np.array([MAX_WIDTH // 2, MAX_HEIGHT]).reshape(1, -1)
        control_points = np.concatenate((current_position, control_points))

    return control_points


def calcBezierPath(control_points, n_points=100):
    """
    :param control_points: (numpy array)
    :param n_points: (int) n_points in the trajectory
    """
    traj = []
    for t in np.linspace(0, 1, n_points):
        traj.append(bezier(t, control_points))

    return np.array(traj)


def bernsteinPoly(n, i, t):
    """
    Bernstein polynom
    :param n: (int) polynom degree
    :param i: (int)
    :param t: (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)
    :param control_points: (numpy array)
    """
    n = len(control_points) - 1
    return np.sum([bernsteinPoly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)

@@ -20,20 +20,12 @@
 ith 
-4 point Beiz
+Bezi
 er c
@@ -30,16 +30,25 @@
  curve%0A%0A
+Original 
 Author: 
@@ -441,16 +441,43 @@
  (bool)%0A
+    :return: (numpy array)%0A
     %22%22%22%0A
@@ -809,24 +809,82 @@
 0):%0A    %22%22%22%0A
+    Compute bezier path (trajectory) given control points%0A
     :param c
@@ -966,16 +966,43 @@
 jectory%0A
+    :return: (numpy array)%0A
     %22%22%22%0A
@@ -1264,24 +1264,45 @@
  t: (float)%0A
+    :return: (float)%0A
     %22%22%22%0A    
@@ -1500,32 +1500,59 @@
 : (numpy array)%0A
+    :return: (numpy array)%0A
     %22%22%22%0A    n = 

Add comments