Given that the arm exists in a 2D plane, the joints are hinge joints and not ball and socket joints, and the rotation arc for each joint is +/- maxangle:
Let H = the position of the end effector.
Let S = the position of the "shoulder" joint.
Let Slen = the length from the shoulder to the elbow.
Let Sdef = a normal vector representing the default orientation of S.
Let E = the position of the "elbow".
Let Elen = the length from the elbow to the hand.
Let SHMin = the minimum possible separation of the Shoulder and Hand (this can be pre-calculated).
TO FIND SHMin: Let Sdef = Vec(1, 0) (i.e. yaw set to zero)
- Set the vector SE from S to E: SE = Vec(cos(Smaxang), sin(Smaxang)) * Slen.
- Add SE to S to get E: E = S + SE.
- Add Smaxang to Emaxang to get SEangle: SEangle = Smaxang + Emaxang.
- Set the vector EH from E to H: EH = Vec(cos(SEangle), sin(SEangle)) * Elen.
- Add EH to E to get H: H = E + EH.
- Get the length of SH for Emaxang: SHlenemax = length(H-S).
- Repeat 3-6 for -1 * Emaxang to get SHlenemin.
- SHMin is the lesser of SHlenemin and SHlenemax.
TO DETERMINE IF POINT IS REACHABLE:
- Set H equal to a position in the list.
- Get the distance between S and H: SHlen = length(H-S).
- Early Out: If (SHlen > Slen + Elen), then point is beyond the reach of the arm.
- Early Out: If (SHlen < SHMin), then the point is too close for the arm to reach.
- You now have the length of all three sides of a triangle. Use law of cosines to find angle between S and SH: c^2 = a^2 + b^2 − 2ab cos(C). This will be Elen^2 = Slen^2 + SHlen^2 - 2(Slen)(ElenSHlen)cos(C) which solves to C = ACOS((Slen^2 + SHlen^2 - Elen^2) / (2(Slen)(ElenSHlen)).
- Create a normalized vector representing the direction of SE: VecA = (cos(C), sin(C)). SHnorm = normalize(SH). SEnorm = normalize(VecA + SHnorm).
- Get the angle Sang between SEnorm and Sdef: Sang = ACOS(SEnorm (dot) Sdef).
- Early Out: If (Sang > Sangmax), then arm can't reach the point.
- Get the position of E using SHnorm, Slen, and S: E = S + (SHnorm * Slen).
- Get the normalized vector EHnorm from E to H: EHnorm = normalize(H-E).
- Get the angle Eang between EHnorm and SHnorm: Eang = ACOS(EHnorm (dot) SHnorm).
- Final Out: If (Eang > Eangmax), then arm can't reach the point.
