def __init__(self, settings_f): """ Setup field of fire metrics based on settings read in from file. """ ##pass setting to the test bench api settings_dict = tba.load_settings(settings_f) ## Optional test bench settings self.show_3d = settings_dict.get("show_3d", False) horizontal_divs = settings_dict.get("horizontal_divs", 720) self.up_direction = settings_dict.get("up_direction", np.array([0, 1.0, 0])) self.ground = settings_dict.get("ground", np.array([0, 1.0, 0])) ## Set up some constant parameters self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs) ## How many rays are considered contiguous - 3degrees of arc self.contiguous = int(ceil(3 * horizontal_divs / 360.0)) self.incr_elev = 2 * pi / 720.0 ## Get the data for the weapon. fof_data = tba.get_data(parts_of_interest)["Weapon_Station_Remote"] ## Get a weapon object. API will enforce there is only one. wep_name = fof_data.keys()[0] self.weapon = Weapon_Station(wep_name, fof_data) ## Specify the classes of objects we want geometry for and load them in. class_set = tba.get_all_geom_set( ) - tba.geom_sets["never_exterior_classes"] surface = tba.load_geometry(class_set, single_file=True) ## Make a binary space partition to speed up intersection calculations self.nodes = np.vstack((surface['x'], surface['y'], surface['z'])).T self.tris = surface['tris'] self.b_tree = BSP_Tree(self.nodes, self.tris) self.b_tree.generate_tree() ## Set up result storage. self.traverse_results = []
def __init__(self, settings_f): """ Setup field of fire metrics based on settings read in from file. """ ##pass setting to the test bench api settings_dict = tba.load_settings(settings_f) ## Optional test bench settings self.show_3d = settings_dict.get("show_3d", False) horizontal_divs = settings_dict.get("horizontal_divs", 720) self.up_direction = settings_dict.get("up_direction", np.array([0, 1.0, 0])) self.ground = settings_dict.get("ground", np.array([0, 1.0, 0])) ## Set up some constant parameters self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs) ## How many rays are considered contiguous - 3degrees of arc self.contiguous = int(ceil(3 * horizontal_divs / 360.0)) self.incr_elev = 2 * pi / 720.0 ## Get the data for the weapon. fof_data = tba.get_data(parts_of_interest)["Weapon_Station_Remote"] ## Get a weapon object. API will enforce there is only one. wep_name = fof_data.keys()[0] self.weapon = Weapon_Station(wep_name, fof_data) ## Specify the classes of objects we want geometry for and load them in. class_set = tba.get_all_geom_set() - tba.geom_sets["never_exterior_classes"] surface = tba.load_geometry(class_set, single_file=True) ## Make a binary space partition to speed up intersection calculations self.nodes = np.vstack((surface['x'], surface['y'], surface['z'])).T self.tris = surface['tris'] self.b_tree = BSP_Tree(self.nodes, self.tris) self.b_tree.generate_tree() ## Set up result storage. self.traverse_results = []
def __init__(self, settings_f): """ Setup vision metrics based on settings read in from file. """ ##pass setting to the test bench api settings_dict = tba.load_settings(settings_f) ## Optional test bench settings self.show_3d = settings_dict.get("show_3d", False) horizontal_divs = settings_dict.get("horizontal_divs", 720) self.focal_length_multiplier = settings_dict.get( "focal_length_multiplier", 1.5) self.far_dist = settings_dict.get("far_dist", 20.0) self.up_direction = settings_dict.get("up_direction", np.array([0, 1.0, 0])) self.fore_direction = settings_dict.get("fore_direction", np.array([0, 0, -1.0])) self.ground = settings_dict.get("ground", np.array([0, 1.0, 0])) ## Set up some constant parameters self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs) self.target_points_horizon = np.zeros(horizontal_divs, np.uint32) ## Get a dictionary of parts and properties form the api (periscopes, manikins, screens) fov_data = tba.get_data(parts_of_interest) ## Get Periscope objects for any that are found self.periscopes = [ Vision_Device(p, fov_data["Periscope"]) for p in fov_data["Periscope"] ] ## Get Manikin objects for the vehicle occupants that have vision requirements m_data = fov_data["Manikin"] req_roles = ["driver", "vehicle_commander", "troop_commander"] vis_roles = lambda m: m_data[m]["properties"]["vehicle_role" ] in req_roles self.manikins = [ Manikin(m, fov_data["Manikin"]) for m in m_data if vis_roles(m) ] ## Need exactly one of each roles = [m.vehicle_role for m in self.manikins] if len(set(roles)) != len(req_roles): msg = "Didn't get 1 of each vehicle_role={} instead got={}".format( req_roles, roles) raise ValueError(msg) ## Specify the classes of objects we want geometry for. class_set = tba.get_all_geom_set( ) - tba.geom_sets["never_exterior_classes"] ## And load them all as an effective single file surface = tba.load_geometry(class_set, single_file=True) ## Make a binary space partition to speed up intersection calculations self.nodes = np.vstack((surface['x'], surface['y'], surface['z'])).T self.tris = surface['tris'] self.b_tree = BSP_Tree(self.nodes, self.tris) self.b_tree.generate_tree() ## TODO this won't cope with unusual orientations ## determine orientation of the vehicle given the direction up and forward fore = np.dot(self.nodes, self.fore_direction).min() fore = np.min(surface["z"]) self.side_direction = np.cross(self.up_direction, self.fore_direction) side_array = np.dot(self.nodes, self.side_direction) self.z_ground = np.dot(self.ground, self.up_direction) center = (side_array.min() + side_array.max()) / 2.0 center = (np.min(surface["x"]) + np.max(surface["x"])) * 0.5 ## Find the vehicle origin (point on ground at front on centerline) self.veh_origin = np.array([center, self.z_ground, fore]) logging.info("Vehicle origin is at {}".format(self.veh_origin)) self.tran_veh = translate(self.veh_origin) self.fore_aft = [None] * len(self.periscopes) self.uplook = np.array([-1000.0] * len(self.periscopes)) self.hit = [] self.hor_fans = []
class Vision_Metrics(object): """ Calculates field of vision metrics. """ def __init__(self, settings_f): """ Setup vision metrics based on settings read in from file. """ ##pass setting to the test bench api settings_dict = tba.load_settings(settings_f) ## Optional test bench settings self.show_3d = settings_dict.get("show_3d", False) horizontal_divs = settings_dict.get("horizontal_divs", 720) self.focal_length_multiplier = settings_dict.get( "focal_length_multiplier", 1.5) self.far_dist = settings_dict.get("far_dist", 20.0) self.up_direction = settings_dict.get("up_direction", np.array([0, 1.0, 0])) self.fore_direction = settings_dict.get("fore_direction", np.array([0, 0, -1.0])) self.ground = settings_dict.get("ground", np.array([0, 1.0, 0])) ## Set up some constant parameters self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs) self.target_points_horizon = np.zeros(horizontal_divs, np.uint32) ## Get a dictionary of parts and properties form the api (periscopes, manikins, screens) fov_data = tba.get_data(parts_of_interest) ## Get Periscope objects for any that are found self.periscopes = [ Vision_Device(p, fov_data["Periscope"]) for p in fov_data["Periscope"] ] ## Get Manikin objects for the vehicle occupants that have vision requirements m_data = fov_data["Manikin"] req_roles = ["driver", "vehicle_commander", "troop_commander"] vis_roles = lambda m: m_data[m]["properties"]["vehicle_role" ] in req_roles self.manikins = [ Manikin(m, fov_data["Manikin"]) for m in m_data if vis_roles(m) ] ## Need exactly one of each roles = [m.vehicle_role for m in self.manikins] if len(set(roles)) != len(req_roles): msg = "Didn't get 1 of each vehicle_role={} instead got={}".format( req_roles, roles) raise ValueError(msg) ## Specify the classes of objects we want geometry for. class_set = tba.get_all_geom_set( ) - tba.geom_sets["never_exterior_classes"] ## And load them all as an effective single file surface = tba.load_geometry(class_set, single_file=True) ## Make a binary space partition to speed up intersection calculations self.nodes = np.vstack((surface['x'], surface['y'], surface['z'])).T self.tris = surface['tris'] self.b_tree = BSP_Tree(self.nodes, self.tris) self.b_tree.generate_tree() ## TODO this won't cope with unusual orientations ## determine orientation of the vehicle given the direction up and forward fore = np.dot(self.nodes, self.fore_direction).min() fore = np.min(surface["z"]) self.side_direction = np.cross(self.up_direction, self.fore_direction) side_array = np.dot(self.nodes, self.side_direction) self.z_ground = np.dot(self.ground, self.up_direction) center = (side_array.min() + side_array.max()) / 2.0 center = (np.min(surface["x"]) + np.max(surface["x"])) * 0.5 ## Find the vehicle origin (point on ground at front on centerline) self.veh_origin = np.array([center, self.z_ground, fore]) logging.info("Vehicle origin is at {}".format(self.veh_origin)) self.tran_veh = translate(self.veh_origin) self.fore_aft = [None] * len(self.periscopes) self.uplook = np.array([-1000.0] * len(self.periscopes)) self.hit = [] self.hor_fans = [] 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 assign_to_manikins(self): """ Assign periscopes to manikins (i.e. who can see what) """ for man in self.manikins: for periscope in self.periscopes: visible = False for eye_inv_trans in (man.left_eye_veh_inv, man.right_eye_veh_inv): mtrx_eye_to_glass = np.dot(eye_inv_trans, periscope.trans_inner_glass) trans_eye_to_glass = mtrx_eye_to_glass[:3, 3] resultant_z_into_glass = np.dot(mtrx_eye_to_glass, np.array([0, 0, -1, 0]))[:3] unit_sight_line = normalize(trans_eye_to_glass) sight_distance = sqrt( np.dot(trans_eye_to_glass, trans_eye_to_glass)) dot_prod = np.dot(resultant_z_into_glass, unit_sight_line) theta = acos(dot_prod) * 180.0 / pi if sight_distance < 1.0 and theta < 75.0: visible = True break man.vision_devices.append(visible) msg = "\nmanikin {} can see these devices:\n".format(man.name) for p in man.vision_devices: msg += "\t{}\n".format(p) logging.info(msg) def post_process(self): """ Generate the metrics from the stored raw ray trace results. """ out = {} for man in self.manikins: vis_mask = sum( [2**p for p, vis in enumerate(man.vision_devices) if vis]) horizon_fraction = 0.0 num_pts = np.shape(self.hor_sweep)[0] for i in xrange(num_pts): if self.target_points_horizon[i] & vis_mask: horizon_fraction += 1.0 / num_pts fore_aft_arr = np.array(self.fore_aft) pfx = man.vehicle_role if any(man.vision_devices): out.update({ pfx + "_horizon_percentage": horizon_fraction, pfx + "_fore_closest_visible_point": np.min(fore_aft_arr[np.array(man.vision_devices), 0]), pfx + "_aft_closest_visible_point": np.min(fore_aft_arr[np.array(man.vision_devices), 1]), pfx + "_max_uplook": np.max(self.uplook[np.array(man.vision_devices)]), }) else: out.update({ pfx + "_horizon_percentage": horizon_fraction, pfx + "_fore_closest_visible_point": "Fail", pfx + "_aft_closest_visible_point": "Fail", pfx + "_max_uplook": "Fail", }) ## Write results and echo to log. tba.write_results(out) if self.show_3d: self.post_process_3d() def post_process_3d(self): """ Make optional 3D plots, """ ## Import delayed till here so only need mayavi if 3D output is enabled. from mayavi import mlab for man in self.manikins: ## The devices this manikin is assigned to (i.e. can see). is_dev_viz = man.vision_devices ## Accumulate all points this manikin can see through all his assigned devices. hits = [ h for hit, vis in zip(self.hit, is_dev_viz) for h in hit if vis ] ## Plot horizon visibility fans and rays for max uplook and closest ground point. plots(hits, [hor for hor, v in zip(self.hor_fans, is_dev_viz) if v], mlab) ## Plot the vehicle as shaded surfaces. show_vehicle(self.veh_origin, self.nodes, self.tris, mlab) mlab.show()
class Field_Of_Fire_Metrics(object): """ Calculates field of fire metrics. """ def __init__(self, settings_f): """ Setup field of fire metrics based on settings read in from file. """ ##pass setting to the test bench api settings_dict = tba.load_settings(settings_f) ## Optional test bench settings self.show_3d = settings_dict.get("show_3d", False) horizontal_divs = settings_dict.get("horizontal_divs", 720) self.up_direction = settings_dict.get("up_direction", np.array([0, 1.0, 0])) self.ground = settings_dict.get("ground", np.array([0, 1.0, 0])) ## Set up some constant parameters self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs) ## How many rays are considered contiguous - 3degrees of arc self.contiguous = int(ceil(3 * horizontal_divs / 360.0)) self.incr_elev = 2 * pi / 720.0 ## Get the data for the weapon. fof_data = tba.get_data(parts_of_interest)["Weapon_Station_Remote"] ## Get a weapon object. API will enforce there is only one. wep_name = fof_data.keys()[0] self.weapon = Weapon_Station(wep_name, fof_data) ## Specify the classes of objects we want geometry for and load them in. class_set = tba.get_all_geom_set( ) - tba.geom_sets["never_exterior_classes"] surface = tba.load_geometry(class_set, single_file=True) ## Make a binary space partition to speed up intersection calculations self.nodes = np.vstack((surface['x'], surface['y'], surface['z'])).T self.tris = surface['tris'] self.b_tree = BSP_Tree(self.nodes, self.tris) self.b_tree.generate_tree() ## Set up result storage. self.traverse_results = [] 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 post_process(self): """ Generate the metrics from the stored raw ray trace results. """ data = self.traverse_results ## Check raw traverse results for continguous regions of no ground coverage. none_runs = [ len(list(grp)) for k, grp in itertools.groupby(data, lambda x: x) if k is None ] if none_runs and max(none_runs) >= self.contiguous: ## Fail because there is a contiguous region larger than allowed fwd = "Fail" aft = "Fail" no_fire_area = "Fail" else: trav_cen = get_translation(self.weapon.trans_trav) angles = np.array([x[0] for x in data if x[1] is not None]) muz_pts = np.array([x[1][0] for x in data if x[1] is not None]) gnd_pts = np.array([x[1][1] for x in data if x[1] is not None]) ## Distance along the ground gnd_dist = np.sqrt((trav_cen[0] - gnd_pts[:, 0])**2 + (trav_cen[2] - gnd_pts[:, 2])**2) ## Calculate the minimum shoot distance over contiguous regions mod_dist = grey_erosion(gnd_dist, self.contiguous, mode="wrap") ## Calculate an area metric (A = 0.5 * a.b.sin(c_ang)) num_tris = len(angles) no_fire_area = 0.0 for i in xrange(num_tris): next_ang = (angles[0] + 2 * pi) if i + 1 >= num_tris else angles[i + 1] c_ang = next_ang - angles[i] a = gnd_dist[i] b = gnd_dist[(i + 1) % num_tris] no_fire_area += 0.5 * a * b * sin(c_ang) ## Calculate min forward and aft shoot distance. fwd = np.max(mod_dist[np.where((angles >= 3 * pi / 2) | (angles <= pi / 2))]) aft = np.max(mod_dist[np.where((angles <= 3 * pi / 2) & (angles >= pi / 2))]) self.post_process_2d(angles, gnd_dist, mod_dist, no_fire_area, fwd, aft) if self.show_3d: self.post_process_3d(muz_pts, gnd_pts) ## Write results to json file and echo to log. out = { "min_fire_dist_fore_180": fwd, "min_fire_dist_aft_180": aft, "no_fire_area": no_fire_area } tba.write_results(out) def post_process_2d(self, angles, gnd_dist, mod_dist, area, fwd, aft): """ Save a 2d figure to illustrate the no-fire region and the metrics. """ ## Setup polar plot with textual labels rather than numerical ax = vehicle_polar_axes() ## Plot a line for the raw data and a filled region for the processed data. ax.plot(angles, gnd_dist, color="r", alpha=0.3) ax.fill(angles, mod_dist, color="r", alpha=0.5) ## Show semi-circles for the fore and aft closest point metrics. for offset, val in [(0, fwd), (pi, aft)]: phi = np.linspace(offset - pi / 2, offset + pi / 2, 180) r = np.ones_like(phi) * val ax.fill(phi, r, color="k", alpha=0.1) ax.plot(phi, r, color="k", linewidth=2) ## Add the metrics text. ax.text(-pi / 4, fwd + 2, "{}[m]".format(round(fwd, 1)), va="bottom") ax.text(pi + pi / 4, aft + 2, "{}[m]".format(round(aft, 1)), va="top") ax.text(0.0, fwd + 10, "No Fire Area = {}[m2]".format(int(area)), ha="center", color="r") ## Add the vehicle as a bounding box. trav_cen = get_translation(self.weapon.trans_trav) l_r = trav_cen[0] - np.min(self.nodes[:, 0]), trav_cen[0] - np.max( self.nodes[:, 0]) f_b = trav_cen[2] - np.min(self.nodes[:, 2]), trav_cen[2] - np.max( self.nodes[:, 2]) veh = np.array([(atan2(x, y), sqrt(x * x + y * y)) for x, y in itertools.product(l_r, f_b)]) ax.fill(veh[[0, 1, 3, 2, 0], 0], veh[[0, 1, 3, 2, 0], 1], color="k", alpha=0.4) plt.savefig("field_of_fire_pic.png") def post_process_3d(self, muz_pts, gnd_pts): """ Make optional 3D plots, """ ## Import delayed till here so only need mayavi if 3D output is enabled. from mayavi import mlab ## Plot closest shot lines plot_shot_lines(muz_pts, gnd_pts, mlab) ## Plot the vehicle as shaded surfaces. show_vehicle(self.nodes, self.tris, mlab) mlab.show()
class Field_Of_Fire_Metrics(object): """ Calculates field of fire metrics. """ def __init__(self, settings_f): """ Setup field of fire metrics based on settings read in from file. """ ##pass setting to the test bench api settings_dict = tba.load_settings(settings_f) ## Optional test bench settings self.show_3d = settings_dict.get("show_3d", False) horizontal_divs = settings_dict.get("horizontal_divs", 720) self.up_direction = settings_dict.get("up_direction", np.array([0, 1.0, 0])) self.ground = settings_dict.get("ground", np.array([0, 1.0, 0])) ## Set up some constant parameters self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs) ## How many rays are considered contiguous - 3degrees of arc self.contiguous = int(ceil(3 * horizontal_divs / 360.0)) self.incr_elev = 2 * pi / 720.0 ## Get the data for the weapon. fof_data = tba.get_data(parts_of_interest)["Weapon_Station_Remote"] ## Get a weapon object. API will enforce there is only one. wep_name = fof_data.keys()[0] self.weapon = Weapon_Station(wep_name, fof_data) ## Specify the classes of objects we want geometry for and load them in. class_set = tba.get_all_geom_set() - tba.geom_sets["never_exterior_classes"] surface = tba.load_geometry(class_set, single_file=True) ## Make a binary space partition to speed up intersection calculations self.nodes = np.vstack((surface['x'], surface['y'], surface['z'])).T self.tris = surface['tris'] self.b_tree = BSP_Tree(self.nodes, self.tris) self.b_tree.generate_tree() ## Set up result storage. self.traverse_results = [] 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 post_process(self): """ Generate the metrics from the stored raw ray trace results. """ data = self.traverse_results ## Check raw traverse results for continguous regions of no ground coverage. none_runs = [len(list(grp)) for k, grp in itertools.groupby(data, lambda x:x) if k is None] if none_runs and max(none_runs) >= self.contiguous: ## Fail because there is a contiguous region larger than allowed fwd = "Fail" aft = "Fail" no_fire_area = "Fail" else: trav_cen = get_translation(self.weapon.trans_trav) angles = np.array([x[0] for x in data if x[1] is not None]) muz_pts = np.array([x[1][0] for x in data if x[1] is not None]) gnd_pts = np.array([x[1][1] for x in data if x[1] is not None]) ## Distance along the ground gnd_dist = np.sqrt((trav_cen[0] - gnd_pts[:, 0]) ** 2 + (trav_cen[2] - gnd_pts[:, 2]) ** 2) ## Calculate the minimum shoot distance over contiguous regions mod_dist = grey_erosion(gnd_dist, self.contiguous, mode="wrap") ## Calculate an area metric (A = 0.5 * a.b.sin(c_ang)) num_tris = len(angles) no_fire_area = 0.0 for i in xrange(num_tris): next_ang = (angles[0] + 2 * pi) if i + 1 >= num_tris else angles[i+1] c_ang = next_ang - angles[i] a = gnd_dist[i] b = gnd_dist[(i + 1) % num_tris] no_fire_area += 0.5 * a * b * sin(c_ang) ## Calculate min forward and aft shoot distance. fwd = np.max(mod_dist[np.where((angles >= 3 * pi / 2) | (angles <= pi / 2))]) aft = np.max(mod_dist[np.where((angles <= 3 * pi / 2) & (angles >= pi / 2))]) self.post_process_2d(angles, gnd_dist, mod_dist, no_fire_area, fwd, aft) if self.show_3d: self.post_process_3d(muz_pts, gnd_pts) ## Write results to json file and echo to log. out = { "min_fire_dist_fore_180" : fwd, "min_fire_dist_aft_180" : aft, "no_fire_area" : no_fire_area } tba.write_results(out) def post_process_2d(self, angles, gnd_dist, mod_dist, area, fwd, aft): """ Save a 2d figure to illustrate the no-fire region and the metrics. """ ## Setup polar plot with textual labels rather than numerical ax = vehicle_polar_axes() ## Plot a line for the raw data and a filled region for the processed data. ax.plot(angles, gnd_dist, color="r", alpha=0.3) ax.fill(angles, mod_dist, color="r", alpha=0.5) ## Show semi-circles for the fore and aft closest point metrics. for offset, val in [(0, fwd), (pi, aft)]: phi = np.linspace(offset - pi / 2, offset + pi / 2, 180) r = np.ones_like(phi) * val ax.fill(phi, r, color="k", alpha=0.1) ax.plot(phi, r, color="k", linewidth=2) ## Add the metrics text. ax.text(-pi / 4, fwd + 2, "{}[m]".format(round(fwd, 1)), va="bottom") ax.text(pi + pi / 4, aft + 2, "{}[m]".format(round(aft, 1)), va="top") ax.text(0.0, fwd + 10, "No Fire Area = {}[m2]".format(int(area)), ha="center", color="r") ## Add the vehicle as a bounding box. trav_cen = get_translation(self.weapon.trans_trav) l_r = trav_cen[0] - np.min(self.nodes[:, 0]), trav_cen[0] - np.max(self.nodes[:, 0]) f_b = trav_cen[2] - np.min(self.nodes[:, 2]), trav_cen[2] - np.max(self.nodes[:, 2]) veh = np.array([(atan2(x, y), sqrt(x * x + y * y)) for x, y in itertools.product(l_r, f_b)]) ax.fill(veh[[0, 1, 3, 2, 0], 0], veh[[0, 1, 3, 2, 0], 1], color="k", alpha=0.4) plt.savefig("field_of_fire_pic.png") def post_process_3d(self, muz_pts, gnd_pts): """ Make optional 3D plots, """ ## Import delayed till here so only need mayavi if 3D output is enabled. from mayavi import mlab ## Plot closest shot lines plot_shot_lines(muz_pts, gnd_pts, mlab) ## Plot the vehicle as shaded surfaces. show_vehicle(self.nodes, self.tris, mlab) mlab.show()
def __init__(self, settings_f): """ Setup vision metrics based on settings read in from file. """ ##pass setting to the test bench api settings_dict = tba.load_settings(settings_f) ## Optional test bench settings self.show_3d = settings_dict.get("show_3d", False) horizontal_divs = settings_dict.get("horizontal_divs", 720) self.focal_length_multiplier = settings_dict.get("focal_length_multiplier", 1.5) self.far_dist = settings_dict.get("far_dist", 20.0) self.up_direction = settings_dict.get("up_direction", np.array([0, 1.0, 0])) self.fore_direction = settings_dict.get("fore_direction", np.array([0, 0, -1.0])) self.ground = settings_dict.get("ground", np.array([0, 1.0, 0])) ## Set up some constant parameters self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs) self.target_points_horizon = np.zeros(horizontal_divs, np.uint32) ## Get a dictionary of parts and properties form the api (periscopes, manikins, screens) fov_data = tba.get_data(parts_of_interest) ## Get Periscope objects for any that are found self.periscopes = [Vision_Device(p, fov_data["Periscope"]) for p in fov_data["Periscope"]] ## Get Manikin objects for the vehicle occupants that have vision requirements m_data = fov_data["Manikin"] req_roles = ["driver", "vehicle_commander", "troop_commander"] vis_roles = lambda m: m_data[m]["properties"]["vehicle_role"] in req_roles self.manikins = [Manikin(m, fov_data["Manikin"]) for m in m_data if vis_roles(m)] ## Need exactly one of each roles = [m.vehicle_role for m in self.manikins] if len(set(roles)) != len(req_roles): msg = "Didn't get 1 of each vehicle_role={} instead got={}".format(req_roles, roles) raise ValueError(msg) ## Specify the classes of objects we want geometry for. class_set = tba.get_all_geom_set() - tba.geom_sets["never_exterior_classes"] ## And load them all as an effective single file surface = tba.load_geometry(class_set, single_file=True) ## Make a binary space partition to speed up intersection calculations self.nodes = np.vstack((surface["x"], surface["y"], surface["z"])).T self.tris = surface["tris"] self.b_tree = BSP_Tree(self.nodes, self.tris) self.b_tree.generate_tree() ## TODO this won't cope with unusual orientations ## determine orientation of the vehicle given the direction up and forward fore = np.dot(self.nodes, self.fore_direction).min() fore = np.min(surface["z"]) self.side_direction = np.cross(self.up_direction, self.fore_direction) side_array = np.dot(self.nodes, self.side_direction) self.z_ground = np.dot(self.ground, self.up_direction) center = (side_array.min() + side_array.max()) / 2.0 center = (np.min(surface["x"]) + np.max(surface["x"])) * 0.5 ## Find the vehicle origin (point on ground at front on centerline) self.veh_origin = np.array([center, self.z_ground, fore]) logging.info("Vehicle origin is at {}".format(self.veh_origin)) self.tran_veh = translate(self.veh_origin) self.fore_aft = [None] * len(self.periscopes) self.uplook = np.array([-1000.0] * len(self.periscopes)) self.hit = [] self.hor_fans = []
class Vision_Metrics(object): """ Calculates field of vision metrics. """ def __init__(self, settings_f): """ Setup vision metrics based on settings read in from file. """ ##pass setting to the test bench api settings_dict = tba.load_settings(settings_f) ## Optional test bench settings self.show_3d = settings_dict.get("show_3d", False) horizontal_divs = settings_dict.get("horizontal_divs", 720) self.focal_length_multiplier = settings_dict.get("focal_length_multiplier", 1.5) self.far_dist = settings_dict.get("far_dist", 20.0) self.up_direction = settings_dict.get("up_direction", np.array([0, 1.0, 0])) self.fore_direction = settings_dict.get("fore_direction", np.array([0, 0, -1.0])) self.ground = settings_dict.get("ground", np.array([0, 1.0, 0])) ## Set up some constant parameters self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs) self.target_points_horizon = np.zeros(horizontal_divs, np.uint32) ## Get a dictionary of parts and properties form the api (periscopes, manikins, screens) fov_data = tba.get_data(parts_of_interest) ## Get Periscope objects for any that are found self.periscopes = [Vision_Device(p, fov_data["Periscope"]) for p in fov_data["Periscope"]] ## Get Manikin objects for the vehicle occupants that have vision requirements m_data = fov_data["Manikin"] req_roles = ["driver", "vehicle_commander", "troop_commander"] vis_roles = lambda m: m_data[m]["properties"]["vehicle_role"] in req_roles self.manikins = [Manikin(m, fov_data["Manikin"]) for m in m_data if vis_roles(m)] ## Need exactly one of each roles = [m.vehicle_role for m in self.manikins] if len(set(roles)) != len(req_roles): msg = "Didn't get 1 of each vehicle_role={} instead got={}".format(req_roles, roles) raise ValueError(msg) ## Specify the classes of objects we want geometry for. class_set = tba.get_all_geom_set() - tba.geom_sets["never_exterior_classes"] ## And load them all as an effective single file surface = tba.load_geometry(class_set, single_file=True) ## Make a binary space partition to speed up intersection calculations self.nodes = np.vstack((surface["x"], surface["y"], surface["z"])).T self.tris = surface["tris"] self.b_tree = BSP_Tree(self.nodes, self.tris) self.b_tree.generate_tree() ## TODO this won't cope with unusual orientations ## determine orientation of the vehicle given the direction up and forward fore = np.dot(self.nodes, self.fore_direction).min() fore = np.min(surface["z"]) self.side_direction = np.cross(self.up_direction, self.fore_direction) side_array = np.dot(self.nodes, self.side_direction) self.z_ground = np.dot(self.ground, self.up_direction) center = (side_array.min() + side_array.max()) / 2.0 center = (np.min(surface["x"]) + np.max(surface["x"])) * 0.5 ## Find the vehicle origin (point on ground at front on centerline) self.veh_origin = np.array([center, self.z_ground, fore]) logging.info("Vehicle origin is at {}".format(self.veh_origin)) self.tran_veh = translate(self.veh_origin) self.fore_aft = [None] * len(self.periscopes) self.uplook = np.array([-1000.0] * len(self.periscopes)) self.hit = [] self.hor_fans = [] 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 assign_to_manikins(self): """ Assign periscopes to manikins (i.e. who can see what) """ for man in self.manikins: for periscope in self.periscopes: visible = False for eye_inv_trans in (man.left_eye_veh_inv, man.right_eye_veh_inv): mtrx_eye_to_glass = np.dot(eye_inv_trans, periscope.trans_inner_glass) trans_eye_to_glass = mtrx_eye_to_glass[:3, 3] resultant_z_into_glass = np.dot(mtrx_eye_to_glass, np.array([0, 0, -1, 0]))[:3] unit_sight_line = normalize(trans_eye_to_glass) sight_distance = sqrt(np.dot(trans_eye_to_glass, trans_eye_to_glass)) dot_prod = np.dot(resultant_z_into_glass, unit_sight_line) theta = acos(dot_prod) * 180.0 / pi if sight_distance < 1.0 and theta < 75.0: visible = True break man.vision_devices.append(visible) msg = "\nmanikin {} can see these devices:\n".format(man.name) for p in man.vision_devices: msg += "\t{}\n".format(p) logging.info(msg) def post_process(self): """ Generate the metrics from the stored raw ray trace results. """ out = {} for man in self.manikins: vis_mask = sum([2 ** p for p, vis in enumerate(man.vision_devices) if vis]) horizon_fraction = 0.0 num_pts = np.shape(self.hor_sweep)[0] for i in xrange(num_pts): if self.target_points_horizon[i] & vis_mask: horizon_fraction += 1.0 / num_pts fore_aft_arr = np.array(self.fore_aft) pfx = man.vehicle_role if any(man.vision_devices): out.update( { pfx + "_horizon_percentage": horizon_fraction, pfx + "_fore_closest_visible_point": np.min(fore_aft_arr[np.array(man.vision_devices), 0]), pfx + "_aft_closest_visible_point": np.min(fore_aft_arr[np.array(man.vision_devices), 1]), pfx + "_max_uplook": np.max(self.uplook[np.array(man.vision_devices)]), } ) else: out.update( { pfx + "_horizon_percentage": horizon_fraction, pfx + "_fore_closest_visible_point": "Fail", pfx + "_aft_closest_visible_point": "Fail", pfx + "_max_uplook": "Fail", } ) ## Write results and echo to log. tba.write_results(out) if self.show_3d: self.post_process_3d() def post_process_3d(self): """ Make optional 3D plots, """ ## Import delayed till here so only need mayavi if 3D output is enabled. from mayavi import mlab for man in self.manikins: ## The devices this manikin is assigned to (i.e. can see). is_dev_viz = man.vision_devices ## Accumulate all points this manikin can see through all his assigned devices. hits = [h for hit, vis in zip(self.hit, is_dev_viz) for h in hit if vis] ## Plot horizon visibility fans and rays for max uplook and closest ground point. plots(hits, [hor for hor, v in zip(self.hor_fans, is_dev_viz) if v], mlab) ## Plot the vehicle as shaded surfaces. show_vehicle(self.veh_origin, self.nodes, self.tris, mlab) mlab.show()