def visualize(self, points=[], vectors=[]):
for local, ref_frame, vis_id in points:
if len(local) == 3:
# for points
pose = dot(ref_frame, homo_matrix(trans=local))
else:
# for frames
pose = dot(ref_frame, local)
self.view_objects.send_prop(vis_id, "pose",
pose.reshape(16).tolist())
for single_vector in vectors:
vector_local = single_vector[0]
ref_frame_vector = single_vector[1]
point_local = single_vector[2]
ref_frame_point = single_vector[3]
vis_id = single_vector[4]
point = dot(ref_frame_point, homo_matrix(trans=point_local))
self.view_objects.send_prop(vis_id, "pose",
point.reshape(16).tolist())
vector = dot(ref_frame_vector[:3, :3], vector_local)
self.view_objects.send_prop(vis_id, "axis",
0.5 * vector / norm(vector))
def most_far_away_face_from_point(crossing_faces_local, crossing_faces_reference_frame, point):
dist=-1
final_face=-1
for face in crossing_faces_local:
touch_point_local=crossing_faces_local[face][2][1]
touch_point=dot(crossing_faces_reference_frame,homo_matrix(trans=touch_point_local))[:3,3]
print "Touch point", face, touch_point
cur_dist=norm(touch_point-point)
if dist<0.:
dist=cur_dist
final_touch_point=touch_point
final_face=face
else:
if cur_dist>dist:
dist=cur_dist
final_touch_point=touch_point
final_face=face
return(final_touch_point,final_face)
def calc_finger_start_pos(finger_offset, touch_point, object_pos, touch_face, box_planes):
touch_point_local=dot(inv(object_pos), homo_matrix(trans=touch_point))[:3,3]
print "touch local", touch_point_local, "plane", box_planes[touch_face][0]
return(dot(object_pos,homo_matrix(trans=touch_point_local+array(box_planes[touch_face][0])*finger_offset))[:3,3])