A museum guide robot plans its next move to an exhibit in two parts: turn on the spot by the bearing, then drive straight for the range.
- The range is the straight-line distance from the robot's centre to the target, in metres.
- The bearing is the angle from the robot's forward direction to the direction of the target, in radians, wrapped to (-\pi, \pi]:
0is dead ahead, positive is to the robot's left (counter-clockwise), negative to its right.
The robot's pose is (x, y, theta): its centre in world coordinates (x to the right, y up) and the angle theta from the world's +x axis to its forward direction, counter-clockwise, in radians.
Write range_bearing(pose, targets) that returns, for each target (x, y) in world coordinates, the pair (range, bearing), in the same order. A target exactly at the robot's centre has no direction: return (0.0, 0.0) for it.
Examples
Input: pose = (1, 1, 1.5707963267948966), targets = [(1, 4), (4, 1), (0, 0), (1, 1)]
Output: [(3.0, 0.0), (3.0, -1.5707963267948966), (1.4142135623730951, 2.356194490192345), (0.0, 0.0)]
Explanation: the robot faces +y. (1, 4) is 3 m dead ahead; (4, 1) is 3 m to its right (-pi/2).
(0, 0) is behind it and to its left: a left turn of 3*pi/4 points at it.
Input: pose = (0, 0, 3.0), targets = [(-1, -0.5), (1, 0.2)]
Output: [(1.118033988749895, 0.6052402625905993), (1.019803902718557, -2.802604440150119)]
Explanation: facing almost along -x, the point (-1, -0.5) is ahead and to the left; (1, 0.2) is
behind, and the right turn of 2.80 radians is shorter than the left turn of 3.48.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Measure the straight-line distance from the robot to a target
- Find the target's direction relative to where the robot faces, with `math.atan2`
- Wrap a relative angle so that left is positive and right is negative