An autonomous canal boat has lost its satellite fix, but its camera can see two church spires whose positions are on the chart: l1 = (x1, y1) and l2 = (x2, y2), in metres (x to the right, y up). Its compass gives its heading heading (radians, counter-clockwise from +x), and the camera measures the bearing of each spire: the angle from the boat's heading to the direction of the spire, counter-clockwise positive. So spire 1 lies in the world direction heading + b1 as seen from the boat, and spire 2 in the direction heading + b2.
The boat must stand on the line through spire 1 with direction heading + b1, and on the line through spire 2 with direction heading + b2. Where the two lines cross is the boat's position: this is triangulation.
Write fix_from_bearings(heading, l1, b1, l2, b2) that returns the boat's position (x, y). If the two directions are parallel (the boat and both spires on one line, or the bearings inconsistent in that way) the lines do not give a single point: return None. Treat the directions as parallel when the absolute value of the sine of the angle between them is less than 1e-9.
Examples
Input: heading = 0, l1 = (5, 5), b1 = math.pi / 4, l2 = (1, 4), b2 = math.pi / 2
Output: (1.0, 1.0000000000000009)
Explanation: spire 1 is up and to the right at 45 degrees, spire 2 straight up (+y).
The only point with both views is (1, 1).
Input: heading = math.pi / 2, l1 = (5, 5), b1 = -math.pi / 4, l2 = (1, 4), b2 = 0
Output: (1.0, 1.0000000000000009)
Explanation: the same place, but the boat faces +y, so the bearings are a quarter turn smaller.
Input: heading = 1.0, l1 = (0, 10), b1 = 0.3, l2 = (0, 20), b2 = 0.3
Output: None
Constraints
- the two spires are at different places; in the tests, the bearings are consistent (each spire really lies ahead along its line, not behind the boat)
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Turn a bearing measured from the heading into a direction in the world frame
- Find a position as the intersection of two lines through known landmarks
- Recognise when two bearings cannot fix a position