def straighten_up_theta(theta):
if not numpy.isfinite(theta).all():
raise ValueError('Some values are inf or nan, and I cannot use them.')
theta2 = numpy.ndarray(shape=theta.shape, dtype=theta.dtype)
theta2[0] = theta[0]
for i in range(1, len(theta)):
diff = normalize_pi(theta[i] - theta[i - 1])
theta2[i] = theta2[i - 1] + diff
return theta2
def compute_approach_angle(x, y, theta, radius=1):
assert np.isfinite(x)
assert np.isfinite(y)
assert np.isfinite(theta)
assert np.isfinite(radius)
assert radius > 0
assert x **2 + y**2 < radius**2
# || x + cos(theta) t, y + sin(theta) t ||= 1
# leads to
# t^2 + (2 x cos(theta) + 2 y sin(theta)] + (x^2 + y^2 - radius^2) = 0
poly = [1,
2 * x * np.cos(theta) + 2 * y * np.sin(theta),
x ** 2 + y ** 2 - radius ** 2]
# check we are inside
assert poly[2] < 0
roots = np.roots(poly)
# get the positive solution
t = max(roots)
assert t > 0
px = x + t * np.cos(theta)
py = y + t * np.sin(theta)
# check (just in case)
np.testing.assert_almost_equal(px ** 2 + py ** 2, radius ** 2)
phi = np.arctan2(py, px)
approach_angle = normalize_pi(phi - theta)
return approach_angle
def compute_axis_angle(x, y, theta):
''' x,y: coordinates with respect to center of arena.
Returns the angle in radians. '''
angle = np.arctan2(y, x)
axis_angle = normalize_pi(theta - angle)
return axis_angle
