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))
Beispiel #2
0
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)
Beispiel #3
0
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])