utilities.py 8.28 KB
Newer Older
Aravind Bk's avatar
Aravind Bk committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37
from .constants import *
from . import road_geokinemetry as rd
import numpy as np

# Add classes for each kind of utility functions
# Available utilities:
#  1) BoundingBox: for working with bounding boxes


# @brief:
class BoundingBox(object):
    """A class that provides utility functions related to bounding boxes.

    * Bounding box: ndarray of shape (4,2) containing the vertices of the
        rectangle in order
    """

    @staticmethod
    def does_bounding_box_intersect(first_bb, second_bb):
        """Checks if bounding boxes intersect using separating axis test: Two
        objects don't intersect if you can find a line that separates the two
        objects. Works only for quadrilateral bounding boxes (in 2D).

        Args:
            first_bb:  np.ndarray First bounding box
            second_bb: np.ndarray Second bounding box

        Returns:
            true if there is an intersection
        """

        for i, vertex in enumerate(first_bb):
            # for each vertex, get line joining it and next vertex
            line = np.array([vertex, first_bb[(i + 1) % 4]])

            # check side of other 2 vertices in first_bb
            side_of_first_bb = BoundingBox.localize_point_wrt_line(
Ashish Gaurav's avatar
Ashish Gaurav committed
38 39 40
                line,
                first_bb[(i + 2) % 4]) + BoundingBox.localize_point_wrt_line(
                    line, first_bb[(i + 3) % 4])
Aravind Bk's avatar
Aravind Bk committed
41 42 43 44 45

            # Find side of all points in second_bb
            side_of_second_bb = []
            for j in range(second_bb.shape[0]):
                side_of_second_bb.append(
Ashish Gaurav's avatar
Ashish Gaurav committed
46
                    BoundingBox.localize_point_wrt_line(line, second_bb[j, :]))
Aravind Bk's avatar
Aravind Bk committed
47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203

            # Check if all points in second_bb are on same side of line
            # and that other vertices in first_bb are on the opposite side
            if BoundingBox.all_same_sign_in_list(side_of_second_bb) and (
                    side_of_first_bb * sum(side_of_second_bb) < 0):
                return False

        return True

    # TODO: This method was never used in any other places. Remove or keep it?
    @staticmethod
    def does_bounding_box_cross_line(line, bb):
        """Checks if bounding box crosses a line. Line is represented by an
        np.ndarray of size (2,2) representing two points in it. Works only for
        quadrilateral bounding boxes (in 2D).

        Args:
            line:  np.ndarray Two points representing the line
            bb: np.ndarray Bounding box

        Returns:
            true if the bounding box crosses the line
        """

        side_of_vertices = []
        for i, vertex in enumerate(bb):
            side_of_vertices.append(
                BoundingBox.localize_point_wrt_line(line, vertex))

        if BoundingBox.all_same_sign_in_list(side_of_vertices):
            return False
        else:
            return True

    @staticmethod
    def localize_bounding_box_wrt_line(line, bb):
        """Returns the side in which a bounding box is w.r.t a line. Line is
        represented by an np.ndarray of size (2,2) representing two points in
        it. Works only for quadrilateral bounding boxes (in 2D).

        Args:
            line:  np.ndarray Two points representing the line
            bb: np.ndarray Bounding box

        Returns:
            1 or -1 depending upon the side w.r.t the line. returns 0 if it
            intersects the line
        """

        side_of_vertices = []
        for i, vertex in enumerate(bb):
            side_of_vertices.append(
                BoundingBox.localize_point_wrt_line(line, vertex))

        if BoundingBox.all_same_sign_in_list(side_of_vertices):
            for side in side_of_vertices:
                # side_of_vertices can contain 0 if a vertex touches the line
                if side != 0:
                    return side
        else:
            return 0

    @staticmethod
    def localize_point_wrt_line(line, testpoint):
        """Check the side in which a point lies w.r.t a line.

        Args:
            line: np.ndarray Two points representing the line
            testpoint: the point to be checked

        Returns:
            1 or -1 depending upon the side w.r.t the edge;
            returns 0 if it is on the line
        """

        edge = line[0, :] - line[1, :]
        vertex_in_edge = line[0, :]

        # find perpendicular to line
        rotated_edge = np.array([-edge[1], edge[0]])

        # return -1 or 1 depening on the side. 0 if on the line
        return np.sign(rotated_edge[0] * (testpoint[0] - vertex_in_edge[0]) +
                       rotated_edge[1] * (testpoint[1] - vertex_in_edge[1]))

    @staticmethod
    def all_same_sign_in_list(test_list):
        """Check if all elements in a list are of the same sign. Zero does not
        have a sign.

        Args:
            test_list: the python list to be checked

        Returns:
            1 or -1 depending upon the side w.r.t the edge
        """

        return all(k >= 0 for k in test_list) or all(k <= 0 for k in test_list)


