def find_twist_frame(hexapod, unit_coxia_vector):
twist = angle_between(unit_coxia_vector, hexapod.x_axis)
is_ccw = is_counter_clockwise(unit_coxia_vector, hexapod.x_axis,
hexapod.z_axis)
if is_ccw:
twist = -twist
twist_frame = rotz(twist)
return twist, twist_frame
def find_twist_to_recompute_hexapod(a, b):
twist = angle_between(a, b)
z_axis = Point(0, 0, -1)
is_ccw = is_counter_clockwise(a, b, z_axis)
if is_ccw:
twist = -twist
twist_frame = rotz(twist)
return twist, twist_frame
def _twist(v1, v2):
# https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/
theta = atan2(v2.y, v2.x) - atan2(v1.y, v1.x)
return rotz(degrees(theta))