chh3213 / chhrobotics Goto Github PK

def find_near_nodes(self, new_node):
    """
    1) defines a ball centered on new_node
    2) Returns all nodes of the three that are inside this ball
        Arguments:
        ---------
            new_node: Node
                new randomly generated node, without collisions between
                its nearest node
        Returns:
        -------
            list
                List with the indices of the nodes inside the ball of
                radius r
    """
    nnode = len(self.node_list) + 1
    r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))