# Methods (get_APs, get_veh_attributes, config_Pyglet,
# road2image, draw_all_shapes) related to Pyglet for graphical output.


def get_APs(env, index, *args):
    """Retrieve atomic proposition data from the env.

    Args:
        env: SimpleIntersectionEnv object
        index: vehicle index
        args: atomic proposition names (variable arguments)

    Returns:
        multiline string with the required data
    """

    properties = []
    for arg in args:
        properties.append('%s: %s' % (arg, env.vehs[index].APs[arg]))

    return "\n" + "\n".join(properties) + "\n"


# TODO: env and index info are not used for now. Remove or implement it.
def get_veh_attributes(env, index, *args):
    """Retrieve vehicle attributes from env.

    Args:
        env: SimpleIntersectionEnv object
        index: vehicle index
        args: atomic proposition names (variable arguments)

    Returns:
        multiline string with the required data
    """

    attributes = []
    for arg in args:
        attributes.append(
            "%s: %s" % (arg, round(eval("env.vehs[index].%s" % arg), 2)))

    return "\n" + "\n".join(attributes) + "\n"


def config_Pyglet(pyglet, win_x, win_y):
    """Configure pyglet and return the window that can be used by other pyglet
    objects.

    Args:
        win_x: x width of the window
        win_y: y width of the window

    Returns:
        Pyglet window object
    """

    pyglet.options['debug_gl'] = False
204 205 206 207 208 209 210 211

    # Use the no arguments version, if you don't have a GPU. Uncomment the
    # line below then.
    # config = pyglet.gl.Config()

    # Otherwise use the following line.
    config = pyglet.gl.Config(sample_buffers=1, samples=4)

Aravind Bk's avatar
Aravind Bk committed
212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248
    window = pyglet.window.Window(width=win_x, height=win_y, config=config)
    return window


def road2image(pos, which_dir):
    """Transform a 1D point to the 1D point in the graphics coordinate for the
    same direction ('h' or 'v').

    Args:
        pos: 1D position in x or y-axis of the road coordinate system
        which_dir: the direction of the coordinate. It has to be either
            horizontal ('h') or vertical ('v').

    Returns:
        the corresponding 1D position in the graphics coordinate.
    """
    if which_dir is 'h':
        pos_out = (pos - rd.hlanes.start_pos) * H_SPACE_SCALE
    elif which_dir is 'v':
        pos_out = (pos - rd.vlanes.start_pos) * V_SPACE_SCALE
        pos_out = NUM_VPIXELS - pos_out
    else:
        raise AssertionError

    return pos_out


def draw_all_shapes(shapes):
    """Go through all the shapes and call their draw function.

    Args:
        shapes: list of shapes
    """

    for shape in shapes:
        shape.draw()

Ashish Gaurav's avatar
Ashish Gaurav committed
249

Aravind Bk's avatar
Aravind Bk committed
250 251 252 253 254
# Methods (calculate_v_max, calculate_s)
# used in generate_scenario of SimpleIntersectionEnv


def calculate_v_max(dist):
Ashish Gaurav's avatar
Ashish Gaurav committed
255 256
    """Calculate the maximum velocity you can reach at the given position
    ahead.
Aravind Bk's avatar
Aravind Bk committed
257 258 259 260 261 262 263 264 265 266 267

    Args:
        dist: the distance you travel from the current position.

    Returns:
        the maximum reachable velocity.
    """
    return np.sqrt(2.0 * MAX_ACCELERATION * max(dist, 0))


def calculate_s(v):
Ashish Gaurav's avatar
Ashish Gaurav committed
268 269
    """Calculate the distance traveling when the vehicle is maximally de-
    accelerating for a complete stop.
Aravind Bk's avatar
Aravind Bk committed
270 271 272 273 274 275 276 277 278

    Args:
        v: the current speed of the vehicle.

    Returns:
        the distance traveling when the vehicle is under the maximum
        de-acceleration to stop completely.
    """
    return pow(v, 2) / MAX_ACCELERATION / 2.0