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