Ellipse

src.python_motion_planning.global_planner.sample_search.informed_rrt.Ellipse

Ellipse sampling.

Source code in src\python_motion_planning\global_planner\sample_search\informed_rrt.py

Python

class Ellipse:
    """
    Ellipse sampling.
    """
    @staticmethod
    def transform(a: float, c: float, p1: tuple, p2: tuple) -> np.ndarray:
        # center
        center_x = (p1[0] + p2[0]) / 2
        center_y = (p1[1] + p2[1]) / 2

        # rotation
        theta = - np.arctan2(p2[1] - p1[1], p2[0] - p1[0])

        # transform
        b = np.sqrt(a ** 2 - c ** 2)
        T = np.array([[ a * np.cos(theta), b * np.sin(theta), center_x],
                      [-a * np.sin(theta), b * np.cos(theta), center_y],
                      [                 0,                 0,        1]])
        return T