Collision Models
This file contains all the numba just-in-time compiled functions for collision checking between agents. The GJK algorithm (more detail here: https://cse442-17f.github.io/Gilbert-Johnson-Keerthi-Distance-Algorithm/) is used to check for overlap in polygons.
- module collision_models
Functions
- perpendicular(pt)
Return a 2-vector's perpendicular vector Args: pt (np.ndarray, (2,)): input vector Returns: pt (np.ndarray, (2,)): perpendicular vector
- tripleProduct(a, b, c)
Return triple product of three vectors Args: a, b, c (np.ndarray, (2,)): input vectors Returns: (np.ndarray, (2,)): triple product
- avgPoint(vertices)
Return the average point of multiple vertices Args: vertices (np.ndarray, (n, 2)): the vertices we want to find avg on Returns: avg (np.ndarray, (2,)): average point of the vertices
- indexOfFurthestPoint(vertices, d)
Return the index of the vertex furthest away along a direction in the list of vertices Args: vertices (np.ndarray, (n, 2)): the vertices we want to find avg on Returns: idx (int): index of the furthest point
- support(vertices1, vertices2, d)
Minkowski sum support function for GJK Args: vertices1 (np.ndarray, (n, 2)): vertices of the first body vertices2 (np.ndarray, (n, 2)): vertices of the second body d (np.ndarray, (2, )): direction to find the support along Returns: support (np.ndarray, (n, 2)): Minkowski sum
- collision(vertices1, vertices2)
GJK test to see whether two bodies overlap Args: vertices1 (np.ndarray, (n, 2)): vertices of the first body vertices2 (np.ndarray, (n, 2)): vertices of the second body Returns: overlap (boolean): True if two bodies collide
- collision_multiple(vertices)
Check pair-wise collisions for all provided vertices Args: vertices (np.ndarray (num_bodies, 4, 2)): all vertices for checking pair-wise collision Returns: collisions (np.ndarray (num_vertices, )): whether each body is in collision collision_idx (np.ndarray (num_vertices, )): which index of other body is each index's body is in collision, -1 if not in collision
- get_trmtx(pose)
Get transformation matrix of vehicle frame -> global frame Args: pose (np.ndarray (3, )): current pose of the vehicle return: H (np.ndarray (4, 4)): transformation matrix
- get_vertices(pose, length, width)
Utility function to return vertices of the car body given pose and size Args: pose (np.ndarray, (3, )): current world coordinate pose of the vehicle length (float): car length width (float): car width Returns: vertices (np.ndarray, (4, 2)): corner vertices of the vehicle body
Variables
- cache