def draw_heading_bug(self): color = medium_orchid size = 2 a = math.atan2(self.ve, self.vn) q0 = transformations.quaternion_about_axis(self.ap_hdg * d2r, [0.0, 0.0, -1.0]) center = self.ladder_helper(q0, 0, 0) pts = [] pts.append(self.ladder_helper(q0, 0, 2.0)) pts.append(self.ladder_helper(q0, 0.0, -2.0)) pts.append(self.ladder_helper(q0, 1.5, -2.0)) pts.append(self.ladder_helper(q0, 1.5, -1.0)) pts.append(center) pts.append(self.ladder_helper(q0, 1.5, 1.0)) pts.append(self.ladder_helper(q0, 1.5, 2.0)) for i, p in enumerate(pts): if p == None or center == None: return cv2.line(self.frame, pts[0], pts[1], color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, pts[1], pts[2], color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, pts[2], pts[3], color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, pts[3], pts[4], color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, pts[4], pts[5], color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, pts[5], pts[6], color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, pts[6], pts[0], color, self.line_width, cv2.LINE_AA)
def draw_vbars(self): color = medium_orchid size = self.line_width a1 = 10.0 a2 = 1.5 a3 = 3.0 q0 = transformations.quaternion_about_axis(self.psi_rad, [0.0, 0.0, -1.0]) a0 = self.ap_pitch # rotation point (about nose) rot = self.ladder_helper(q0, self.the_rad * r2d, 0.0) if rot == None: return # center point tmp1 = self.ladder_helper(q0, a0, 0.0) if tmp1 == None: return center = self.rotate_pt(tmp1, rot, self.ap_roll * d2r) # right vbar tmp1 = self.ladder_helper(q0, a0 - a3, a1) tmp2 = self.ladder_helper(q0, a0 - a3, a1 + a3) tmp3 = self.ladder_helper(q0, a0 - a2, a1 + a3) if tmp1 != None and tmp2 != None and tmp3 != None: uv1 = self.rotate_pt(tmp1, rot, self.ap_roll * d2r) uv2 = self.rotate_pt(tmp2, rot, self.ap_roll * d2r) uv3 = self.rotate_pt(tmp3, rot, self.ap_roll * d2r) if uv1 != None and uv2 != None and uv3 != None: cv2.line(self.frame, center, uv1, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, center, uv3, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv1, uv2, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv1, uv3, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv2, uv3, color, self.line_width, cv2.LINE_AA) # left vbar tmp1 = self.ladder_helper(q0, a0 - a3, -a1) tmp2 = self.ladder_helper(q0, a0 - a3, -a1 - a3) tmp3 = self.ladder_helper(q0, a0 - a2, -a1 - a3) if tmp1 != None and tmp2 != None and tmp3 != None: uv1 = self.rotate_pt(tmp1, rot, self.ap_roll * d2r) uv2 = self.rotate_pt(tmp2, rot, self.ap_roll * d2r) uv3 = self.rotate_pt(tmp3, rot, self.ap_roll * d2r) if uv1 != None and uv2 != None and uv3 != None: cv2.line(self.frame, center, uv1, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, center, uv3, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv1, uv2, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv1, uv3, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv2, uv3, color, self.line_width, cv2.LINE_AA)
def draw_alpha_beta_marker(self): if self.alpha_rad == None or self.beta_rad == None: return q0 = transformations.quaternion_about_axis(self.psi_rad, [0.0, 0.0, -1.0]) a0 = self.the_rad * r2d center = self.ladder_helper(q0, a0, 0.0) alpha = self.alpha_rad * r2d beta = self.beta_rad * r2d tmp = self.ladder_helper(q0, a0 - alpha, beta) if tmp != None: uv = self.rotate_pt(tmp, center, self.phi_rad) if uv != None: r1 = int(round(self.render_h / 60)) r2 = int(round(self.render_h / 30)) uv1 = (uv[0] + r1, uv[1]) uv2 = (uv[0] + r2, uv[1]) uv3 = (uv[0] - r1, uv[1]) uv4 = (uv[0] - r2, uv[1]) uv5 = (uv[0], uv[1] - r1) uv6 = (uv[0], uv[1] - r2) cv2.circle(self.frame, uv, r1, self.color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv1, uv2, self.color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv3, uv4, self.color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv5, uv6, self.color, self.line_width, cv2.LINE_AA)
def draw_course(self): color = yellow size = 2 self.filter_vn = (1.0 - self.tf_vel) * self.filter_vn + self.tf_vel * self.vn self.filter_ve = (1.0 - self.tf_vel) * self.filter_ve + self.tf_vel * self.ve a = math.atan2(self.filter_ve, self.filter_vn) q0 = transformations.quaternion_about_axis(a, [0.0, 0.0, -1.0]) uv1 = self.ladder_helper(q0, 0, 0) uv2 = self.ladder_helper(q0, 1.5, 1.0) uv3 = self.ladder_helper(q0, 1.5, -1.0) if uv1 != None and uv2 != None and uv3 != None: #uv2 = self.rotate_pt(tmp2, tmp1, -self.cam_roll*d2r) #uv3 = self.rotate_pt(tmp3, tmp1, -self.cam_roll*d2r) cv2.line(self.frame, uv1, uv2, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv1, uv3, color, self.line_width, cv2.LINE_AA)
def draw_bird(self): color = yellow size = 2 a1 = 10.0 a2 = 3.0 q0 = transformations.quaternion_about_axis(self.psi_rad, [0.0, 0.0, -1.0]) a0 = self.the_rad * r2d # print 'pitch:', a0, 'ap:', self.ap_pitch # center point center = self.ladder_helper(q0, a0, 0.0) if center == None: return # right vbar tmp1 = self.ladder_helper(q0, a0 - a2, a1) tmp2 = self.ladder_helper(q0, a0 - a2, a1 - a2) if tmp1 != None and tmp2 != None: uv1 = self.rotate_pt(tmp1, center, self.phi_rad) uv2 = self.rotate_pt(tmp2, center, self.phi_rad) if uv1 != None and uv2 != None: cv2.line(self.frame, center, uv1, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, center, uv2, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv1, uv2, color, self.line_width, cv2.LINE_AA) # left vbar tmp1 = self.ladder_helper(q0, a0 - a2, -a1) tmp2 = self.ladder_helper(q0, a0 - a2, -a1 + a2) if tmp1 != None and tmp2 != None: uv1 = self.rotate_pt(tmp1, center, self.phi_rad) uv2 = self.rotate_pt(tmp2, center, self.phi_rad) if uv1 != None and uv2 != None: cv2.line(self.frame, center, uv1, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, center, uv2, color, self.line_width, cv2.LINE_AA) cv2.line(self.frame, uv1, uv2, color, self.line_width, cv2.LINE_AA)
def draw_pitch_ladder(self, beta_rad=0.0): a1 = 2.0 a2 = 8.0 #slide_rad = self.psi_rad - beta_rad slide_rad = self.psi_rad q0 = transformations.quaternion_about_axis(slide_rad, [0.0, 0.0, -1.0]) for a0 in range(5, 35, 5): # above horizon # right horizontal uv1 = self.ladder_helper(q0, a0, a1) uv2 = self.ladder_helper(q0, a0, a2) if uv1 != None and uv2 != None: cv2.line(self.frame, uv1, uv2, self.color, self.line_width, cv2.LINE_AA) du = uv2[0] - uv1[0] dv = uv2[1] - uv1[1] uv = (uv1[0] + int(1.25 * du), uv1[1] + int(1.25 * dv)) self.draw_label("%d" % a0, uv, self.font_size, self.line_width) # right tick uv1 = self.ladder_helper(q0, a0 - 0.5, a1) uv2 = self.ladder_helper(q0, a0, a1) if uv1 != None and uv2 != None: cv2.line(self.frame, uv1, uv2, self.color, self.line_width, cv2.LINE_AA) # left horizontal uv1 = self.ladder_helper(q0, a0, -a1) uv2 = self.ladder_helper(q0, a0, -a2) if uv1 != None and uv2 != None: cv2.line(self.frame, uv1, uv2, self.color, self.line_width, cv2.LINE_AA) du = uv2[0] - uv1[0] dv = uv2[1] - uv1[1] uv = (uv1[0] + int(1.25 * du), uv1[1] + int(1.25 * dv)) self.draw_label("%d" % a0, uv, self.font_size, self.line_width) # left tick uv1 = self.ladder_helper(q0, a0 - 0.5, -a1) uv2 = self.ladder_helper(q0, a0, -a1) if uv1 != None and uv2 != None: cv2.line(self.frame, uv1, uv2, self.color, self.line_width, cv2.LINE_AA) # below horizon # right horizontal uv1 = self.ladder_helper(q0, -a0, a1) uv2 = self.ladder_helper(q0, -a0 - 0.5, a2) if uv1 != None and uv2 != None: du = uv2[0] - uv1[0] dv = uv2[1] - uv1[1] for i in range(0, 3): tmp1 = (uv1[0] + int(0.375 * i * du), uv1[1] + int(0.375 * i * dv)) tmp2 = (tmp1[0] + int(0.25 * du), tmp1[1] + int(0.25 * dv)) cv2.line(self.frame, tmp1, tmp2, self.color, self.line_width, cv2.LINE_AA) uv = (uv1[0] + int(1.25 * du), uv1[1] + int(1.25 * dv)) self.draw_label("%d" % a0, uv, self.font_size, self.line_width) # right tick uv1 = self.ladder_helper(q0, -a0 + 0.5, a1) uv2 = self.ladder_helper(q0, -a0, a1) if uv1 != None and uv2 != None: cv2.line(self.frame, uv1, uv2, self.color, self.line_width, cv2.LINE_AA) # left horizontal uv1 = self.ladder_helper(q0, -a0, -a1) uv2 = self.ladder_helper(q0, -a0 - 0.5, -a2) if uv1 != None and uv2 != None: du = uv2[0] - uv1[0] dv = uv2[1] - uv1[1] for i in range(0, 3): tmp1 = (uv1[0] + int(0.375 * i * du), uv1[1] + int(0.375 * i * dv)) tmp2 = (tmp1[0] + int(0.25 * du), tmp1[1] + int(0.25 * dv)) cv2.line(self.frame, tmp1, tmp2, self.color, self.line_width, cv2.LINE_AA) uv = (uv1[0] + int(1.25 * du), uv1[1] + int(1.25 * dv)) self.draw_label("%d" % a0, uv, self.font_size, self.line_width) # left tick uv1 = self.ladder_helper(q0, -a0 + 0.5, -a1) uv2 = self.ladder_helper(q0, -a0, -a1) if uv1 != None and uv2 != None: cv2.line(self.frame, uv1, uv2, self.color, self.line_width, cv2.LINE_AA)