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])