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 get_vehicle_csys(self): """ Create a mock vehicle coordinate system based on the calculated properties/metrics file Represents the rotation that would be required to align vehicle csys with +z for occupied voxel grid. (no translation component) """ if self._tr_mat is None: # Rotate around a vector mutually orthogonal to the current and desired "up" vectors upvec = self.get_vertical() rot_around = np.cross(upvec, np.array([0, 0, 1])) rot_ang = -np.arccos(upvec[2]) self._tr_mat = geom_utils.rotation_about_vector(rot_around, rot_ang) return self._tr_mat
def get_vehicle_csys(self): """ Create a mock vehicle coordinate system based on the calculated properties/metrics file Represents the rotation that would be required to align vehicle csys with +z for occupied voxel grid. (no translation component) """ if self._tr_mat is None: # Rotate around a vector mutually orthogonal to the current and desired "up" vectors upvec = self.get_vertical() rot_around = np.cross(upvec, np.array([0, 0, 1])) rot_ang = -np.arccos(upvec[2]) self._tr_mat = geom_utils.rotation_about_vector( rot_around, rot_ang) return self._tr_mat
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)
HULLS = { "Hull_Assembly_Parametric", 'Hull_Assembly_Example_With_Connector' } MANIKINS = {"Manikin"} # Special labels applied to specific types of voxels VOXEL_LABELS = {2: HULLS, 4: DOORS, 8: HATCHES, 16: MANIKINS} vehicle_surfs = tb_api.load_geometry(tb_api.get_all_geom_set() - MANIKINS, single_file=False) # Modify node coords so object aligns with cartesian axes of occ voxel grid, +z=up # Vector to rotate around is cross product of current z axis and sfc normal veh_up = np.array([0., 1., 0.]) rot_around = np.cross(veh_up, np.array([0, 0, 1])) rot_ang = -np.arccos(veh_up[2]) tr_mat = geom_utils.rotation_about_vector(rot_around, rot_ang) # voxel_data = main(vehicle_surfs, tr_mat, VOXEL_LABELS, SETTINGS) vox_veh_folder = r"voxelated_models/vehicles/{}/{}".format( SETTINGS["run_id"], SETTINGS["voxel_size"]) vox_veh_file = "voxels_{}_vox{}_hacked".format(SETTINGS["run_id"], SETTINGS["voxel_size"]) try: voxel_data = data_io.load_array(vox_veh_folder, vox_veh_file, True) except: voxel_data = main(vehicle_surfs, tr_mat, VOXEL_LABELS, SETTINGS) from mayavi import mlab xo, yo, zo = np.where(voxel_data["value"] == 1)