diff --git a/physics/collision_detection.py b/physics/collision_detection.py new file mode 100644 index 000000000000..8e13b76850c1 --- /dev/null +++ b/physics/collision_detection.py @@ -0,0 +1,341 @@ +""" +Collision detection algorithms for 2D geometric shapes. + +Collision detection is a fundamental concept in computational geometry, physics +simulations, and game development. It determines whether two or more geometric +objects intersect or overlap in space. + +This module implements several common 2D collision detection algorithms: +- Axis-Aligned Bounding Box (AABB) collision detection +- Circle-circle collision detection +- Circle-AABB collision detection +- Point-in-rectangle detection +- Point-in-circle detection + +Reference: https://en.wikipedia.org/wiki/Collision_detection +Reference: https://developer.mozilla.org/en-US/docs/Games/Techniques/2D_collision_detection +""" + +from __future__ import annotations + +from math import sqrt + + +def is_aabb_collision( + x1: float, + y1: float, + w1: float, + h1: float, + x2: float, + y2: float, + w2: float, + h2: float, +) -> bool: + """ + Check if two Axis-Aligned Bounding Boxes (AABBs) are colliding. + + Each rectangle is defined by its top-left corner (x, y), width (w), + and height (h). + + >>> is_aabb_collision(0, 0, 10, 10, 5, 5, 10, 10) + True + >>> is_aabb_collision(0, 0, 10, 10, 20, 20, 10, 10) + False + >>> is_aabb_collision(0, 0, 10, 10, 10, 0, 10, 10) + False + >>> is_aabb_collision(0, 0, 5, 5, 3, 3, 5, 5) + True + >>> is_aabb_collision(-5, -5, 10, 10, 0, 0, 10, 10) + True + >>> is_aabb_collision(0, 0, -1, 10, 5, 5, 10, 10) + Traceback (most recent call last): + ... + ValueError: Width and height must be non-negative + >>> is_aabb_collision(0, 0, 10, 10, 5, 5, -1, 10) + Traceback (most recent call last): + ... + ValueError: Width and height must be non-negative + """ + if w1 < 0 or h1 < 0 or w2 < 0 or h2 < 0: + raise ValueError("Width and height must be non-negative") + + return x1 < x2 + w2 and x1 + w1 > x2 and y1 < y2 + h2 and y1 + h1 > y2 + + +def is_circle_collision( + cx1: float, + cy1: float, + r1: float, + cx2: float, + cy2: float, + r2: float, +) -> bool: + """ + Check if two circles are colliding. + + Each circle is defined by its center (cx, cy) and radius (r). + + >>> is_circle_collision(0, 0, 5, 8, 0, 5) + True + >>> is_circle_collision(0, 0, 5, 20, 20, 5) + False + >>> is_circle_collision(0, 0, 10, 5, 5, 10) + True + >>> is_circle_collision(0, 0, 1, 3, 0, 1) + False + >>> is_circle_collision(0, 0, 0, 0, 0, 0) + False + >>> is_circle_collision(0, 0, -1, 5, 5, 3) + Traceback (most recent call last): + ... + ValueError: Radius must be non-negative + """ + if r1 < 0 or r2 < 0: + raise ValueError("Radius must be non-negative") + + distance_squared = (cx2 - cx1) ** 2 + (cy2 - cy1) ** 2 + radius_sum = r1 + r2 + return distance_squared < radius_sum**2 + + +def is_circle_aabb_collision( + cx: float, + cy: float, + radius: float, + rx: float, + ry: float, + rw: float, + rh: float, +) -> bool: + """ + Check if a circle and an Axis-Aligned Bounding Box (AABB) are colliding. + + The circle is defined by its center (cx, cy) and radius. + The rectangle is defined by its top-left corner (rx, ry), width (rw), + and height (rh). + + >>> is_circle_aabb_collision(5, 5, 3, 0, 0, 10, 10) + True + >>> is_circle_aabb_collision(20, 20, 3, 0, 0, 10, 10) + False + >>> is_circle_aabb_collision(12, 5, 3, 0, 0, 10, 10) + True + >>> is_circle_aabb_collision(0, 0, 1, 5, 5, 10, 10) + False + >>> is_circle_aabb_collision(5, 5, -1, 0, 0, 10, 10) + Traceback (most recent call last): + ... + ValueError: Radius must be non-negative + >>> is_circle_aabb_collision(5, 5, 3, 0, 0, -1, 10) + Traceback (most recent call last): + ... + ValueError: Width and height must be non-negative + """ + if radius < 0: + raise ValueError("Radius must be non-negative") + if rw < 0 or rh < 0: + raise ValueError("Width and height must be non-negative") + + closest_x = max(rx, min(cx, rx + rw)) + closest_y = max(ry, min(cy, ry + rh)) + + distance_squared = (cx - closest_x) ** 2 + (cy - closest_y) ** 2 + return distance_squared < radius**2 + + +def is_point_in_rectangle( + px: float, + py: float, + rx: float, + ry: float, + rw: float, + rh: float, +) -> bool: + """ + Check if a point is inside an Axis-Aligned Bounding Box (rectangle). + + The point is defined by (px, py). + The rectangle is defined by its top-left corner (rx, ry), width (rw), + and height (rh). + + >>> is_point_in_rectangle(5, 5, 0, 0, 10, 10) + True + >>> is_point_in_rectangle(15, 15, 0, 0, 10, 10) + False + >>> is_point_in_rectangle(0, 0, 0, 0, 10, 10) + True + >>> is_point_in_rectangle(10, 10, 0, 0, 10, 10) + False + >>> is_point_in_rectangle(-1, 5, 0, 0, 10, 10) + False + >>> is_point_in_rectangle(5, 5, 0, 0, -1, 10) + Traceback (most recent call last): + ... + ValueError: Width and height must be non-negative + """ + if rw < 0 or rh < 0: + raise ValueError("Width and height must be non-negative") + + return rx <= px < rx + rw and ry <= py < ry + rh + + +def is_point_in_circle( + px: float, + py: float, + cx: float, + cy: float, + radius: float, +) -> bool: + """ + Check if a point is inside a circle. + + The point is defined by (px, py). + The circle is defined by its center (cx, cy) and radius. + + >>> is_point_in_circle(3, 4, 0, 0, 10) + True + >>> is_point_in_circle(10, 10, 0, 0, 5) + False + >>> is_point_in_circle(0, 0, 0, 0, 1) + True + >>> is_point_in_circle(5, 0, 0, 0, 5) + False + >>> is_point_in_circle(3, 4, 0, 0, -1) + Traceback (most recent call last): + ... + ValueError: Radius must be non-negative + """ + if radius < 0: + raise ValueError("Radius must be non-negative") + + distance_squared = (px - cx) ** 2 + (py - cy) ** 2 + return distance_squared < radius**2 + + +def detect_all_collisions( + objects: list[dict], +) -> list[tuple[int, int]]: + """ + Detect all pairwise collisions among a list of geometric objects. + + Each object is a dictionary with a 'type' key ('circle' or 'rect') and + the corresponding geometric parameters. + + Circle: {'type': 'circle', 'cx': float, 'cy': float, 'r': float} + Rectangle: {'type': 'rect', 'x': float, 'y': float, 'w': float, 'h': float} + + Returns a list of tuples (i, j) where objects[i] and objects[j] collide. + + >>> objects = [ + ... {'type': 'circle', 'cx': 0, 'cy': 0, 'r': 5}, + ... {'type': 'circle', 'cx': 3, 'cy': 0, 'r': 5}, + ... {'type': 'circle', 'cx': 100, 'cy': 100, 'r': 1}, + ... ] + >>> detect_all_collisions(objects) + [(0, 1)] + >>> objects = [ + ... {'type': 'rect', 'x': 0, 'y': 0, 'w': 10, 'h': 10}, + ... {'type': 'rect', 'x': 5, 'y': 5, 'w': 10, 'h': 10}, + ... {'type': 'circle', 'cx': 20, 'cy': 20, 'r': 3}, + ... ] + >>> detect_all_collisions(objects) + [(0, 1)] + >>> detect_all_collisions([]) + [] + """ + collisions: list[tuple[int, int]] = [] + for i in range(len(objects)): + for j in range(i + 1, len(objects)): + if _check_collision(objects[i], objects[j]): + collisions.append((i, j)) + return collisions + + +def _check_collision(obj1: dict, obj2: dict) -> bool: + """ + Check collision between two geometric objects. + + >>> _check_collision( + ... {'type': 'circle', 'cx': 0, 'cy': 0, 'r': 5}, + ... {'type': 'circle', 'cx': 3, 'cy': 0, 'r': 5}, + ... ) + True + >>> _check_collision( + ... {'type': 'rect', 'x': 0, 'y': 0, 'w': 10, 'h': 10}, + ... {'type': 'rect', 'x': 20, 'y': 20, 'w': 5, 'h': 5}, + ... ) + False + """ + type1, type2 = obj1["type"], obj2["type"] + + if type1 == "circle" and type2 == "circle": + return is_circle_collision( + obj1["cx"], + obj1["cy"], + obj1["r"], + obj2["cx"], + obj2["cy"], + obj2["r"], + ) + + if type1 == "rect" and type2 == "rect": + return is_aabb_collision( + obj1["x"], + obj1["y"], + obj1["w"], + obj1["h"], + obj2["x"], + obj2["y"], + obj2["w"], + obj2["h"], + ) + + if type1 == "circle" and type2 == "rect": + return is_circle_aabb_collision( + obj1["cx"], + obj1["cy"], + obj1["r"], + obj2["x"], + obj2["y"], + obj2["w"], + obj2["h"], + ) + + if type1 == "rect" and type2 == "circle": + return is_circle_aabb_collision( + obj2["cx"], + obj2["cy"], + obj2["r"], + obj1["x"], + obj1["y"], + obj1["w"], + obj1["h"], + ) + + msg = f"Unknown object types: {type1}, {type2}" + raise ValueError(msg) + + +if __name__ == "__main__": + import doctest + + doctest.testmod() + + print("AABB collision:", is_aabb_collision(0, 0, 10, 10, 5, 5, 10, 10)) + print("Circle collision:", is_circle_collision(0, 0, 5, 8, 0, 5)) + print("Point in rect:", is_point_in_rectangle(5, 5, 0, 0, 10, 10)) + print("Point in circle:", is_point_in_circle(3, 4, 0, 0, 10)) + print( + "Circle-AABB collision:", + is_circle_aabb_collision(5, 5, 3, 0, 0, 10, 10), + ) + print( + "Detect all:", + detect_all_collisions( + [ + {"type": "circle", "cx": 0, "cy": 0, "r": 5}, + {"type": "circle", "cx": 3, "cy": 0, "r": 5}, + {"type": "rect", "x": 100, "y": 100, "w": 10, "h": 10}, + ] + ), + )