A robot mows between the trees of an orchard. Each tree trunk is a circle (cx, cy, r): centre and radius in metres (x to the right, y up). The robot's pose is (x, y, theta), theta in radians counter-clockwise from +x. Its laser scanner sends one beam for each angle in angles; each angle is measured from the robot's heading (counter-clockwise positive), so a beam travels from (x, y) along the world direction theta + a.
For each beam, find the nearest point where it meets any trunk's circle, only going forwards. A beam that just touches a circle (a tangent) counts as meeting it. If the nearest such point is at most max_range metres away, the scanner sees it; otherwise the beam has no echo.
Write trunk_scan(pose, angles, trunks, max_range) that returns a list with one entry per beam, in the order of angles: the world point (px, py) where that beam stops, or None for no echo. Collect the points and press Run with plot(xs, ys, kind="scatter") to see the trunks' near sides appear.
Examples
Input: pose = (0, 0, 0), angles = [-0.5, 0, 0.5, math.atan2(2, 2)],
trunks = [(4, 0, 0.5), (2, 2, 0.3), (6, -3, 1.0)], max_range = 10
Output: [(5.032012644512591, -2.749001036622665), (3.5, 0.0), None, (1.787867965644038, 1.7878679656440377)]
Explanation: the beam at angle 0 meets the first trunk on its near side, 3.5 m out.
The beam at -0.5 rad clips the trunk at (6, -3). Nothing lies along +0.5 rad.
The last beam points straight at the centre (2, 2) and stops 0.3 m short of it.
Input: pose = (1, 2, math.pi / 2), angles = [0, -math.pi / 2], trunks = [(1, 5, 1), (4, 2, 1)], max_range = 2
Output: [(1.0000000000000002, 4.0), (3.0, 2.0)]
Explanation: the robot faces +y. Both trunks are exactly 2 m away, which is still in range.
Constraints
- the robot is outside every trunk; every radius is positive
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Find where a ray first meets a circle by solving a quadratic
- Simulate a whole scan: one beam per angle, each relative to the heading
- Turn every range into a point in the world frame, or `None` for no echo