def my_constructor(
self,
SCENEGRAPH=None,
START_MATRIX=avango.gua.make_identity_mat(),
):
### parameters ###
self.fall_velocity = 0.003 # in meter/sec
self.pick_length = 1.0 # in meter
self.pick_direction = avango.gua.Vec3(0.0, -1.0, 0.0)
### variables ###
self.fall_vec = avango.gua.Vec3()
self.lf_time = time.time()
## init internal sub-classes
self.gravity_intersection = Intersection()
self.gravity_intersection.my_constructor(SCENEGRAPH, self.sf_mat,
self.pick_length,
self.pick_direction)
## init field connections
self.mf_pick_result.connect_from(
self.gravity_intersection.mf_pick_result)
## set initial state
self.sf_modified_mat.value = START_MATRIX
def model_for_sensitivity(p_spawn=0.1, max_speed_horizontal=10, max_speed_vertical=10, intersection_type=0):
t_traffic_light_cycle = 5
p_bend = 0.33
p_u_turn = 0.01
p_left = p_bend
p_right = p_bend
p_straight = p_bend
t_from_north = t_traffic_light_cycle
t_from_west = t_traffic_light_cycle
t_from_east = t_traffic_light_cycle
t_from_south = t_traffic_light_cycle
intersection_type = int(intersection_type)
intersections = ['Fourway', 'Traffic lights', 'Equivalent', 'Smart lights']
intersection = intersections[intersection_type]
parameter_set = {
"max_speed_horizontal": int(max_speed_horizontal),
"max_speed_vertical": int(max_speed_vertical),
"seed": 1337,
"bmw_fraction": 0.1,
"intersection_type": intersection,
"t_from_north": t_from_north,
"t_from_west": t_from_west,
"t_from_east": t_from_east,
"t_from_south": t_from_south,
"p_car_spawn_north": p_spawn,
"p_north_to_north": p_u_turn,
"p_north_to_west": p_right,
"p_north_to_east": p_left,
"p_north_to_south": p_straight,
"p_car_spawn_west": p_spawn,
"p_west_to_north": p_left,
"p_west_to_west": p_u_turn,
"p_west_to_east": p_straight,
"p_west_to_south": p_right,
"p_car_spawn_east": p_spawn,
"p_east_to_north": p_right,
"p_east_to_west": p_straight,
"p_east_to_east": p_u_turn,
"p_east_to_south": p_left,
"p_car_spawn_south": p_spawn,
"p_south_to_north": p_straight,
"p_south_to_west": p_left,
"p_south_to_east": p_right,
"p_south_to_south": p_u_turn,
}
model = Intersection(parameters = parameter_set, parameters_as_dict=True)
datawriter = DataWriter(model)
datawriter.run()
data = datawriter.get_runs_by_parameters(parameter_set)
flow = data['results']['throughput'][-1]
crossover = data['results']['mean_crossover_time'][-1]
av_speed = data['results']['average_speed'][-1]
out = np.array([flow,crossover,av_speed])
return out[0]
def my_constructor(self, MF_DOF, ATTENUATION_FACTOR = 1.0, SCENEGRAPH = None, SCENE_ROOT = None, HOST_NAME = None):
self.mf_dof.connect_from(MF_DOF)
self.attenuation_factor = ATTENUATION_FACTOR
self.host_name = HOST_NAME
self.intersection = Intersection(SCENEGRAPH, BLACK_LIST = ["unpickable", "invisible"])
self.room_number = 1
self.speed_factor = 0.1
self.dist = 1
self.script = NavigationScript()
self.script.my_constructor(CLASS = self, ROOM_NUMBER = self.room_number)
def my_constructor(
self,
SCENEGRAPH,
PARENT_NODE,
MF_NAV_DOF,
POINTER_STATION_NAME,
POINTER_TRACKING_NAME,
TRACKING_TRANSMITTER_OFFSET,
):
self.scenegraph = SCENEGRAPH
self.parent_node = PARENT_NODE
### parameters ###
self.ray_length = 100.0 # in meters
self.ray_thickness = 0.01 # in meters
self.intersection_point_size = 0.025 # in meters
self.navidget_duration = 10.0 # in seconds
self.navidget_sphere_size = 0.5 # in meters
### variables ###
self.navigation_mode = 0 # 0 = steering mode, 1 = maneuvering mode, 3 = Navidget target-mode, 4 = Navidget animation-mode
self.navidget_start_pos = None
self.navidget_target_pos = None
self.navidget_start_quat = None
self.navidget_target_quat = None
self.navidget_start_time = None
### navigation variables
self.rotation_center = None
self.first_pick_result = None
self.rot_x_accum_value = 0.0
self.rot_y_accum_value = 0.0
self.manu_distance = 0.0
self.offset_trans_mat = avango.gua.make_identity_mat()
self.offset_rot_mat = avango.gua.make_identity_mat()
self.navidget_trans_mat = avango.gua.make_identity_mat()
self.navidget_rot_mat = avango.gua.make_identity_mat()
self.animation_state = "start"
### resources ###
# init pointer sensors
self.pointer_tracking_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.pointer_tracking_sensor.Station.value = POINTER_TRACKING_NAME
self.pointer_tracking_sensor.ReceiverOffset.value = avango.gua.make_identity_mat(
)
self.pointer_tracking_sensor.TransmitterOffset.value = TRACKING_TRANSMITTER_OFFSET
self.pointer_device_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.pointer_device_sensor.Station.value = POINTER_STATION_NAME
# init nodes and geometries
_loader = avango.gua.nodes.TriMeshLoader()
self.offset_node = avango.gua.nodes.TransformNode(Name="offset_node")
self.offset_node.Transform.value = avango.gua.make_trans_mat(
0.0, 0.0, 0.0)
SCENEGRAPH.Root.value.Children.value.append(self.offset_node)
self.proxy_geo = _loader.create_geometry_from_file(
"proxy_geo", "data/objects/sphere.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.proxy_geo.Transform.value = avango.gua.make_scale_mat(0.2)
SCENEGRAPH.Root.value.Children.value.append(self.proxy_geo)
self.proxy_geo.Material.value.set_uniform(
"Color", avango.gua.Vec4(0.0, 0.5, 1.0, 1.0))
self.rot_helper = avango.gua.nodes.TransformNode(Name="rot_helper")
self.offset_node.Children.value.append(self.rot_helper)
#self.proxy_geo.Tags.value = [] # set geometry invisible
self.ray_transform = avango.gua.nodes.TransformNode(
Name="ray_transform")
PARENT_NODE.Children.value.append(self.ray_transform)
self.ray_geometry = _loader.create_geometry_from_file(
"ray_geometry", "data/objects/cylinder.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.ray_transform.Children.value.append(self.ray_geometry)
self.ray_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(1.0, 0.0, 0.0, 1.0))
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.ray_length * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,self.ray_length)
self.intersection_point_geometry = _loader.create_geometry_from_file(
"intersection_point_geometry", "data/objects/sphere.obj",
avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(
self.intersection_point_geometry)
self.intersection_point_geometry.Tags.value = [
"invisible"
] # set geometry invisible
self.maneuvering_point_geometry = _loader.create_geometry_from_file(
"maneuvering_point_geometry", "data/objects/sphere.obj",
avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(
self.maneuvering_point_geometry)
self.maneuvering_point_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(1.0, 0.0, 0.0, 0.25))
self.maneuvering_point_geometry.Tags.value = [
"invisible"
] # set geometry invisible
## init Navidget nodes
self.navidget_node = avango.gua.nodes.TransformNode(
Name="navidget_node")
SCENEGRAPH.Root.value.Children.value.append(self.navidget_node)
self.navidget_node.Tags.value = ["invisible"]
self.navidget_sphere = avango.gua.nodes.TransformNode(
Name="navidget_sphere")
self.navidget_node.Children.value.append(self.navidget_sphere)
self.navidget_sphere_geometry = _loader.create_geometry_from_file(
"navidget_sphere_geometry", "data/objects/sphere.obj",
avango.gua.LoaderFlags.DEFAULTS
| avango.gua.LoaderFlags.MAKE_PICKABLE)
self.navidget_node.Children.value.append(self.navidget_sphere_geometry)
self.navidget_sphere_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(0.0, 1.0, 1.0, 0.25))
self.navidget_sphere_geometry.Transform.value = avango.gua.make_scale_mat(
self.navidget_sphere_size)
self.navidget_stick = avango.gua.nodes.TransformNode(
Name="navidget_stick")
self.navidget_node.Children.value.append(self.navidget_stick)
self.navidget_stick_geometry = _loader.create_geometry_from_file(
"navidget_stick_geometry", "data/objects/cylinder.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.navidget_stick.Children.value.append(self.navidget_stick_geometry)
self.navidget_stick_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(1.0, 0.0, 1.0, 1.0))
self.navidget_stick_geometry.Transform.value = avango.gua.make_scale_mat(
0.015, 0.015, self.navidget_sphere_size * 2.0)
self.navidget_camera_geometry = _loader.create_geometry_from_file(
"navidget_camera_geometry", "data/objects/cam.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.navidget_node.Children.value.append(self.navidget_camera_geometry)
#self.navidget_camera_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,0.0,0.0,1.0))
self.navidget_camera_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size * 2.0) * \
avango.gua.make_scale_mat(3.0)
# init ray intersection for target identification
self.intersection = Intersection(SCENEGRAPH=SCENEGRAPH)
self.navidget_intersection = Intersection(SCENEGRAPH=SCENEGRAPH)
self.navidget_intersection.white_list = ["navidget_sphere"]
self.navidget_intersection.black_list = [""]
self.frame_trigger = avango.script.nodes.Update(
Callback=self.frame_callback, Active=True)
# init field connections
self.mf_dof.connect_from(MF_NAV_DOF)
self.sf_pointer_button0.connect_from(
self.pointer_device_sensor.Button0)
self.sf_pointer_button1.connect_from(
self.pointer_device_sensor.Button1)
self.ray_transform.Transform.connect_from(
self.pointer_tracking_sensor.Matrix)
class SteeringNavigation(avango.script.Script):
### fields ###
## input fields
mf_dof = avango.MFFloat()
mf_dof.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # init 7 channels
sf_pointer_button0 = avango.SFBool()
sf_pointer_button1 = avango.SFBool()
sf_head_mat = avango.gua.SFMatrix4()
sf_head_mat.value = avango.gua.make_trans_mat(0.0, 0.0, 1.0)
## internal fields
mf_pointer_pick_result = avango.gua.MFPickResult()
## output fields
sf_nav_mat = avango.gua.SFMatrix4()
sf_nav_mat.value = avango.gua.make_identity_mat()
### constructor
def __init__(self):
self.super(SteeringNavigation).__init__()
def my_constructor(
self,
SCENEGRAPH,
PARENT_NODE,
MF_NAV_DOF,
POINTER_STATION_NAME,
POINTER_TRACKING_NAME,
TRACKING_TRANSMITTER_OFFSET,
):
self.scenegraph = SCENEGRAPH
self.parent_node = PARENT_NODE
### parameters ###
self.ray_length = 100.0 # in meters
self.ray_thickness = 0.01 # in meters
self.intersection_point_size = 0.025 # in meters
self.navidget_duration = 10.0 # in seconds
self.navidget_sphere_size = 0.5 # in meters
### variables ###
self.navigation_mode = 0 # 0 = steering mode, 1 = maneuvering mode, 3 = Navidget target-mode, 4 = Navidget animation-mode
self.navidget_start_pos = None
self.navidget_target_pos = None
self.navidget_start_quat = None
self.navidget_target_quat = None
self.navidget_start_time = None
### navigation variables
self.rotation_center = None
self.first_pick_result = None
self.rot_x_accum_value = 0.0
self.rot_y_accum_value = 0.0
self.manu_distance = 0.0
self.offset_trans_mat = avango.gua.make_identity_mat()
self.offset_rot_mat = avango.gua.make_identity_mat()
self.navidget_trans_mat = avango.gua.make_identity_mat()
self.navidget_rot_mat = avango.gua.make_identity_mat()
self.animation_state = "start"
### resources ###
# init pointer sensors
self.pointer_tracking_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.pointer_tracking_sensor.Station.value = POINTER_TRACKING_NAME
self.pointer_tracking_sensor.ReceiverOffset.value = avango.gua.make_identity_mat(
)
self.pointer_tracking_sensor.TransmitterOffset.value = TRACKING_TRANSMITTER_OFFSET
self.pointer_device_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.pointer_device_sensor.Station.value = POINTER_STATION_NAME
# init nodes and geometries
_loader = avango.gua.nodes.TriMeshLoader()
self.offset_node = avango.gua.nodes.TransformNode(Name="offset_node")
self.offset_node.Transform.value = avango.gua.make_trans_mat(
0.0, 0.0, 0.0)
SCENEGRAPH.Root.value.Children.value.append(self.offset_node)
self.proxy_geo = _loader.create_geometry_from_file(
"proxy_geo", "data/objects/sphere.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.proxy_geo.Transform.value = avango.gua.make_scale_mat(0.2)
SCENEGRAPH.Root.value.Children.value.append(self.proxy_geo)
self.proxy_geo.Material.value.set_uniform(
"Color", avango.gua.Vec4(0.0, 0.5, 1.0, 1.0))
self.rot_helper = avango.gua.nodes.TransformNode(Name="rot_helper")
self.offset_node.Children.value.append(self.rot_helper)
#self.proxy_geo.Tags.value = [] # set geometry invisible
self.ray_transform = avango.gua.nodes.TransformNode(
Name="ray_transform")
PARENT_NODE.Children.value.append(self.ray_transform)
self.ray_geometry = _loader.create_geometry_from_file(
"ray_geometry", "data/objects/cylinder.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.ray_transform.Children.value.append(self.ray_geometry)
self.ray_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(1.0, 0.0, 0.0, 1.0))
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.ray_length * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,self.ray_length)
self.intersection_point_geometry = _loader.create_geometry_from_file(
"intersection_point_geometry", "data/objects/sphere.obj",
avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(
self.intersection_point_geometry)
self.intersection_point_geometry.Tags.value = [
"invisible"
] # set geometry invisible
self.maneuvering_point_geometry = _loader.create_geometry_from_file(
"maneuvering_point_geometry", "data/objects/sphere.obj",
avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(
self.maneuvering_point_geometry)
self.maneuvering_point_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(1.0, 0.0, 0.0, 0.25))
self.maneuvering_point_geometry.Tags.value = [
"invisible"
] # set geometry invisible
## init Navidget nodes
self.navidget_node = avango.gua.nodes.TransformNode(
Name="navidget_node")
SCENEGRAPH.Root.value.Children.value.append(self.navidget_node)
self.navidget_node.Tags.value = ["invisible"]
self.navidget_sphere = avango.gua.nodes.TransformNode(
Name="navidget_sphere")
self.navidget_node.Children.value.append(self.navidget_sphere)
self.navidget_sphere_geometry = _loader.create_geometry_from_file(
"navidget_sphere_geometry", "data/objects/sphere.obj",
avango.gua.LoaderFlags.DEFAULTS
| avango.gua.LoaderFlags.MAKE_PICKABLE)
self.navidget_node.Children.value.append(self.navidget_sphere_geometry)
self.navidget_sphere_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(0.0, 1.0, 1.0, 0.25))
self.navidget_sphere_geometry.Transform.value = avango.gua.make_scale_mat(
self.navidget_sphere_size)
self.navidget_stick = avango.gua.nodes.TransformNode(
Name="navidget_stick")
self.navidget_node.Children.value.append(self.navidget_stick)
self.navidget_stick_geometry = _loader.create_geometry_from_file(
"navidget_stick_geometry", "data/objects/cylinder.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.navidget_stick.Children.value.append(self.navidget_stick_geometry)
self.navidget_stick_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(1.0, 0.0, 1.0, 1.0))
self.navidget_stick_geometry.Transform.value = avango.gua.make_scale_mat(
0.015, 0.015, self.navidget_sphere_size * 2.0)
self.navidget_camera_geometry = _loader.create_geometry_from_file(
"navidget_camera_geometry", "data/objects/cam.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.navidget_node.Children.value.append(self.navidget_camera_geometry)
#self.navidget_camera_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,0.0,0.0,1.0))
self.navidget_camera_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size * 2.0) * \
avango.gua.make_scale_mat(3.0)
# init ray intersection for target identification
self.intersection = Intersection(SCENEGRAPH=SCENEGRAPH)
self.navidget_intersection = Intersection(SCENEGRAPH=SCENEGRAPH)
self.navidget_intersection.white_list = ["navidget_sphere"]
self.navidget_intersection.black_list = [""]
self.frame_trigger = avango.script.nodes.Update(
Callback=self.frame_callback, Active=True)
# init field connections
self.mf_dof.connect_from(MF_NAV_DOF)
self.sf_pointer_button0.connect_from(
self.pointer_device_sensor.Button0)
self.sf_pointer_button1.connect_from(
self.pointer_device_sensor.Button1)
self.ray_transform.Transform.connect_from(
self.pointer_tracking_sensor.Matrix)
### callbacks ###
@field_has_changed(mf_dof)
def mf_dof_changed(self):
## deffine minimum translation and roattion to trigger the navigation
_min_translation = 0.01
_min_rotation = 0.01
## handle translation input
_x = self.mf_dof.value[0]
_y = self.mf_dof.value[1]
_z = self.mf_dof.value[2]
_movement_vector = avango.gua.Vec3(_x, _y, _z)
## handle rotation input
_rx = self.mf_dof.value[3]
_ry = self.mf_dof.value[4]
_rz = self.mf_dof.value[5]
_rotation_vector = avango.gua.Vec3(_rx, _ry, _rz)
# print(_rotation_vector)
if self.navigation_mode == 0: # steering mode
## implement steering input
### translation
# check for each axis wheather the input value is beyond the minimum defined translation
# if the translation is bigger than the minimum translation
# add the min translation with the sign of current translation to the input translation
if (abs(_movement_vector.x) < _min_translation):
_movement_vector.x = 0
else:
_movement_vector.x -= math.copysign(_min_translation,
_movement_vector.x)
if (abs(_movement_vector.y) < _min_translation):
_movement_vector.y = 0
else:
_movement_vector.y -= math.copysign(_min_translation,
_movement_vector.y)
if (abs(_movement_vector.z) < _min_translation):
_movement_vector.z = 0
else:
_movement_vector.z -= math.copysign(_min_translation,
_movement_vector.z)
# update the _movement_vector by setting it to the 3rd power of the previous movement version
_movement_vector.x = pow(_movement_vector.x, 3) * 2
_movement_vector.y = pow(_movement_vector.y, 3) * 2
_movement_vector.z = pow(_movement_vector.z, 3) * 2
# print(_rotation_vector.x)
### rotation
# same procedure as translation
if (abs(_rotation_vector.x) < _min_rotation):
_rotation_vector.x = 0
else:
_rotation_vector.x -= math.copysign(_min_rotation,
_rotation_vector.x)
if (abs(_rotation_vector.y) < _min_rotation):
_rotation_vector.y = 0
else:
_rotation_vector.y -= math.copysign(_min_rotation,
_rotation_vector.y)
if (abs(_rotation_vector.z) < _min_rotation):
_rotation_vector.z = 0
else:
_rotation_vector.z -= math.copysign(_min_rotation,
_rotation_vector.z)
_rotation_vector.x = pow(_rotation_vector.x, 3) * 4
_rotation_vector.y = pow(_rotation_vector.y, 3) * 4
_rotation_vector.z = pow(_rotation_vector.z, 3) * 4
# print(_rotation_vector.x)
# get a quaternion for the current rotation
_current_rotation = self.sf_nav_mat.value.get_rotate()
# print(type(self.sf_nav_mat.value))
# print(type(self.sf_nav_mat.value.get_rotate()))
# create rotation matrix by getting the values of the quaternion
_current_rotation_mat = avango.gua.make_rot_mat(
_current_rotation.get_angle(), _current_rotation.get_axis())
# rotation matrix
_rotation_matrix = avango.gua.make_rot_mat(_rotation_vector.x,1,0,0) * \
avango.gua.make_rot_mat(_rotation_vector.y,0,1,0) * \
avango.gua.make_rot_mat(_rotation_vector.z,0,0,1)
# create new movement matrix containing the
_movement_matrix = _current_rotation_mat * avango.gua.make_trans_mat(
_movement_vector)
_final_movement = avango.gua.make_trans_mat( self.sf_nav_mat.value.get_translate() + \
_movement_matrix .get_translate())
# apply final movement to the navigation
self.sf_nav_mat.value = _final_movement * _current_rotation_mat * _rotation_matrix
elif self.navigation_mode == 1: # maneuvering mode
# _trans_world_vec = self.rotation_center - self.sf_nav_mat.value.get_translate()
# _quat = avango.gua.make_inverse_mat(self.sf_nav_mat.value).get_rotate()
# _adjusted_trans = avango.gua.make_rot_mat(_quat.get_angle(), _quat.get_axis()) * _trans_world_vec
# #_adjusted_trans.z -= _movement_vector.z
# self.manu_distance = _movement_vector.z
# _zoom_mat = avango.gua.make_trans_mat(0.0,0.0,self.manu_distance )
# _zoom_mat_inv = avango.gua.make_inverse_mat(_zoom_mat)
# _adjusted_trans_vec = avango.gua.Vec3(_adjusted_trans.x, _adjusted_trans.y, _adjusted_trans.z)
# _adjusted_trans_mat = avango.gua.make_trans_mat(_adjusted_trans_vec)
# _adjusted_trans_mat_inv = avango.gua.make_inverse_mat(_adjusted_trans_mat)
# ## rotation
# _rotation_matrix = avango.gua.make_rot_mat(_rotation_vector.x,1,0,0) * \
# avango.gua.make_rot_mat(_rotation_vector.y,0,1,0)
# self.sf_nav_mat.value = self.sf_nav_mat.value * _adjusted_trans_mat * _rotation_matrix * _adjusted_trans_mat_inv * _zoom_mat
self.rot_x_accum_value -= _rotation_vector.x
self.rot_y_accum_value -= _rotation_vector.y
self.manu_distance += _movement_vector.z
#_rot_mat = avango.gua.make_rot_mat(self.rot_accum_value, 0,1,0)
_rot_mat = avango.gua.make_rot_mat(self.rot_x_accum_value,1,0,0) * \
avango.gua.make_rot_mat(self.rot_y_accum_value,0,1,0)
self.rot_helper.Transform.value = avango.gua.make_trans_mat(
0.0, 0.0, self.manu_distance)
self.offset_node.Transform.value = self.offset_trans_mat * self.offset_rot_mat * _rot_mat
_manu_pos = self.rot_helper.WorldTransform.value.get_translate()
#self.sf_nav_mat.value = avango.gua.make_trans_mat(_manu_pos)
self.sf_nav_mat.value = self.rot_helper.WorldTransform.value
@field_has_changed(sf_pointer_button0)
def sf_pointer_button0_changed(self):
if self.sf_pointer_button0.value == True:
self.set_navigation_mode(1)
# if there are objects hit by the ray
if len(self.mf_pointer_pick_result.value) > 0:
self.first_pick_result = self.mf_pointer_pick_result.value[0]
else:
self.first_pick_result = None
if self.first_pick_result is not None:
_obj_pos = self.first_pick_result.WorldPosition.value # world position of selected object
# self.rotation_center = _obj_pos
# _nav_rot_q = self.sf_nav_mat.value.get_rotate()
# self.nav_rot = avango.gua.make_rot_mat(_nav_rot_q.get_angle(), _nav_rot_q.get_axis())
# self.rot_accum_value = avango.gua.make_identity_mat()
# self.last_rotation = avango.gua.make_identity_mat()
# self.maneuvering_point_geometry.Tags.value = []
# self.maneuvering_point_geometry.Transform.value = avango.gua.make_trans_mat(self.rotation_center)
_obj_mat = avango.gua.make_trans_mat(_obj_pos)
p2 = self.parent_node.WorldTransform.value.get_translate()
_head_distance = self.sf_head_mat.value.get_translate()
self.manu_distance = (_obj_pos -
p2).length() #-0.6# +_head_distance.z
self.rot_helper.Transform.value = avango.gua.make_trans_mat(
0.0, 0.0, self.manu_distance)
print("_head_distance", _head_distance.z)
print("abstand obj - prox",
self.rot_helper.Transform.value.get_translate().length())
print("abstand obj - wir",
(_obj_pos - p2).length()) # + _head_distance.z)
#print("unserer pos",p2)
#print("prox pos",self.rot_helper.WorldTransform.value.get_translate())
# translate the indicator and make it visible
self.maneuvering_point_geometry.Transform.value = _obj_mat
self.maneuvering_point_geometry.Tags.value = []
self.offset_node.Transform.value = _obj_mat
p3 = self.offset_node.WorldTransform.value.get_translate()
#print("abstand zwischen rothelper und off",(p3-p1).length())
#print("abstand zwischen uns und off",(p3-p2).length())
#print("abstand zwischen uns und rot",(p1-p2).length())
vec1 = avango.gua.Vec3(0.0, 0.0, -1.0)
vec2 = p3 - p2
self.offset_trans_mat = _obj_mat
self.offset_rot_mat = self.get_rotation_matrix_between_vectors(
vec1, vec2)
self.offset_node.Transform.value = self.offset_trans_mat * self.offset_rot_mat
#print("prox pos",self.rot_helper.WorldTransform.value.get_translate())
self.rot_x_accum_value = 0.0
self.rot_y_accum_value = 0.0
#self.sf_nav_mat.value = self.rot_helper.Transform.value
# if there is no selected node hide the grab indicator
else:
self.maneuvering_point_geometry.Tags.value = ["invisible"]
self.rotation_center = avango.gua.make_identity_mat()
self.manu_distance = 0
else:
self.set_navigation_mode(0)
@field_has_changed(sf_pointer_button1)
def sf_pointer_button1_changed(self):
if self.sf_pointer_button1.value == True:
print("hallo")
self.set_navigation_mode(3)
"""
if len(self.mf_pointer_pick_result.value) > 0: # intersection found
_pick_result = self.mf_pointer_pick_result.value[0] # get first intersection target
## set initial Navidget GUI position and orientation
# if there are objects hit by the ray
else:
_pick_result = None
if _pick_result is not None:
_obj_pos = _pick_result.WorldPosition.value # world position of selected object
_obj_mat = avango.gua.make_trans_mat(_obj_pos)
self.navidget_node.Transform.value = _obj_mat
self.navidget_stick.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size) #* \
#avango.gua.make_scale_mat(0.015,0.015,self.navidget_sphere_size * 2.0)
self.navidget_camera_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size * 2.0) * \
avango.gua.make_scale_mat(3.0)
self.navidget_trans_mat = _obj_mat
"""
else:
## start Navidget animation-mode on button release
if self.navigation_mode == 3:
self.set_navigation_mode(4)
def frame_callback(self): # executed every frame when active
## calc intersection
_mf_pick_result = self.intersection.calc_pick_result(
PICK_MAT=self.ray_transform.WorldTransform.value,
PICK_LENGTH=self.ray_length)
self.mf_pointer_pick_result.value = _mf_pick_result.value
self.update_ray_parameters()
if self.navigation_mode == 3: # Navidget target-mode
self.update_navidget_parameters()
elif self.navigation_mode == 4: # Navidget animation-mode
self.sf_nav_mat.value = self.animate_navidget()
### functions ###
def set_navigation_mode(self, MODE):
self.navigation_mode = MODE
if self.navigation_mode == 0: # switch to Steering mode
# set other modes geometry invisible
self.maneuvering_point_geometry.Tags.value = ["invisible"]
self.navidget_node.Tags.value = ["invisible"]
print("SWITCH TO STEERING MODE")
elif self.navigation_mode == 1: # switch to maneuvering mode
self.maneuvering_point_geometry.Tags.value = []
self.navidget_node.Tags.value = ["invisible"]
print("SWITCH TO MANEUVERING MODE")
elif self.navigation_mode == 3: # switch to Navidget target mode
self.navidget_sphere_geometry.Tags.value = []
self.navidget_sphere_geometry.Tags.value = ["navidget_sphere"]
self.navidget_node.Tags.value = []
if len(self.mf_pointer_pick_result.value
) > 0: # intersection found
_pick_result = self.mf_pointer_pick_result.value[
0] # get first intersection target
## set initial Navidget GUI position and orientation
# if there are objects hit by the ray
else:
_pick_result = None
if _pick_result is not None:
_obj_pos = _pick_result.WorldPosition.value # world position of selected object
_obj_mat = avango.gua.make_trans_mat(_obj_pos)
self.navidget_node.Transform.value = _obj_mat
self.navidget_stick.Transform.value = avango.gua.make_trans_mat(
0.0, 0.0, self.navidget_sphere_size) #* \
#avango.gua.make_scale_mat(0.015,0.015,self.navidget_sphere_size * 2.0)
self.navidget_camera_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size * 2.0) * \
avango.gua.make_scale_mat(3.0)
self.navidget_trans_mat = _obj_mat
print("SWITCH TO NAVIDGET TARGET-MODE")
elif self.navigation_mode == 4:
print("try animation")
## define start and target parameters for Navidget animation
self.navidget_start_pos = self.parent_node.WorldTransform.value.get_translate(
)
self.navidget_target_pos = self.navidget_camera_geometry.WorldTransform.value.get_translate(
)
self.navidget_start_quat = self.parent_node.WorldTransform.value.get_rotate(
)
self.navidget_target_quat = self.navidget_camera_geometry.WorldTransform.value.get_rotate(
)
self.navidget_start_time = time.time()
print("SWITCH TO NAVIDGET ANIMATION-MODE")
def update_ray_parameters(self):
if len(self.mf_pointer_pick_result.value) > 0: # intersection found
_pick_result = self.mf_pointer_pick_result.value[
0] # get first intersection target
_point = _pick_result.WorldPosition.value # intersection point in world coordinate system
_distance = (
_point -
self.ray_transform.WorldTransform.value.get_translate()
).length()
# update intersection point visualization
self.intersection_point_geometry.Tags.value = [
] # set geometry visible
self.intersection_point_geometry.Transform.value = avango.gua.make_trans_mat(_point) * \
avango.gua.make_scale_mat(self.intersection_point_size)
# update ray visualization (ray length)
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,_distance * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,_distance)
else: # no intersection found
# update intersection point visualization
self.intersection_point_geometry.Tags.value = [
"invisible"
] # set geometry invisible
# update ray visualization (ray length)
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.ray_length * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,self.ray_length)
def update_navidget_parameters(self):
if self.navigation_mode == 3: # maneuvering mode
_mf_pick_result = self.navidget_intersection.calc_pick_result(
PICK_MAT=self.ray_transform.WorldTransform.value,
PICK_LENGTH=self.ray_length)
# if there are objects hit by the ray
if len(_mf_pick_result.value) > 0:
_sphere_pick = _mf_pick_result.value[0]
else:
_sphere_pick = None
if _sphere_pick is not None:
_sphere_pick_pos = _sphere_pick.WorldPosition.value # world position of selected object
_sphere_pick_mat = avango.gua.make_trans_mat(_sphere_pick_pos)
self.proxy_geo.Transform.value = _sphere_pick_mat * avango.gua.make_scale_mat(
0.08)
######
_origin_axis = avango.gua.Vec3(
0.0, 0.0, -1.0
) #self.navidget_trans_mat.get_translate() + avango.gua.Vec3(0.0,0.0,1.0) # original rot
#_our_pos = self.parent_node.WorldTransform.value.get_translate()
_center = self.navidget_trans_mat.get_translate()
#vec1 = _hit_point-_origin_axis
#vec2 = _hit_point-_our_pos
#_origin_rot_mat = self.get_rotation_matrix_between_vectors(vec1, vec2)
#_hit_point = self.navidget_node.WorldTransform.value.get_translate()
#vec1 = _hit_point-_our_pos
vec1 = _center - _sphere_pick_pos
vec2 = _origin_axis
self.navidget_rot_mat = self.get_rotation_matrix_between_vectors(
vec2, vec1)
self.navidget_node.Transform.value = self.navidget_trans_mat * self.navidget_rot_mat
#print("prox pos",self.rot_helper.WorldTransform.value.get_translate())
def animate_navidget(self):
print("animphase 1")
_current_time = time.time()
_slerp_ratio = (_current_time -
self.navidget_start_time) / self.navidget_duration
# last animatuion step - finish animation afterwards
if _slerp_ratio >= 1:
self.set_navigation_mode(0)
print("slerp done")
_new_io_mat = avango.gua.make_trans_mat(navidget_target_pos) *\
avango.gua.make_rot_mat(navidget_target_quat)
return _new_io_mat
# move sun
else:
print("animating", _slerp_ratio)
_factor = _slerp_ratio
_new_pos = (self.navidget_start_pos *
(1 - _factor)) + (self.navidget_target_pos * _factor)
_new_quat = self.slerp(self.navidget_start_quat,
self.navidget_target_quat, _slerp_ratio)
print("animating4", _new_pos)
_new_io_mat = avango.gua.make_trans_mat(_new_pos) *\
avango.gua.make_rot_mat(_new_quat)
print("gogogo")
return _new_io_mat
def slerp(self, qa, qb, SLERP_RATIO):
_quat = qa.slerp_to(qb, SLERP_RATIO)
return _quat
def get_rotation_matrix_between_vectors(self, VEC1, VEC2):
VEC1.normalize()
VEC2.normalize()
_angle = math.degrees(math.acos(VEC1.dot(VEC2)))
_axis = VEC1.cross(VEC2)
return avango.gua.make_rot_mat(_angle, _axis)
class GroundFollowing(avango.script.Script):
## input fields
sf_mat = avango.gua.SFMatrix4()
## ouput fields
sf_modified_mat = avango.gua.SFMatrix4()
## internal fields
mf_pick_result = avango.gua.MFPickResult()
## constructor
def __init__(self):
self.super(GroundFollowing).__init__()
def my_constructor(
self,
SCENEGRAPH=None,
START_MATRIX=avango.gua.make_identity_mat(),
):
### parameters ###
self.fall_velocity = 0.003 # in meter/sec
self.pick_length = 1.0 # in meter
self.pick_direction = avango.gua.Vec3(0.0, -1.0, 0.0)
### variables ###
self.fall_vec = avango.gua.Vec3()
self.lf_time = time.time()
## init internal sub-classes
self.gravity_intersection = Intersection()
self.gravity_intersection.my_constructor(SCENEGRAPH, self.sf_mat,
self.pick_length,
self.pick_direction)
## init field connections
self.mf_pick_result.connect_from(
self.gravity_intersection.mf_pick_result)
## set initial state
self.sf_modified_mat.value = START_MATRIX
### callback functions ###
def evaluate(
self): # evaluated once every frame if any input field has changed
self.lf_time = time.time(
) # save absolute time of last frame (required for frame-rate independent mapping)
#print(len(self.mf_pick_result.value))
if len(self.mf_pick_result.value) > 0: # intersection found
## compute gravity response
_pick_result = self.mf_pick_result.value[
0] # get first intersection target from list
_distance = _pick_result.Distance.value # distance from ray matrix to intersection point
_distance -= 0.025 # subtract half avatar height from distance
_distance -= self.fall_velocity
if _distance > 0.01: # avatar above ground
self.fall_vec.y = self.fall_velocity * -1.0
else: # avatar (almost) on ground
self.fall_vec.y = 0.0
#print(self.fall_vec)
self.sf_modified_mat.value = \
avango.gua.make_trans_mat(self.fall_vec) * \
self.sf_mat.value
else: # no intersection found
self.sf_modified_mat.value = self.sf_mat.value # no changes needed
def my_constructor(self,
SCENEGRAPH,
PARENT_NODE,
MF_NAV_DOF,
POINTER_STATION_NAME,
POINTER_TRACKING_NAME,
TRACKING_TRANSMITTER_OFFSET,
):
### parameters ###
self.ray_length = 100.0 # in meters
self.ray_thickness = 0.01 # in meters
self.intersection_point_size = 0.025 # in meters
self.navidget_duration = 3.0 # in seconds
self.navidget_sphere_size = 0.5 # in meters
self.attenuation_factor = 1
self.rot_center_offset = avango.gua.Vec3(0.0,0.0,0.0)
### variables ###
self.navigation_mode = 0 # 0 = steering mode, 1 = maneuvering mode, 3 = Navidget target-mode, 4 = Navidget animation-mode
self.navidget_start_pos = None
self.navidget_target_pos = None
self.navidget_start_quat = None
self.navidget_target_quat = None
self.navidget_start_time = None
### resources ###
# init pointer sensors
self.pointer_tracking_sensor = avango.daemon.nodes.DeviceSensor(DeviceService = avango.daemon.DeviceService())
self.pointer_tracking_sensor.Station.value = POINTER_TRACKING_NAME
self.pointer_tracking_sensor.ReceiverOffset.value = avango.gua.make_identity_mat()
self.pointer_tracking_sensor.TransmitterOffset.value = TRACKING_TRANSMITTER_OFFSET
self.pointer_device_sensor = avango.daemon.nodes.DeviceSensor(DeviceService = avango.daemon.DeviceService())
self.pointer_device_sensor.Station.value = POINTER_STATION_NAME
# init nodes and geometries
_loader = avango.gua.nodes.TriMeshLoader()
self.ray_transform = avango.gua.nodes.TransformNode(Name = "ray_transform")
PARENT_NODE.Children.value.append(self.ray_transform)
self.ray_geometry = _loader.create_geometry_from_file("ray_geometry", "data/objects/cylinder.obj", avango.gua.LoaderFlags.DEFAULTS)
self.ray_transform.Children.value.append(self.ray_geometry)
self.ray_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0, 0.0, 0.0, 1.0))
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.ray_length * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,self.ray_length)
self.intersection_point_geometry = _loader.create_geometry_from_file("intersection_point_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(self.intersection_point_geometry)
self.intersection_point_geometry.Tags.value = ["invisible"] # set geometry invisible
self.maneuvering_point_geometry = _loader.create_geometry_from_file("maneuvering_point_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(self.maneuvering_point_geometry)
self.maneuvering_point_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0, 0.0, 0.0, 0.25))
self.maneuvering_point_geometry.Tags.value = ["invisible"] # set geometry invisible
## init Navidget nodes
self.navidget_node = avango.gua.nodes.TransformNode(Name = "navidget_node")
SCENEGRAPH.Root.value.Children.value.append(self.navidget_node)
self.navidget_node.Tags.value = ["invisible"]
self.navidget_sphere_geometry = _loader.create_geometry_from_file("navidget_sphere_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS | avango.gua.LoaderFlags.MAKE_PICKABLE)
self.navidget_node.Children.value.append(self.navidget_sphere_geometry)
self.navidget_sphere_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,1.0,1.0,0.25))
self.navidget_sphere_geometry.Transform.value = avango.gua.make_scale_mat(self.navidget_sphere_size)
self.navidget_stick_geometry = _loader.create_geometry_from_file("navidget_stick_geometry", "data/objects/cylinder.obj", avango.gua.LoaderFlags.DEFAULTS)
self.navidget_node.Children.value.append(self.navidget_stick_geometry)
self.navidget_stick_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,0.0,0.0,1.0))
self.navidget_stick_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size) * \
avango.gua.make_scale_mat(0.015,0.015,self.navidget_sphere_size * 2.0)
self.navidget_camera_geometry = _loader.create_geometry_from_file("navidget_camera_geometry", "data/objects/cam.obj", avango.gua.LoaderFlags.DEFAULTS)
self.navidget_node.Children.value.append(self.navidget_camera_geometry)
#self.navidget_camera_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,0.0,0.0,1.0))
self.navidget_camera_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size * 2.0) * \
avango.gua.make_scale_mat(3.0)
# init ray intersection for target identification
self.intersection = Intersection(SCENEGRAPH = SCENEGRAPH)
self.frame_trigger = avango.script.nodes.Update(Callback = self.frame_callback, Active = True)
# init field connections
self.mf_dof.connect_from(MF_NAV_DOF)
self.sf_pointer_button0.connect_from(self.pointer_device_sensor.Button0)
self.sf_pointer_button1.connect_from(self.pointer_device_sensor.Button1)
self.ray_transform.Transform.connect_from(self.pointer_tracking_sensor.Matrix)
class SteeringNavigation(avango.script.Script):
### fields ###
## input fields
mf_dof = avango.MFFloat()
mf_dof.value = [0.0,0.0,0.0,0.0,0.0,0.0,0.0] # init 7 channels
sf_pointer_button0 = avango.SFBool()
sf_pointer_button1 = avango.SFBool()
sf_head_mat = avango.gua.SFMatrix4()
sf_head_mat.value = avango.gua.make_trans_mat(0.0,0.0,1.0)
## internal fields
mf_pointer_pick_result = avango.gua.MFPickResult()
## output fields
sf_nav_mat = avango.gua.SFMatrix4()
sf_nav_mat.value = avango.gua.make_identity_mat()
### constructor
def __init__(self):
self.super(SteeringNavigation).__init__()
def my_constructor(self,
SCENEGRAPH,
PARENT_NODE,
MF_NAV_DOF,
POINTER_STATION_NAME,
POINTER_TRACKING_NAME,
TRACKING_TRANSMITTER_OFFSET,
):
### parameters ###
self.ray_length = 100.0 # in meters
self.ray_thickness = 0.01 # in meters
self.intersection_point_size = 0.025 # in meters
self.navidget_duration = 3.0 # in seconds
self.navidget_sphere_size = 0.5 # in meters
self.attenuation_factor = 1
self.rot_center_offset = avango.gua.Vec3(0.0,0.0,0.0)
### variables ###
self.navigation_mode = 0 # 0 = steering mode, 1 = maneuvering mode, 3 = Navidget target-mode, 4 = Navidget animation-mode
self.navidget_start_pos = None
self.navidget_target_pos = None
self.navidget_start_quat = None
self.navidget_target_quat = None
self.navidget_start_time = None
### resources ###
# init pointer sensors
self.pointer_tracking_sensor = avango.daemon.nodes.DeviceSensor(DeviceService = avango.daemon.DeviceService())
self.pointer_tracking_sensor.Station.value = POINTER_TRACKING_NAME
self.pointer_tracking_sensor.ReceiverOffset.value = avango.gua.make_identity_mat()
self.pointer_tracking_sensor.TransmitterOffset.value = TRACKING_TRANSMITTER_OFFSET
self.pointer_device_sensor = avango.daemon.nodes.DeviceSensor(DeviceService = avango.daemon.DeviceService())
self.pointer_device_sensor.Station.value = POINTER_STATION_NAME
# init nodes and geometries
_loader = avango.gua.nodes.TriMeshLoader()
self.ray_transform = avango.gua.nodes.TransformNode(Name = "ray_transform")
PARENT_NODE.Children.value.append(self.ray_transform)
self.ray_geometry = _loader.create_geometry_from_file("ray_geometry", "data/objects/cylinder.obj", avango.gua.LoaderFlags.DEFAULTS)
self.ray_transform.Children.value.append(self.ray_geometry)
self.ray_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0, 0.0, 0.0, 1.0))
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.ray_length * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,self.ray_length)
self.intersection_point_geometry = _loader.create_geometry_from_file("intersection_point_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(self.intersection_point_geometry)
self.intersection_point_geometry.Tags.value = ["invisible"] # set geometry invisible
self.maneuvering_point_geometry = _loader.create_geometry_from_file("maneuvering_point_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(self.maneuvering_point_geometry)
self.maneuvering_point_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0, 0.0, 0.0, 0.25))
self.maneuvering_point_geometry.Tags.value = ["invisible"] # set geometry invisible
## init Navidget nodes
self.navidget_node = avango.gua.nodes.TransformNode(Name = "navidget_node")
SCENEGRAPH.Root.value.Children.value.append(self.navidget_node)
self.navidget_node.Tags.value = ["invisible"]
self.navidget_sphere_geometry = _loader.create_geometry_from_file("navidget_sphere_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS | avango.gua.LoaderFlags.MAKE_PICKABLE)
self.navidget_node.Children.value.append(self.navidget_sphere_geometry)
self.navidget_sphere_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,1.0,1.0,0.25))
self.navidget_sphere_geometry.Transform.value = avango.gua.make_scale_mat(self.navidget_sphere_size)
self.navidget_stick_geometry = _loader.create_geometry_from_file("navidget_stick_geometry", "data/objects/cylinder.obj", avango.gua.LoaderFlags.DEFAULTS)
self.navidget_node.Children.value.append(self.navidget_stick_geometry)
self.navidget_stick_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,0.0,0.0,1.0))
self.navidget_stick_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size) * \
avango.gua.make_scale_mat(0.015,0.015,self.navidget_sphere_size * 2.0)
self.navidget_camera_geometry = _loader.create_geometry_from_file("navidget_camera_geometry", "data/objects/cam.obj", avango.gua.LoaderFlags.DEFAULTS)
self.navidget_node.Children.value.append(self.navidget_camera_geometry)
#self.navidget_camera_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,0.0,0.0,1.0))
self.navidget_camera_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size * 2.0) * \
avango.gua.make_scale_mat(3.0)
# init ray intersection for target identification
self.intersection = Intersection(SCENEGRAPH = SCENEGRAPH)
self.frame_trigger = avango.script.nodes.Update(Callback = self.frame_callback, Active = True)
# init field connections
self.mf_dof.connect_from(MF_NAV_DOF)
self.sf_pointer_button0.connect_from(self.pointer_device_sensor.Button0)
self.sf_pointer_button1.connect_from(self.pointer_device_sensor.Button1)
self.ray_transform.Transform.connect_from(self.pointer_tracking_sensor.Matrix)
### callbacks ###
@field_has_changed(mf_dof)
def mf_dof_changed(self):
## handle translation input
_x = self.mf_dof.value[0]
_y = self.mf_dof.value[1]
_z = self.mf_dof.value[2]
_trans_vec = avango.gua.Vec3(_x, _y, _z) * self.attenuation_factor
_trans_input = _trans_vec.length()
if _trans_input > 0.0:
## transfer-function for translation
#_exponent = 3
_exponent = 2
_multiple = int(_trans_input)
_rest = _trans_input - _multiple
_factor = _multiple + pow(_rest, _exponent)
_trans_vec.normalize()
_trans_vec = _trans_vec * _factor * 0.1
## handle rotation input
_rx = self.mf_dof.value[3]
_ry = self.mf_dof.value[4]
_rz = self.mf_dof.value[5]
_rot_vec = avango.gua.Vec3(_rx, _ry, _rz) * self.attenuation_factor
_rot_input = _rot_vec.length()
## accumulate input
if self.navigation_mode == 0: # steering mode
## implement steering input
# ...
#x= value[0][3], y = value[1][3], z = value[2][3]
#Another option to get the value
#head_x = self.sf_head_mat.value.get_translate()[0]
head_x = self.sf_head_mat.value.get_element(0,3)
head_y = self.sf_head_mat.value.get_element(1,3)
head_z = self.sf_head_mat.value.get_element(2,3)
new_center_offset = avango.gua.Vec3(head_x,head_y,head_z)
print(new_center_offset)
if _trans_input or _rot_input > 0.0:
self.sf_nav_mat.value = self.sf_nav_mat.value * \
avango.gua.make_trans_mat(_trans_vec) * \
avango.gua.make_trans_mat(new_center_offset) * \
avango.gua.make_rot_mat(_rot_vec.y,0,1,0) * \
avango.gua.make_rot_mat(_rot_vec.x,1,0,0) * \
avango.gua.make_rot_mat(_rot_vec.z,0,0,1) * \
avango.gua.make_trans_mat(new_center_offset * -1)
elif self.navigation_mode == 1: # maneuvering mode
## implement maneuvering input
# ...
pass
@field_has_changed(sf_pointer_button0)
def sf_pointer_button0_changed(self):
## toogle between steering and maneuvering mode
# ...
pass
@field_has_changed(sf_pointer_button1)
def sf_pointer_button1_changed(self):
## enable Navidget target-mode on button press
# ...
## start Navidget animation-mode on button release
# ...
pass
def frame_callback(self): # executed every frame when active
## calc intersection
_mf_pick_result = self.intersection.calc_pick_result(PICK_MAT = self.ray_transform.WorldTransform.value, PICK_LENGTH = self.ray_length)
self.mf_pointer_pick_result.value = _mf_pick_result.value
self.update_ray_parameters()
if self.navigation_mode == 3: # Navidget target-mode
self.update_navidget_parameters()
elif self.navigation_mode == 4: # Navidget animation-mode
self.animate_navidget()
### functions ###
def set_navigation_mode(self, MODE):
self.navigation_mode = MODE
if self.navigation_mode == 0: # switch to Steering mode
# set other modes geometry invisible
self.maneuvering_point_geometry.Tags.value = ["invisible"]
self.navidget_node.Tags.value = ["invisible"]
print("SWITCH TO STEERING MODE")
elif self.navigation_mode == 1: # switch to maneuvering mode
self.maneuvering_point_geometry.Tags.value = []
print("SWITCH TO MANEUVERING MODE")
elif self.navigation_mode == 3: # switch to Navidget target mode
if len(self.mf_pointer_pick_result.value) > 0: # intersection found
_pick_result = self.mf_pointer_pick_result.value[0] # get first intersection target
## set initial Navidget GUI position and orientation
# ...
print("SWITCH TO NAVIDGET TARGET-MODE")
elif self.navigation_mode == 4:
## define start and target parameters for Navidget animation
# ...
print("SWITCH TO NAVIDGET ANIMATION-MODE")
def update_ray_parameters(self):
if len(self.mf_pointer_pick_result.value) > 0: # intersection found
_pick_result = self.mf_pointer_pick_result.value[0] # get first intersection target
_point = _pick_result.WorldPosition.value # intersection point in world coordinate system
_distance = (_point - self.ray_transform.WorldTransform.value.get_translate()).length()
# update intersection point visualization
self.intersection_point_geometry.Tags.value = [] # set geometry visible
self.intersection_point_geometry.Transform.value = avango.gua.make_trans_mat(_point) * \
avango.gua.make_scale_mat(self.intersection_point_size)
# update ray visualization (ray length)
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,_distance * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,_distance)
else: # no intersection found
# update intersection point visualization
self.intersection_point_geometry.Tags.value = ["invisible"] # set geometry invisible
# update ray visualization (ray length)
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.ray_length * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,self.ray_length)
def update_navidget_parameters(self):
## update Navidget gui elements if Navidget GUI-sphere is hit
# ...
## use the get_rotation_between_vectors() function to obtain the rotation matrix between a reference direction, e.g. Vec3(0.0,0.0,-1.0) the default viewing direction in world space, and a target direction
# ...
pass
def animate_navidget(self):
# use linear interpolation between two vectors to animate the position of the camera --> use function lerp_to() on vector
# ...
# use spherical linear interpolation between two quaternions to animate the orientation of the camera --> use function slerp_to on quaternion
# ...
# stop Navidget animation mode after animation duration has exceeded
# ...
pass
def get_rotation_matrix_between_vectors(self, VEC1, VEC2):
VEC1.normalize()
VEC2.normalize()
_angle = math.degrees(math.acos(VEC1.dot(VEC2)))
_axis = VEC1.cross(VEC2)
return avango.gua.make_rot_mat(_angle, _axis)
class SteeringNavigation(avango.script.Script):
### fields ###
## input fields
mf_dof = avango.MFFloat()
mf_dof.value = [0.0,0.0,0.0,0.0,0.0,0.0,0.0] # init 7 channels
## output fields
sf_nav_mat = avango.gua.SFMatrix4()
sf_nav_mat.value = avango.gua.make_identity_mat()
### constructor
def __init__(self):
self.super(SteeringNavigation).__init__()
### parameters ###
self.rot_center_offset = avango.gua.Vec3(0.0,0.0,0.0)
self.attenuation_factor = 1.0
def my_constructor(self, MF_DOF, ATTENUATION_FACTOR = 1.0, SCENEGRAPH = None, SCENE_ROOT = None, HOST_NAME = None):
self.mf_dof.connect_from(MF_DOF)
self.attenuation_factor = ATTENUATION_FACTOR
self.host_name = HOST_NAME
self.intersection = Intersection(SCENEGRAPH, BLACK_LIST = ["unpickable", "invisible"])
self.room_number = 1
self.speed_factor = 0.1
self.dist = 1
self.script = NavigationScript()
self.script.my_constructor(CLASS = self, ROOM_NUMBER = self.room_number)
@field_has_changed(mf_dof)
def mf_dof_changed(self):
## handle translation input
_x = self.mf_dof.value[0]
_y = self.mf_dof.value[1]
_z = self.mf_dof.value[2]
_trans_vec = avango.gua.Vec3(_x, _y, _z) * self.attenuation_factor
_trans_input = _trans_vec.length()
if _trans_input > 0.0:
## transfer-function for translation
_exponent = 2
_multiple = int(_trans_input)
_rest = _trans_input - _multiple
_factor = _multiple + pow(_rest, _exponent)
self.intersection.set_pick_mat(self.sf_nav_mat.value)
self.intersection.set_pick_direction(_trans_vec)
self.intersection.set_pick_length(20.0)
_mf_pick_result = self.intersection.compute_intersection()
_trans_vec.normalize()
_trans_vec = _trans_vec * _factor * self.speed_factor
for _pick_result in _mf_pick_result.value:
_node = _pick_result.Object.value
_distance = _pick_result.Distance.value * 20.0
_world_intersection_pos = _pick_result.WorldPosition.value
_distance2 = (_world_intersection_pos - self.sf_nav_mat.value.get_translate()).length()
if _distance < self.dist: # add radius of 0.5 m
_trans_vec = avango.gua.Vec3(0.0, 0.0, 0.0)
## handle rotation input
_rx = 0.0
_ry = self.mf_dof.value[4]
_rz = 0.0
_rot_vec = avango.gua.Vec3(_rx, _ry, _rz) * self.attenuation_factor
_rot_input = _rot_vec.length()
if (_trans_input or _rot_input) and self.host_name is not "athena" > 0.0:
## accumulate input
self.sf_nav_mat.value = self.sf_nav_mat.value * \
avango.gua.make_trans_mat(_trans_vec) * \
avango.gua.make_trans_mat(self.rot_center_offset) * \
avango.gua.make_rot_mat(_rot_vec.y,0,1,0) * \
avango.gua.make_rot_mat(_rot_vec.x,1,0,0) * \
avango.gua.make_rot_mat(_rot_vec.z,0,0,1) * \
avango.gua.make_trans_mat(self.rot_center_offset * -1)
#print("MATRIX", self.sf_nav_mat.value)
#print("Pos", self.sf_nav_mat.value.get_translate(), _trans_vec)
### functions ###
def set_start_transformation(self, MATRIX):
self.sf_nav_mat.value = MATRIX
def set_rotation_center_offset(self, OFFSET_VEC):
self.rot_center_offset = OFFSET_VEC
class SteeringNavigation(avango.script.Script):
### fields ###
## input fields
mf_dof = avango.MFFloat()
mf_dof.value = [0.0,0.0,0.0,0.0,0.0,0.0,0.0] # init 7 channels
sf_pointer_button0 = avango.SFBool()
sf_pointer_button1 = avango.SFBool()
sf_head_mat = avango.gua.SFMatrix4()
sf_head_mat.value = avango.gua.make_trans_mat(0.0,0.0,1.0)
## internal fields
mf_pointer_pick_result = avango.gua.MFPickResult()
## output fields
sf_nav_mat = avango.gua.SFMatrix4()
sf_nav_mat.value = avango.gua.make_identity_mat()
### constructor
def __init__(self):
self.super(SteeringNavigation).__init__()
def my_constructor(self,
SCENEGRAPH,
PARENT_NODE,
MF_NAV_DOF,
POINTER_STATION_NAME,
POINTER_TRACKING_NAME,
TRACKING_TRANSMITTER_OFFSET,
):
self.scenegraph = SCENEGRAPH
self.parent_node = PARENT_NODE
### parameters ###
self.ray_length = 100.0 # in meters
self.ray_thickness = 0.01 # in meters
self.intersection_point_size = 0.025 # in meters
self.navidget_duration = 10.0 # in seconds
self.navidget_sphere_size = 0.5 # in meters
### variables ###
self.navigation_mode = 0 # 0 = steering mode, 1 = maneuvering mode, 3 = Navidget target-mode, 4 = Navidget animation-mode
self.navidget_start_pos = None
self.navidget_target_pos = None
self.navidget_start_quat = None
self.navidget_target_quat = None
self.navidget_start_time = None
### navigation variables
self.rotation_center = None
self.first_pick_result = None
self.rot_x_accum_value = 0.0
self.rot_y_accum_value = 0.0
self.manu_distance = 0.0
self.offset_trans_mat = avango.gua.make_identity_mat()
self.offset_rot_mat = avango.gua.make_identity_mat()
self.navidget_trans_mat = avango.gua.make_identity_mat()
self.navidget_rot_mat = avango.gua.make_identity_mat()
self.animation_state = "start"
### resources ###
# init pointer sensors
self.pointer_tracking_sensor = avango.daemon.nodes.DeviceSensor(DeviceService = avango.daemon.DeviceService())
self.pointer_tracking_sensor.Station.value = POINTER_TRACKING_NAME
self.pointer_tracking_sensor.ReceiverOffset.value = avango.gua.make_identity_mat()
self.pointer_tracking_sensor.TransmitterOffset.value = TRACKING_TRANSMITTER_OFFSET
self.pointer_device_sensor = avango.daemon.nodes.DeviceSensor(DeviceService = avango.daemon.DeviceService())
self.pointer_device_sensor.Station.value = POINTER_STATION_NAME
# init nodes and geometries
_loader = avango.gua.nodes.TriMeshLoader()
self.offset_node = avango.gua.nodes.TransformNode(Name = "offset_node")
self.offset_node.Transform.value = avango.gua.make_trans_mat(0.0,0.0,0.0)
SCENEGRAPH.Root.value.Children.value.append(self.offset_node)
self.proxy_geo = _loader.create_geometry_from_file("proxy_geo", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS)
self.proxy_geo.Transform.value = avango.gua.make_scale_mat(0.2)
SCENEGRAPH.Root.value.Children.value.append(self.proxy_geo)
self.proxy_geo.Material.value.set_uniform("Color", avango.gua.Vec4(0.0, 0.5, 1.0, 1.0))
self.rot_helper = avango.gua.nodes.TransformNode(Name = "rot_helper")
self.offset_node.Children.value.append(self.rot_helper)
#self.proxy_geo.Tags.value = [] # set geometry invisible
self.ray_transform = avango.gua.nodes.TransformNode(Name = "ray_transform")
PARENT_NODE.Children.value.append(self.ray_transform)
self.ray_geometry = _loader.create_geometry_from_file("ray_geometry", "data/objects/cylinder.obj", avango.gua.LoaderFlags.DEFAULTS)
self.ray_transform.Children.value.append(self.ray_geometry)
self.ray_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0, 0.0, 0.0, 1.0))
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.ray_length * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,self.ray_length)
self.intersection_point_geometry = _loader.create_geometry_from_file("intersection_point_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(self.intersection_point_geometry)
self.intersection_point_geometry.Tags.value = ["invisible"] # set geometry invisible
self.maneuvering_point_geometry = _loader.create_geometry_from_file("maneuvering_point_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS)
SCENEGRAPH.Root.value.Children.value.append(self.maneuvering_point_geometry)
self.maneuvering_point_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0, 0.0, 0.0, 0.25))
self.maneuvering_point_geometry.Tags.value = ["invisible"] # set geometry invisible
## init Navidget nodes
self.navidget_node = avango.gua.nodes.TransformNode(Name = "navidget_node")
SCENEGRAPH.Root.value.Children.value.append(self.navidget_node)
self.navidget_node.Tags.value = ["invisible"]
self.navidget_sphere = avango.gua.nodes.TransformNode(Name = "navidget_sphere")
self.navidget_node.Children.value.append(self.navidget_sphere)
self.navidget_sphere_geometry = _loader.create_geometry_from_file("navidget_sphere_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS | avango.gua.LoaderFlags.MAKE_PICKABLE)
self.navidget_node.Children.value.append(self.navidget_sphere_geometry)
self.navidget_sphere_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(0.0,1.0,1.0,0.25))
self.navidget_sphere_geometry.Transform.value = avango.gua.make_scale_mat(self.navidget_sphere_size)
self.navidget_stick = avango.gua.nodes.TransformNode(Name = "navidget_stick")
self.navidget_node.Children.value.append(self.navidget_stick)
self.navidget_stick_geometry = _loader.create_geometry_from_file("navidget_stick_geometry", "data/objects/cylinder.obj", avango.gua.LoaderFlags.DEFAULTS)
self.navidget_stick.Children.value.append(self.navidget_stick_geometry)
self.navidget_stick_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,0.0,1.0,1.0))
self.navidget_stick_geometry.Transform.value = avango.gua.make_scale_mat(0.015,0.015,self.navidget_sphere_size * 2.0)
self.navidget_camera_geometry = _loader.create_geometry_from_file("navidget_camera_geometry", "data/objects/cam.obj", avango.gua.LoaderFlags.DEFAULTS)
self.navidget_node.Children.value.append(self.navidget_camera_geometry)
#self.navidget_camera_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,0.0,0.0,1.0))
self.navidget_camera_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size * 2.0) * \
avango.gua.make_scale_mat(3.0)
# init ray intersection for target identification
self.intersection = Intersection(SCENEGRAPH = SCENEGRAPH)
self.navidget_intersection = Intersection(SCENEGRAPH = SCENEGRAPH)
self.navidget_intersection.white_list = ["navidget_sphere"]
self.navidget_intersection.black_list = [""]
self.frame_trigger = avango.script.nodes.Update(Callback = self.frame_callback, Active = True)
# init field connections
self.mf_dof.connect_from(MF_NAV_DOF)
self.sf_pointer_button0.connect_from(self.pointer_device_sensor.Button0)
self.sf_pointer_button1.connect_from(self.pointer_device_sensor.Button1)
self.ray_transform.Transform.connect_from(self.pointer_tracking_sensor.Matrix)
### callbacks ###
@field_has_changed(mf_dof)
def mf_dof_changed(self):
## deffine minimum translation and roattion to trigger the navigation
_min_translation = 0.01
_min_rotation = 0.01
## handle translation input
_x = self.mf_dof.value[0]
_y = self.mf_dof.value[1]
_z = self.mf_dof.value[2]
_movement_vector = avango.gua.Vec3(_x, _y, _z)
## handle rotation input
_rx = self.mf_dof.value[3]
_ry = self.mf_dof.value[4]
_rz = self.mf_dof.value[5]
_rotation_vector = avango.gua.Vec3(_rx, _ry, _rz)
# print(_rotation_vector)
if self.navigation_mode == 0: # steering mode
## implement steering input
### translation
# check for each axis wheather the input value is beyond the minimum defined translation
# if the translation is bigger than the minimum translation
# add the min translation with the sign of current translation to the input translation
if(abs(_movement_vector.x) < _min_translation):
_movement_vector.x = 0
else:
_movement_vector.x -= math.copysign(_min_translation, _movement_vector.x)
if(abs(_movement_vector.y) < _min_translation):
_movement_vector.y = 0
else:
_movement_vector.y -= math.copysign(_min_translation, _movement_vector.y)
if(abs(_movement_vector.z) < _min_translation):
_movement_vector.z = 0
else:
_movement_vector.z -= math.copysign(_min_translation, _movement_vector.z)
# update the _movement_vector by setting it to the 3rd power of the previous movement version
_movement_vector.x = pow(_movement_vector.x, 3) * 2
_movement_vector.y = pow(_movement_vector.y, 3) * 2
_movement_vector.z = pow(_movement_vector.z, 3) * 2
# print(_rotation_vector.x)
### rotation
# same procedure as translation
if(abs(_rotation_vector.x) < _min_rotation):
_rotation_vector.x = 0
else:
_rotation_vector.x -= math.copysign(_min_rotation, _rotation_vector.x)
if(abs(_rotation_vector.y) < _min_rotation):
_rotation_vector.y = 0
else:
_rotation_vector.y -= math.copysign(_min_rotation, _rotation_vector.y)
if(abs(_rotation_vector.z) < _min_rotation):
_rotation_vector.z = 0
else:
_rotation_vector.z -= math.copysign(_min_rotation, _rotation_vector.z)
_rotation_vector.x = pow(_rotation_vector.x, 3) * 4
_rotation_vector.y = pow(_rotation_vector.y, 3) * 4
_rotation_vector.z = pow(_rotation_vector.z, 3) * 4
# print(_rotation_vector.x)
# get a quaternion for the current rotation
_current_rotation = self.sf_nav_mat.value.get_rotate()
# print(type(self.sf_nav_mat.value))
# print(type(self.sf_nav_mat.value.get_rotate()))
# create rotation matrix by getting the values of the quaternion
_current_rotation_mat = avango.gua.make_rot_mat( _current_rotation.get_angle(), _current_rotation.get_axis() )
# rotation matrix
_rotation_matrix = avango.gua.make_rot_mat(_rotation_vector.x,1,0,0) * \
avango.gua.make_rot_mat(_rotation_vector.y,0,1,0) * \
avango.gua.make_rot_mat(_rotation_vector.z,0,0,1)
# create new movement matrix containing the
_movement_matrix = _current_rotation_mat * avango.gua.make_trans_mat(_movement_vector)
_final_movement = avango.gua.make_trans_mat( self.sf_nav_mat.value.get_translate() + \
_movement_matrix .get_translate())
# apply final movement to the navigation
self.sf_nav_mat.value = _final_movement * _current_rotation_mat * _rotation_matrix
elif self.navigation_mode == 1: # maneuvering mode
# _trans_world_vec = self.rotation_center - self.sf_nav_mat.value.get_translate()
# _quat = avango.gua.make_inverse_mat(self.sf_nav_mat.value).get_rotate()
# _adjusted_trans = avango.gua.make_rot_mat(_quat.get_angle(), _quat.get_axis()) * _trans_world_vec
# #_adjusted_trans.z -= _movement_vector.z
# self.manu_distance = _movement_vector.z
# _zoom_mat = avango.gua.make_trans_mat(0.0,0.0,self.manu_distance )
# _zoom_mat_inv = avango.gua.make_inverse_mat(_zoom_mat)
# _adjusted_trans_vec = avango.gua.Vec3(_adjusted_trans.x, _adjusted_trans.y, _adjusted_trans.z)
# _adjusted_trans_mat = avango.gua.make_trans_mat(_adjusted_trans_vec)
# _adjusted_trans_mat_inv = avango.gua.make_inverse_mat(_adjusted_trans_mat)
# ## rotation
# _rotation_matrix = avango.gua.make_rot_mat(_rotation_vector.x,1,0,0) * \
# avango.gua.make_rot_mat(_rotation_vector.y,0,1,0)
# self.sf_nav_mat.value = self.sf_nav_mat.value * _adjusted_trans_mat * _rotation_matrix * _adjusted_trans_mat_inv * _zoom_mat
self.rot_x_accum_value -= _rotation_vector.x
self.rot_y_accum_value -= _rotation_vector.y
self.manu_distance += _movement_vector.z
#_rot_mat = avango.gua.make_rot_mat(self.rot_accum_value, 0,1,0)
_rot_mat = avango.gua.make_rot_mat(self.rot_x_accum_value,1,0,0) * \
avango.gua.make_rot_mat(self.rot_y_accum_value,0,1,0)
self.rot_helper.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.manu_distance)
self.offset_node.Transform.value = self.offset_trans_mat * self.offset_rot_mat * _rot_mat
_manu_pos = self.rot_helper.WorldTransform.value.get_translate()
#self.sf_nav_mat.value = avango.gua.make_trans_mat(_manu_pos)
self.sf_nav_mat.value = self.rot_helper.WorldTransform.value
@field_has_changed(sf_pointer_button0)
def sf_pointer_button0_changed(self):
if self.sf_pointer_button0.value == True:
self.set_navigation_mode(1)
# if there are objects hit by the ray
if len(self.mf_pointer_pick_result.value) > 0:
self.first_pick_result = self.mf_pointer_pick_result.value[0]
else:
self.first_pick_result = None
if self.first_pick_result is not None:
_obj_pos = self.first_pick_result.WorldPosition.value# world position of selected object
# self.rotation_center = _obj_pos
# _nav_rot_q = self.sf_nav_mat.value.get_rotate()
# self.nav_rot = avango.gua.make_rot_mat(_nav_rot_q.get_angle(), _nav_rot_q.get_axis())
# self.rot_accum_value = avango.gua.make_identity_mat()
# self.last_rotation = avango.gua.make_identity_mat()
# self.maneuvering_point_geometry.Tags.value = []
# self.maneuvering_point_geometry.Transform.value = avango.gua.make_trans_mat(self.rotation_center)
_obj_mat = avango.gua.make_trans_mat(_obj_pos)
p2 = self.parent_node.WorldTransform.value.get_translate()
_head_distance = self.sf_head_mat.value.get_translate()
self.manu_distance = (_obj_pos - p2).length() #-0.6# +_head_distance.z
self.rot_helper.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.manu_distance)
print("_head_distance",_head_distance.z)
print("abstand obj - prox",self.rot_helper.Transform.value.get_translate().length())
print("abstand obj - wir",(_obj_pos - p2).length())# + _head_distance.z)
#print("unserer pos",p2)
#print("prox pos",self.rot_helper.WorldTransform.value.get_translate())
# translate the indicator and make it visible
self.maneuvering_point_geometry.Transform.value = _obj_mat
self.maneuvering_point_geometry.Tags.value = []
self.offset_node.Transform.value = _obj_mat
p3 = self.offset_node.WorldTransform.value.get_translate()
#print("abstand zwischen rothelper und off",(p3-p1).length())
#print("abstand zwischen uns und off",(p3-p2).length())
#print("abstand zwischen uns und rot",(p1-p2).length())
vec1 = avango.gua.Vec3 (0.0,0.0,-1.0)
vec2 = p3-p2
self.offset_trans_mat = _obj_mat
self.offset_rot_mat = self.get_rotation_matrix_between_vectors(vec1, vec2 )
self.offset_node.Transform.value = self.offset_trans_mat * self.offset_rot_mat
#print("prox pos",self.rot_helper.WorldTransform.value.get_translate())
self.rot_x_accum_value = 0.0
self.rot_y_accum_value = 0.0
#self.sf_nav_mat.value = self.rot_helper.Transform.value
# if there is no selected node hide the grab indicator
else:
self.maneuvering_point_geometry.Tags.value = ["invisible"]
self.rotation_center = avango.gua.make_identity_mat()
self.manu_distance = 0
else:
self.set_navigation_mode(0)
@field_has_changed(sf_pointer_button1)
def sf_pointer_button1_changed(self):
if self.sf_pointer_button1.value == True:
print("hallo")
self.set_navigation_mode(3)
"""
if len(self.mf_pointer_pick_result.value) > 0: # intersection found
_pick_result = self.mf_pointer_pick_result.value[0] # get first intersection target
## set initial Navidget GUI position and orientation
# if there are objects hit by the ray
else:
_pick_result = None
if _pick_result is not None:
_obj_pos = _pick_result.WorldPosition.value # world position of selected object
_obj_mat = avango.gua.make_trans_mat(_obj_pos)
self.navidget_node.Transform.value = _obj_mat
self.navidget_stick.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size) #* \
#avango.gua.make_scale_mat(0.015,0.015,self.navidget_sphere_size * 2.0)
self.navidget_camera_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size * 2.0) * \
avango.gua.make_scale_mat(3.0)
self.navidget_trans_mat = _obj_mat
"""
else:
## start Navidget animation-mode on button release
if self.navigation_mode == 3:
self.set_navigation_mode(4)
def frame_callback(self): # executed every frame when active
## calc intersection
_mf_pick_result = self.intersection.calc_pick_result(PICK_MAT = self.ray_transform.WorldTransform.value, PICK_LENGTH = self.ray_length)
self.mf_pointer_pick_result.value = _mf_pick_result.value
self.update_ray_parameters()
if self.navigation_mode == 3: # Navidget target-mode
self.update_navidget_parameters()
elif self.navigation_mode == 4: # Navidget animation-mode
self.sf_nav_mat.value = self.animate_navidget()
### functions ###
def set_navigation_mode(self, MODE):
self.navigation_mode = MODE
if self.navigation_mode == 0: # switch to Steering mode
# set other modes geometry invisible
self.maneuvering_point_geometry.Tags.value = ["invisible"]
self.navidget_node.Tags.value = ["invisible"]
print("SWITCH TO STEERING MODE")
elif self.navigation_mode == 1: # switch to maneuvering mode
self.maneuvering_point_geometry.Tags.value = []
self.navidget_node.Tags.value = ["invisible"]
print("SWITCH TO MANEUVERING MODE")
elif self.navigation_mode == 3: # switch to Navidget target mode
self.navidget_sphere_geometry.Tags.value = []
self.navidget_sphere_geometry.Tags.value = ["navidget_sphere"]
self.navidget_node.Tags.value = []
if len(self.mf_pointer_pick_result.value) > 0: # intersection found
_pick_result = self.mf_pointer_pick_result.value[0] # get first intersection target
## set initial Navidget GUI position and orientation
# if there are objects hit by the ray
else:
_pick_result = None
if _pick_result is not None:
_obj_pos = _pick_result.WorldPosition.value # world position of selected object
_obj_mat = avango.gua.make_trans_mat(_obj_pos)
self.navidget_node.Transform.value = _obj_mat
self.navidget_stick.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size) #* \
#avango.gua.make_scale_mat(0.015,0.015,self.navidget_sphere_size * 2.0)
self.navidget_camera_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.navidget_sphere_size * 2.0) * \
avango.gua.make_scale_mat(3.0)
self.navidget_trans_mat = _obj_mat
print("SWITCH TO NAVIDGET TARGET-MODE")
elif self.navigation_mode == 4:
print("try animation")
## define start and target parameters for Navidget animation
self.navidget_start_pos = self.parent_node.WorldTransform.value.get_translate()
self.navidget_target_pos = self.navidget_camera_geometry.WorldTransform.value.get_translate()
self.navidget_start_quat = self.parent_node.WorldTransform.value.get_rotate()
self.navidget_target_quat = self.navidget_camera_geometry.WorldTransform.value.get_rotate()
self.navidget_start_time = time.time()
print("SWITCH TO NAVIDGET ANIMATION-MODE")
def update_ray_parameters(self):
if len(self.mf_pointer_pick_result.value) > 0: # intersection found
_pick_result = self.mf_pointer_pick_result.value[0] # get first intersection target
_point = _pick_result.WorldPosition.value # intersection point in world coordinate system
_distance = (_point - self.ray_transform.WorldTransform.value.get_translate()).length()
# update intersection point visualization
self.intersection_point_geometry.Tags.value = [] # set geometry visible
self.intersection_point_geometry.Transform.value = avango.gua.make_trans_mat(_point) * \
avango.gua.make_scale_mat(self.intersection_point_size)
# update ray visualization (ray length)
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,_distance * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,_distance)
else: # no intersection found
# update intersection point visualization
self.intersection_point_geometry.Tags.value = ["invisible"] # set geometry invisible
# update ray visualization (ray length)
self.ray_geometry.Transform.value = avango.gua.make_trans_mat(0.0,0.0,self.ray_length * -0.5) * \
avango.gua.make_scale_mat(self.ray_thickness,self.ray_thickness,self.ray_length)
def update_navidget_parameters(self):
if self.navigation_mode == 3: # maneuvering mode
_mf_pick_result = self.navidget_intersection.calc_pick_result(PICK_MAT = self.ray_transform.WorldTransform.value, PICK_LENGTH = self.ray_length)
# if there are objects hit by the ray
if len(_mf_pick_result.value) > 0:
_sphere_pick = _mf_pick_result.value[0]
else:
_sphere_pick = None
if _sphere_pick is not None:
_sphere_pick_pos = _sphere_pick.WorldPosition.value # world position of selected object
_sphere_pick_mat = avango.gua.make_trans_mat(_sphere_pick_pos)
self.proxy_geo.Transform.value =_sphere_pick_mat* avango.gua.make_scale_mat(0.08)
######
_origin_axis = avango.gua.Vec3(0.0,0.0,-1.0) #self.navidget_trans_mat.get_translate() + avango.gua.Vec3(0.0,0.0,1.0) # original rot
#_our_pos = self.parent_node.WorldTransform.value.get_translate()
_center = self.navidget_trans_mat.get_translate()
#vec1 = _hit_point-_origin_axis
#vec2 = _hit_point-_our_pos
#_origin_rot_mat = self.get_rotation_matrix_between_vectors(vec1, vec2)
#_hit_point = self.navidget_node.WorldTransform.value.get_translate()
#vec1 = _hit_point-_our_pos
vec1 = _center - _sphere_pick_pos
vec2 = _origin_axis
self.navidget_rot_mat = self.get_rotation_matrix_between_vectors(vec2, vec1)
self.navidget_node.Transform.value = self.navidget_trans_mat * self.navidget_rot_mat
#print("prox pos",self.rot_helper.WorldTransform.value.get_translate())
def animate_navidget(self):
print("animphase 1")
_current_time = time.time()
_slerp_ratio = (_current_time - self.navidget_start_time) / self.navidget_duration
# last animatuion step - finish animation afterwards
if _slerp_ratio >= 1:
self.set_navigation_mode(0)
print("slerp done")
_new_io_mat = avango.gua.make_trans_mat(navidget_target_pos) *\
avango.gua.make_rot_mat(navidget_target_quat)
return _new_io_mat
# move sun
else:
print("animating", _slerp_ratio)
_factor = _slerp_ratio
_new_pos = (self.navidget_start_pos * (1-_factor)) + ( self.navidget_target_pos * _factor)
_new_quat = self.slerp(self.navidget_start_quat, self.navidget_target_quat, _slerp_ratio)
print("animating4", _new_pos)
_new_io_mat = avango.gua.make_trans_mat(_new_pos) *\
avango.gua.make_rot_mat(_new_quat)
print ( "gogogo")
return _new_io_mat
def slerp(self, qa , qb, SLERP_RATIO):
_quat = qa.slerp_to(qb, SLERP_RATIO)
return _quat
def get_rotation_matrix_between_vectors(self, VEC1, VEC2):
VEC1.normalize()
VEC2.normalize()
_angle = math.degrees(math.acos(VEC1.dot(VEC2)))
_axis = VEC1.cross(VEC2)
return avango.gua.make_rot_mat(_angle, _axis)
'p_east_to_north':
from_0_to_1,
'p_east_to_west':
from_0_to_1,
'p_east_to_east':
from_0_to_1,
'p_east_to_south':
from_0_to_1,
'p_car_spawn_south':
p_spawn,
'p_south_to_north':
from_0_to_1,
'p_south_to_west':
from_0_to_1,
'p_south_to_east':
from_0_to_1,
'p_south_to_south':
from_0_to_1
}
parameter_space = [
dict(zip(parameters, v)) for v in product(*parameters.values())
]
print('Generating {} intersections'.format(len(parameter_space)))
for parameter_set in parameter_space:
intersection = Intersection(parameters=parameter_set,
parameters_as_dict=True)
datawriter = DataWriter(intersection)
datawriter.run(10)
