(alpha, beta, d)
| 80 | |
| 81 | |
| 82 | def RSL(alpha, beta, d): |
| 83 | sa = math.sin(alpha) |
| 84 | sb = math.sin(beta) |
| 85 | ca = math.cos(alpha) |
| 86 | cb = math.cos(beta) |
| 87 | c_ab = math.cos(alpha - beta) |
| 88 | |
| 89 | p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb)) |
| 90 | mode = ["R", "S", "L"] |
| 91 | if p_squared < 0: |
| 92 | return None, None, None, mode |
| 93 | p = math.sqrt(p_squared) |
| 94 | tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p) |
| 95 | t = mod2pi(alpha - tmp2) |
| 96 | q = mod2pi(beta - tmp2) |
| 97 | |
| 98 | return t, p, q, mode |
| 99 | |
| 100 | |
| 101 | def RLR(alpha, beta, d): |