From d7cc2e86c32aef876e8b7f35d341b6d78a974861 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 25 May 2025 14:36:45 +0900 Subject: [PATCH] Add utility module for collision detection Introduced `collision_detection.py` with helper functions and enums for calculating point orientation to line segments. This module facilitates collision detection tasks and enhances modularity in the codebase. --- utils/collision_detection.py | 39 ++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 utils/collision_detection.py diff --git a/utils/collision_detection.py b/utils/collision_detection.py new file mode 100644 index 0000000000..4e06af87fe --- /dev/null +++ b/utils/collision_detection.py @@ -0,0 +1,39 @@ +""" + This module contains utility functions for collision detection +""" + +from enum import Enum +import numpy as np + + +class PointPosition(Enum): + LEFT = -1 + ON_LINE = 0 + RIGHT = 1 + + +def cal_point_orientation_to_line(point, line_start, line_end): + """ + Calculate the orientation of a point with respect to a line segment. + + Args: + point (tuple): The point to check (x, y). + line_start (tuple): The start point of the line segment (x1, y1). + line_end (tuple): The end point of the line segment (x2, y2). + + Returns: + PointPosition: The position of the point relative to the line segment. + """ + x1, y1 = line_start + x2, y2 = line_end + x0, y0 = point + + # Calculate the cross-product + cross_product = (x2 - x1) * (y0 - y1) - (y2 - y1) * (x0 - x1) + + if cross_product > 0: + return PointPosition.LEFT + elif cross_product < 0: + return PointPosition.RIGHT + else: + return PointPosition.ON_LINE