Class Rectangle

limsim/evaluation/collision_statistics.py:83–141

A simple rectangle representation length w ---------------------------- i - - d - center - t - - h ---------------------------- yaw is in radians and in anti-clockwise direction

@dataclass
class Rectangle:
    """A simple rectangle representation

                  length
    w ----------------------------
    i -                          -
    d -           center         -
    t -                          -
    h ----------------------------
    yaw is in radians and in anti-clockwise direction
    """
    center: np.ndarray
    width: float
    length: float
    yaw: float

    def __repr__(self) -> str:
        return f"center={self.center}, width={self.width}, length={self.length}, yaw={self.yaw}"

    def corners(self) -> np.ndarray:
        """Get the coordinates of corners of a rectangle

        Returns:
            List[np.ndarray]: an array of shape (4, 2)
            each of them represents the coordinate of a corner
        """
        points = np.array([[self.length, self.width], [
            -self.length, self.width
        ], [-self.length, -self.width], [self.length, -self.width]]) / 2
        rotation = np.array([[np.cos(self.yaw), -np.sin(self.yaw)],
                             [np.sin(self.yaw),
                              np.cos(self.yaw)]])
        return self.center + np.array(
            [rotation.dot(point) for point in points])

    def in_collision(self, other_rectangle: 'Rectangle') -> bool:
        """check if two rectangles intersects (in collision)

        Args:
            other_rectangle (Rectangle): another rectangle

        Returns:
            bool: True if two rectangle intersects, False otherwise
        """
        self_corners = self.corners()
        other_corners = other_rectangle.corners()

        # first use AABB to filter impossible cases
        self_x_max, self_y_max = np.max(self_corners, axis=0)
        self_x_min, self_y_min = np.min(self_corners, axis=0)
        other_x_max, other_y_max = np.max(other_corners, axis=0)
        other_x_min, other_y_min = np.min(other_corners, axis=0)

        if self_x_max < other_x_min or self_x_min > other_x_max or \
           self_y_max < other_y_min or self_y_min > other_y_max:
            return False

        # use separate axis theorem check in-collision or not