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)
