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 __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)
