A museum arm hands visitors leaflets. Its base stands on the floor, at the origin, and a glass wall runs somewhere near it. The arm is planar with n links: link i has length lengths[i] and joint angle angles[i] (radians, counter-clockwise). The angles are relative: angles[0] is measured from the +x axis, every later one from the direction of the link before it. x points right and y up, so the floor is the line y = 0.
Number the arm's points 0 (the base) to n (the hand): point k is the end of link k - 1. Before a pose is used, the controller checks two things:
- below: the points that go below the floor, y < -10^{-9}. Resting exactly on the floor is fine.
- hits: the points that touch or pass the glass. The wall is the whole straight line through
pandq; the base is on one side of it. A point hits the wall when it lies on the other side, or on the line itself, or less than 10^{-9} from it.
Write check_arm(lengths, angles, p, q) that returns a tuple (below, hits) of two lists of point numbers, each in increasing order.
Examples
Input: lengths = [1, 1, 1], angles = [math.pi / 4, -math.pi / 2, -math.pi / 2],
p = (1.2, -1), q = (1.2, 3)
Output: ([3], [2])
Explanation: the points are (0, 0), (0.71, 0.71), (1.41, 0) and (0.71, -0.71).
Point 2 is past the wall x = 1.2; point 3 is below the floor. Point 2's height
comes out as about 1e-16, which counts as on the floor, not below it.
Input: lengths = [1, 1], angles = [math.pi / 2, -math.pi / 2], p = (2, 0), q = (0, 2)
Output: ([], [2])
Explanation: the hand is at (1, 1), exactly on the wall line x + y = 2: a touch is a hit.
Input: lengths = [0.5, 0.5, 0.4], angles = [1.0, 0.6, -2.0], p = (-1, 1.5), q = (3, 1.2)
Output: ([], [])
Constraints
pandqare different points, and the base is more than1e-6from the wall line
Goals
- Compute every joint of an arm and test each one against two boundaries
- Decide which side of a line a point lies on with a cross product
- Apply a small tolerance consistently at a boundary