def __init__(self, manikin, m_data): """ Setup the manikin using data from the supplied xml file. """ ##get manikin properties from the dictionary returned by the api datums = m_data[manikin]["datums"] properties = m_data[manikin]["properties"] self.name = m_data[manikin]["name"] self.vehicle_role = properties["vehicle_role"] ##datums self.left_eye = datums["EXT_EYE_CSYS_LEFT"]["local"] self.right_eye = datums["EXT_EYE_CSYS_RIGHT"]["local"] ## This is the transform to the origin of the component in vehicle space trans_origin = m_data[manikin]["trans"] eye_rot = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) ## Get transform to eyes in vehicle space self.left_eye_veh = mul(trans_origin, self.left_eye, eye_rot) self.right_eye_veh = mul(trans_origin, self.right_eye, eye_rot) self.left_eye_veh_inv = np.linalg.inv(self.left_eye_veh) self.right_eye_veh_inv = np.linalg.inv(self.right_eye_veh) ## Axis to rotate around to scan horizontally from eye point (in eye csys) self.eyes_hor_axis = np.array([0, 1, 0, 1]) ## This list will store the vision devices this manikin can interact with self.vision_devices = []
def _is_target_point_visible(self, target_point_trans, c_periscope): """ Return True if the ``target_point_trans`` is visible from the current view device. """ ## Target point in the focal point coord system t_pt_foc = get_translation(c_periscope.focal_point_trans_inv, target_point_trans) ## Calculate angle to target w.r.t to periscope view axis h_ang = atan2(t_pt_foc[0], t_pt_foc[2]) rad_in_plane = sqrt(t_pt_foc[0] * t_pt_foc[0] + t_pt_foc[2] * t_pt_foc[2]) v_ang = atan2(-t_pt_foc[1], rad_in_plane) ## Transform to project forward from the focal point to be slightly in front of the glass lens_dist = translate([0.0, 0.0, c_periscope.focal_distance * self.focal_length_multiplier, 1.0]) ## Check if within the periscope view frustrum if c_periscope.is_ray_within_frustrum(h_ang, v_ang): ## Calculate the lens offset point tran_hor = rotation_about_vector(c_periscope.lens_hor_axis, h_ang) tran_vert = rotation_about_vector(mul(tran_hor, c_periscope.lens_hor_axis), v_ang) lens_point = get_translation(c_periscope.focal_point_trans, tran_vert, tran_hor, lens_dist) ## Actually do the intersection here t = self.b_tree.get_line_intersection(lens_point, get_translation(target_point_trans)) if t < 0: ## Visible return True ## Not visible return False
def _is_target_point_visible(self, target_point_trans, c_periscope): """ Return True if the ``target_point_trans`` is visible from the current view device. """ ## Target point in the focal point coord system t_pt_foc = get_translation(c_periscope.focal_point_trans_inv, target_point_trans) ## Calculate angle to target w.r.t to periscope view axis h_ang = atan2(t_pt_foc[0], t_pt_foc[2]) rad_in_plane = sqrt(t_pt_foc[0] * t_pt_foc[0] + t_pt_foc[2] * t_pt_foc[2]) v_ang = atan2(-t_pt_foc[1], rad_in_plane) ## Transform to project forward from the focal point to be slightly in front of the glass lens_dist = translate([ 0.0, 0.0, c_periscope.focal_distance * self.focal_length_multiplier, 1.0 ]) ## Check if within the periscope view frustrum if c_periscope.is_ray_within_frustrum(h_ang, v_ang): ## Calculate the lens offset point tran_hor = rotation_about_vector(c_periscope.lens_hor_axis, h_ang) tran_vert = rotation_about_vector( mul(tran_hor, c_periscope.lens_hor_axis), v_ang) lens_point = get_translation(c_periscope.focal_point_trans, tran_vert, tran_hor, lens_dist) ## Actually do the intersection here t = self.b_tree.get_line_intersection( lens_point, get_translation(target_point_trans)) if t < 0: ## Visible return True ## Not visible return False
def trace_rays(self): """ Trace the rays to calculate the raw hit points for each device. """ for p, periscope in enumerate(self.periscopes): hit = [] hor_fan = [] hor_fan.append(get_translation(periscope.trans_glass)) dist_ver = get_translation( periscope.trans_glass)[1] - self.z_ground ## Sweep around the global up direction to find horizontal view percent for i, ang_hor in enumerate(self.hor_sweep): ## Global transform to target point tran_hor_world = rotation_about_vector(self.up_direction, ang_hor) target_point_trans = mul( self.tran_veh, tran_hor_world, translate(np.array([self.far_dist, 0, 0])), translate(np.array([0, dist_ver, 0]))) if self._is_target_point_visible(target_point_trans, periscope): self.target_points_horizon[i] += 2**p hor_fan.append(get_translation(target_point_trans)) else: hor_fan.append(get_translation(periscope.trans_glass)) self.hor_fans.append(hor_fan) ## Find the highest visible point in front of the vehicle max_uplook = 50 accuracy = 0.001 self.uplook[p] = 0.0 tran_hor_world = rotation_about_vector(self.up_direction, pi) upper_uplook = max_uplook lower_uplook = 0.0 while (upper_uplook - lower_uplook) > accuracy: ## Global transform to target point height = (upper_uplook + lower_uplook) * 0.5 target_point_trans = mul( self.tran_veh, tran_hor_world, translate(np.array([0.0, height, 50.0]))) if self._is_target_point_visible(target_point_trans, periscope): self.uplook[p] = height lower_uplook = height else: upper_uplook = height if self.uplook[p] > 0.0: hit.extend((get_translation(target_point_trans), get_translation(target_point_trans), get_translation(periscope.trans_glass))) ## Find the closest visible ground point fore and aft max_radius = 2e6 self.fore_aft[p] = [max_radius, max_radius] for i, rot in enumerate([pi, 0.0]): tran_hor_world = rotation_about_vector(self.up_direction, rot) upper_radius = max_radius lower_radius = 0.0 while (upper_radius - lower_radius) > accuracy: ## Global transform to target point radius = (upper_radius + lower_radius) * 0.5 target_point_trans = mul( self.tran_veh, tran_hor_world, translate(np.array([0, 0, radius]))) if self._is_target_point_visible(target_point_trans, periscope): self.fore_aft[p][i] = radius upper_radius = radius else: lower_radius = radius if self.fore_aft[p][i] < max_radius * 0.5: hit.extend((get_translation(target_point_trans), get_translation(target_point_trans), get_translation(periscope.trans_glass))) self.hit.append(hit)
def trace_rays(self): """ Trace the rays to calculate the raw hit points for each device. """ wep = self.weapon z_rot = np.array([0, 0, 1, 1]) tran_traverse = np.eye(4) tran_elevation = np.eye(4) elev_angle = -wep.max_depr for rot in self.hor_sweep: logging.info("Scanning angle={}".format(rot)) ## Rotate the weapon by traverse angle. tran_traverse = rotation_about_vector(z_rot, rot, tran_traverse) tran_elev_point = mul(wep.trans_trav, tran_traverse, wep.elev_from_trav) elev_point = get_translation(tran_elev_point) first = True good_shot = None while True: ## Rotate the weapon up or down by elevation angle. tran_elevation = rotation_about_vector(z_rot, elev_angle, tran_elevation) muz_point = get_translation( mul(tran_elev_point, tran_elevation, wep.muz_from_elev)) ## Find out where shot would hit ground (or None if it won't hit the ground). gnd_hit = ray_plane_intersection(elev_point, muz_point, self.up_direction, self.ground) ## Shot either 1) wouldn't hit the ground, 2) hit the ground or 3) hit the vehicle. if gnd_hit is not None: ## Test if shot line cleared the vehicle. t = self.b_tree.get_line_intersection(muz_point, gnd_hit) shot = "cleared" if t < 0 else "collide" else: ## Shot above the horizon shot = "above_horizon" ## On first shot at this traverse angle determine if need to elevate or depress aim. if first: first = False if shot == "collide": ## Need to try raising elevation elev_change = self.incr_elev elif shot == "cleared": ## Need to try lowering elevation, but store because this might be the best. elev_change = -self.incr_elev good_shot = (muz_point, gnd_hit) else: elev_change = -self.incr_elev else: if elev_change > 0.0: ## elevation was being raised to find clearance if shot == "cleared": ## This is the closest shot possible, store it and stop looking. good_shot = (muz_point, gnd_hit) break elif shot == "above_horizon" or elev_angle > wep.max_elev: ## Missed the ground or exceeded weapon elevation. break else: ## elevation was being lower to find a closer shot if shot == "collide" or elev_angle < -wep.max_depr: ## Must have already stored the best shot previously. break else: ## Store this shot but keep looking. good_shot = (muz_point, gnd_hit) elev_angle += elev_change if good_shot is not None: self.traverse_results.append((rot, good_shot)) else: self.traverse_results.append((rot, None))
def trace_rays(self): """ Trace the rays to calculate the raw hit points for each device. """ wep = self.weapon z_rot = np.array([0, 0, 1, 1]) tran_traverse = np.eye(4) tran_elevation = np.eye(4) elev_angle = -wep.max_depr for rot in self.hor_sweep: logging.info("Scanning angle={}".format(rot)) ## Rotate the weapon by traverse angle. tran_traverse = rotation_about_vector(z_rot, rot, tran_traverse) tran_elev_point = mul(wep.trans_trav, tran_traverse, wep.elev_from_trav) elev_point = get_translation(tran_elev_point) first = True good_shot = None while True: ## Rotate the weapon up or down by elevation angle. tran_elevation = rotation_about_vector(z_rot, elev_angle, tran_elevation) muz_point = get_translation(mul(tran_elev_point, tran_elevation, wep.muz_from_elev)) ## Find out where shot would hit ground (or None if it won't hit the ground). gnd_hit = ray_plane_intersection(elev_point, muz_point, self.up_direction, self.ground) ## Shot either 1) wouldn't hit the ground, 2) hit the ground or 3) hit the vehicle. if gnd_hit is not None: ## Test if shot line cleared the vehicle. t = self.b_tree.get_line_intersection(muz_point, gnd_hit) shot = "cleared" if t < 0 else "collide" else: ## Shot above the horizon shot = "above_horizon" ## On first shot at this traverse angle determine if need to elevate or depress aim. if first: first = False if shot == "collide": ## Need to try raising elevation elev_change = self.incr_elev elif shot == "cleared": ## Need to try lowering elevation, but store because this might be the best. elev_change = -self.incr_elev good_shot = (muz_point, gnd_hit) else: elev_change = -self.incr_elev else: if elev_change > 0.0: ## elevation was being raised to find clearance if shot == "cleared": ## This is the closest shot possible, store it and stop looking. good_shot = (muz_point, gnd_hit) break elif shot == "above_horizon" or elev_angle > wep.max_elev: ## Missed the ground or exceeded weapon elevation. break else: ## elevation was being lower to find a closer shot if shot == "collide" or elev_angle < -wep.max_depr: ## Must have already stored the best shot previously. break else: ## Store this shot but keep looking. good_shot = (muz_point, gnd_hit) elev_angle += elev_change if good_shot is not None: self.traverse_results.append((rot, good_shot)) else: self.traverse_results.append((rot, None))
def trace_rays(self): """ Trace the rays to calculate the raw hit points for each device. """ for p, periscope in enumerate(self.periscopes): hit = [] hor_fan = [] hor_fan.append(get_translation(periscope.trans_glass)) dist_ver = get_translation(periscope.trans_glass)[1] - self.z_ground ## Sweep around the global up direction to find horizontal view percent for i, ang_hor in enumerate(self.hor_sweep): ## Global transform to target point tran_hor_world = rotation_about_vector(self.up_direction, ang_hor) target_point_trans = mul( self.tran_veh, tran_hor_world, translate(np.array([self.far_dist, 0, 0])), translate(np.array([0, dist_ver, 0])), ) if self._is_target_point_visible(target_point_trans, periscope): self.target_points_horizon[i] += 2 ** p hor_fan.append(get_translation(target_point_trans)) else: hor_fan.append(get_translation(periscope.trans_glass)) self.hor_fans.append(hor_fan) ## Find the highest visible point in front of the vehicle max_uplook = 50 accuracy = 0.001 self.uplook[p] = 0.0 tran_hor_world = rotation_about_vector(self.up_direction, pi) upper_uplook = max_uplook lower_uplook = 0.0 while (upper_uplook - lower_uplook) > accuracy: ## Global transform to target point height = (upper_uplook + lower_uplook) * 0.5 target_point_trans = mul(self.tran_veh, tran_hor_world, translate(np.array([0.0, height, 50.0]))) if self._is_target_point_visible(target_point_trans, periscope): self.uplook[p] = height lower_uplook = height else: upper_uplook = height if self.uplook[p] > 0.0: hit.extend( ( get_translation(target_point_trans), get_translation(target_point_trans), get_translation(periscope.trans_glass), ) ) ## Find the closest visible ground point fore and aft max_radius = 2e6 self.fore_aft[p] = [max_radius, max_radius] for i, rot in enumerate([pi, 0.0]): tran_hor_world = rotation_about_vector(self.up_direction, rot) upper_radius = max_radius lower_radius = 0.0 while (upper_radius - lower_radius) > accuracy: ## Global transform to target point radius = (upper_radius + lower_radius) * 0.5 target_point_trans = mul(self.tran_veh, tran_hor_world, translate(np.array([0, 0, radius]))) if self._is_target_point_visible(target_point_trans, periscope): self.fore_aft[p][i] = radius upper_radius = radius else: lower_radius = radius if self.fore_aft[p][i] < max_radius * 0.5: hit.extend( ( get_translation(target_point_trans), get_translation(target_point_trans), get_translation(periscope.trans_glass), ) ) self.hit.append(hit)