A robot in a workshop carries a single-beam lidar. The workshop map is a list of walls, each a straight segment ((ax, ay), (bx, by)) between two end points, in metres (x to the right, y up). The robot's pose is (x, y, theta), theta in radians counter-clockwise from +x, and the beam leaves the robot's position along (\cos\theta, \sin\theta).
The lidar reports the distance to the first wall the beam meets, under these rules:
- The beam only travels forwards (distances of 0 or more).
- A wall is hit anywhere between its two end points, end points included.
- A wall parallel to the beam is never hit, even when the beam runs exactly along it: the beam grazes it.
- If nothing is hit within
max_rangemetres, the lidar reportsmax_range.
Write beam_range(pose, walls, max_range) that returns the reported distance as a float.
Examples
walls = [((4, 0), (4, 3)), ((2, 2), (3, 0.5)), ((0, 5), (6, 5))]
Input: pose = (1, 1, 0), walls, max_range = 10
Output: 1.6666666666666667
Explanation: the slanted wall crosses y = 1 two thirds of the way from (2, 2) to (3, 0.5),
at x = 2.667, before the wall at x = 4.
Input: pose = (1, 1, math.pi / 2), walls, max_range = 3
Output: 3.0
Explanation: straight up, the beam would meet y = 5 after 4 m, beyond the 3 m range.
Input: pose = (0, 1, 0), walls = [((2, 1), (5, 1)), ((7, 0), (7, 4))], max_range = 10
Output: 7.0
Explanation: the first wall lies along the beam, so the beam grazes it and goes on to x = 7.
Constraints
- the robot does not stand on a wall; every wall has two different end points
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Intersect a ray with a line segment by solving two linear equations
- Keep only hits ahead of the robot and within the segment's ends
- Take the nearest hit over many walls, capped at the maximum range