def joint_distance_in_space(self, joint_a, joint_b):
return np.linalg.norm(joint_to_array(joint_a) - joint_to_array(joint_b))
def joint_line_angle_relative_to_plane(self, joint_a, joint_b, plane_normal):
# http://www.vitutor.com/geometry/distance/line_plane.html
line = joint_to_array(joint_b) - joint_to_array(joint_a)
return np.arcsin(np.dot(line, plane_normal) / (np.linalg.norm(line) * np.linalg.norm(plane_normal))) / np.pi
def joint_angle_relative_to_joint(self, root, appendage_a, appendage_b):
# inverse of cosine law -- translate the vectors to the root by subtracting out the root, then using an inverted dot product forumla isolating for the angle
root = joint_to_array(root)
a = joint_to_array(appendage_a) - root
b = joint_to_array(appendage_b) - root
return np.arccos(np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b))) / np.pi
