class MultiDofInput(avango.script.Script):
### fields ###
## output 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
mf_buttons = avango.MFBool()
mf_buttons.value = [False, False, False] # init 3 buttons
### functions ###
def filter_channel(self, VALUE, OFFSET, MIN, MAX, NEG_THRESHOLD, POS_THRESHOLD):
VALUE = VALUE - OFFSET
MIN = MIN - OFFSET
MAX = MAX - OFFSET
if VALUE > 0:
_pos = MAX * POS_THRESHOLD * 0.01
if VALUE > _pos: # above positive threshold
VALUE = min( (VALUE - _pos) / (MAX - _pos), 1.0) # normalize interval
else: # below positive threshold
VALUE = 0
elif VALUE < 0:
_neg = MIN * NEG_THRESHOLD * 0.01
if VALUE < _neg:
VALUE = max( (VALUE - _neg) / abs(MIN - _neg), -1.0)
else: # above negative threshold
VALUE = 0
return VALUE
class MultiDofInput(avango.script.Script):
### fields ###
## output 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
mf_buttons = avango.MFBool()
mf_buttons.value = [False, False, False] # init 3 buttons
## Map an input value to a certain interval.
# @param VALUE The value to be mapped.
# @param OFFSET The offset to be applied to VALUE, MIN and MAX.
# @param MIN The minimum value of the old interval.
# @param MAX The maximum value of the old interval.
# @param NEG_THRESHOLD The negative threshold to be used.
# @param POS_THRESHOLD The positive threshold to be used.
def filter_channel(self, VALUE, OFFSET, MIN, MAX, NEG_THRESHOLD,
POS_THRESHOLD):
VALUE -= OFFSET
MIN -= OFFSET
MAX -= OFFSET
#print("+", VALUE, MAX, POS_THRESHOLD)
#print("-", VALUE, MIN, NEG_THRESHOLD)
if VALUE > 0:
_pos = MAX * POS_THRESHOLD * 0.01
if VALUE > _pos: # above positive threshold
VALUE = min((VALUE - _pos) / (MAX - _pos),
1.0) # normalize interval
else: # below positive threshold
VALUE = 0
elif VALUE < 0:
_neg = MIN * abs(NEG_THRESHOLD) * 0.01
if VALUE < _neg:
VALUE = max((VALUE - _neg) / abs(MIN - _neg), -1.0)
else: # above negative threshold
VALUE = 0
return VALUE
class NavigationScript(avango.script.Script):
### input fields ###
mf_dof = avango.MFFloat()
### output fields ###
sf_nav_mat = avango.gua.SFMatrix4()
sf_nav_mat.value = avango.gua.make_identity_mat()
### Default constructor.
def __init__(self):
self.super(NavigationScript).__init__()
self.CLASS = None
def my_constructor(self, CLASS):
# references
self.CLASS = CLASS
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 TUIOHand(avango.script.Script):
HandClass = avango.SFFloat()
Finger1SID = avango.SFFloat()
Finger2SID = avango.SFFloat()
Finger3SID = avango.SFFloat()
Finger4SID = avango.SFFloat()
Finger5SID = avango.SFFloat()
FingerSIDs = avango.MFFloat()
SessionID = avango.SFFloat()
HandID = avango.SFInt()
CLASS_UNKNOWN = 0
CLASS_LEFT = 1
CLASS_RIGHT = 2
def __init__(self):
self.super(TUIOHand).__init__()
self.FingerSIDs.value = [-1.0, -1.0, -1.0, -1.0, 1.0]
self.SessionID.value = -1.0
self.Finger1SID.value = -1.0
self.Finger2SID.value = -1.0
self.Finger3SID.value = -1.0
self.Finger4SID.value = -1.0
self.Finger5SID.value = -1.0
self.device_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.HandClass.connect_from(self.device_sensor.Value0)
self.Finger1SID.connect_from(self.device_sensor.Value1)
self.Finger2SID.connect_from(self.device_sensor.Value2)
self.Finger3SID.connect_from(self.device_sensor.Value3)
self.Finger4SID.connect_from(self.device_sensor.Value4)
self.Finger5SID.connect_from(self.device_sensor.Value5)
self.SessionID.connect_from(self.device_sensor.Value6)
@field_has_changed(HandID)
def set_station(self):
"""
Set station name.
"""
self.device_sensor.Station.value = "gua-finger{}#hand".format(
self.HandID.value)
@field_has_changed(Finger1SID)
def updateFinger1InField(self):
self.FingerSIDs.value[0] = self.Finger1SID.value
@field_has_changed(Finger2SID)
def updateFinger2InField(self):
self.FingerSIDs.value[1] = self.Finger2SID.value
@field_has_changed(Finger3SID)
def updateFinger3InField(self):
self.FingerSIDs.value[2] = self.Finger3SID.value
@field_has_changed(Finger4SID)
def updateFinger4InField(self):
self.FingerSIDs.value[3] = self.Finger4SID.value
@field_has_changed(Finger5SID)
def updateFinger5InField(self):
self.FingerSIDs.value[4] = self.Finger5SID.value
class Manipulation(avango.script.Script):
### 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_mat = avango.gua.SFMatrix4()
sf_mat.value = avango.gua.make_identity_mat()
### constructor
def __init__(self):
self.super(Manipulation).__init__()
# variables
self.type = ""
self.enable_flag = False
### callbacks
@field_has_changed(mf_dof)
def mf_dof_changed(self):
if self.enable_flag == True:
self.manipulate()
### functions
def enable_manipulation(self, FLAG):
self.enable_flag = FLAG
if self.enable_flag == True:
print(self.type + " enabled")
self.reset()
def manipulate(self):
pass
def reset(self):
pass
def clamp_matrix(self, MATRIX):
# clamp translation to certain range
_x_range = 0.3 # in meter
_y_range = 0.15 # in meter
_z_range = 0.15 # in meter
MATRIX.set_element(0, 3,
min(_x_range,
max(-_x_range,
MATRIX.get_element(0, 3)))) # clamp x-axis
MATRIX.set_element(1, 3,
min(_y_range,
max(-_y_range,
MATRIX.get_element(1, 3)))) # clamp y-axis
MATRIX.set_element(2, 3,
min(_z_range,
max(-_z_range,
MATRIX.get_element(2, 3)))) # clamp z-axis
return MATRIX
def set_matrix(self, MATRIX):
if MATRIX != self.sf_mat.value: # check for valid input --> hand was moves
MATRIX = self.clamp_matrix(MATRIX) # clamp to translation range
self.sf_mat.value = MATRIX # apply new hand matrix
class ManipulationManager(avango.script.Script):
### input fields
sf_key_1 = avango.SFFloat()
sf_key_2 = avango.SFFloat()
sf_key_3 = avango.SFFloat()
mf_dof = avango.MFFloat()
mf_buttons = avango.MFBool()
### internal fields
sf_hand_mat = avango.gua.SFMatrix4()
# constructor
def __init__(self):
self.super(ManipulationManager).__init__()
def my_constructor(self, PARENT_NODE, MOUSE_DEVICE, SPACEMOUSE_DEVICE):
# references
self.mouse_device = MOUSE_DEVICE
self.spacemouse_device = SPACEMOUSE_DEVICE
# variables
self.dragging_technique = None
self.dragged_objects = []
self.parent_node = PARENT_NODE
self.scene_root = PARENT_NODE
self.offset = avango.gua.make_identity_mat()
self.diff_mat = avango.gua.make_identity_mat()
self.last_frame = avango.gua.make_identity_mat()
while self.scene_root.Parent.value:
self.scene_root = self.scene_root.Parent.value
self.lf_hand_mat = avango.gua.make_identity_mat(
) # last frame hand matrix
### init hand geometry
_loader = avango.gua.nodes.TriMeshLoader(
) # init trimesh loader to load external meshes
self.hand_geometry = _loader.create_geometry_from_file(
"hand_geometry", "data/objects/hand.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.hand_geometry.Transform.value = avango.gua.make_rot_mat(45.0,1,0,0) * \
avango.gua.make_rot_mat(180.0,0,1,0) * \
avango.gua.make_scale_mat(0.06)
self.hand_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(1.0, 0.86, 0.54, 1.0))
self.hand_geometry.Material.value.set_uniform("Emissivity", 0.9)
self.hand_geometry.Material.value.set_uniform("Metalness", 0.1)
self.hand_transform = avango.gua.nodes.TransformNode(
Name="hand_transform")
self.hand_transform.Children.value = [self.hand_geometry]
self.hand_transform.Transform.value = avango.gua.make_identity_mat()
self.parent_node.Children.value.append(self.hand_transform)
self.sf_hand_mat.connect_from(self.hand_transform.WorldTransform)
# init manipulation technique
self.isotonic_position_control_manipulation = IsotonicPositionControlManipulation(
)
self.isotonic_position_control_manipulation.my_constructor(
self.hand_transform.Transform, self.mf_dof)
self.isotonic_position_control_manipulation.enable_manipulation(True)
self.mf_dof.connect_from(self.mouse_device.mf_dof)
self.mf_buttons.connect_from(self.mouse_device.mf_buttons)
# init dragging technique
self.set_dragging_technique(1)
# init keyboard sensor
self.keyboard_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.keyboard_sensor.Station.value = "device-keyboard"
self.sf_key_1.connect_from(self.keyboard_sensor.Button12) # key 1
self.sf_key_2.connect_from(self.keyboard_sensor.Button13) # key 2
self.sf_key_3.connect_from(self.keyboard_sensor.Button14) # key 3
### functions
def set_dragging_technique(self, INT):
self.dragging_technique = INT
print("Dragging Technique set to technique", self.dragging_technique)
def start_dragging(self):
_hand_mat = self.sf_hand_mat.value
# travers all scenegraph nodes
for _node in self.scene_root.Children.value:
_name = _node.Name.value
if _name.count("monkey") > 0: # a monkey node
if self.is_highlight_material(
_node.CurrentColor.value
): # monkey node in close proximity
_node.CurrentColor.value = avango.gua.Vec4(
1.0, 0.0, 0.0, 1.0)
_node.Material.value.set_uniform(
"Color", _node.CurrentColor.value
) # switch to dragging material
self.dragged_objects.append(_node) # add node for dragging
## dragging without snapping
## TODO: Implement dragging strategies here ##
if self.dragging_technique == 1: # change of node order in scenegraph
self.offset = avango.gua.make_inverse_mat(
self.hand_transform.WorldTransform.value
) * _node.Transform.value
self.scene_root.Children.value.remove(_node)
_node.Transform.value = self.offset
self.hand_transform.Children.value.append(_node)
elif self.dragging_technique == 2: # absolute tool-hand offset to tool space
self.offset = avango.gua.make_inverse_mat(
self.hand_transform.WorldTransform.value
) * _node.Transform.value
elif self.dragging_technique == 3: # relative tool input to object space
self.last_frame = self.hand_transform.WorldTransform.value
def object_dragging(self):
# apply hand movement to (all) dragged objects
for _node in self.dragged_objects:
## TODO: Implement dragging strategies here ##
if self.dragging_technique == 1: # change of node order in scenegraph
pass
elif self.dragging_technique == 2: # absolute tool-hand offset to tool space
_node.Transform.value = self.hand_transform.WorldTransform.value * self.offset
elif self.dragging_technique == 3: # relative tool input to object space
self.diff_mat = avango.gua.make_inverse_mat(
self.last_frame) * self.hand_transform.WorldTransform.value
self.last_frame = self.hand_transform.WorldTransform.value
_node.Transform.value = self.diff_mat * _node.Transform.value
def stop_dragging(self):
# travers all dragged objects
for _node in self.dragged_objects:
_node.CurrentColor.value = avango.gua.Vec4(0.0, 1.0, 0.0, 1.0)
_node.Material.value.set_uniform(
"Color",
_node.CurrentColor.value) # switch to highlight material
## TODO: Implement dragging strategies here ##
if self.dragging_technique == 1: # change of node order in scenegraph
_node.Transform.value = _node.WorldTransform.value
self.hand_transform.Children.value.remove(_node)
self.scene_root.Children.value.append(_node)
elif self.dragging_technique == 2: # absolute tool-hand offset to tool space
pass
elif self.dragging_technique == 3: # relative tool input to object space
pass
self.dragged_objects = [] # clear list
def is_default_material(self, VEC4):
return VEC4.x == 1.0 and VEC4.y == 1.0 and VEC4.z == 1.0 and VEC4.w == 1.0
def is_highlight_material(self, VEC4):
return VEC4.x == 0.0 and VEC4.y == 1.0 and VEC4.z == 0.0 and VEC4.w == 1.0
def is_dragging_material(self, VEC4):
return VEC4.x == 1.0 and VEC4.y == 0.0 and VEC4.z == 0.0 and VEC4.w == 1.0
### callbacks
@field_has_changed(sf_key_1)
def sf_key_1_changed(self):
if self.sf_key_1.value == True: # key is pressed
self.set_dragging_technique(1) # switch dragging technique
@field_has_changed(sf_key_2)
def sf_key_2_changed(self):
if self.sf_key_2.value == True: # key is pressed
self.set_dragging_technique(2) # switch dragging technique
@field_has_changed(sf_key_3)
def sf_key_3_changed(self):
if self.sf_key_3.value == True: # key is pressed
self.set_dragging_technique(3) # switch dragging technique
@field_has_changed(mf_buttons)
def mf_buttons_changed(self):
_left_button = self.mf_buttons.value[0]
_right_button = self.mf_buttons.value[1]
_button = _left_button ^ _right_button # button left XOR button right
if _button == True: # key is pressed
self.start_dragging()
else:
self.stop_dragging()
@field_has_changed(sf_hand_mat)
def sf_hand_mat_changed(self):
_hand_pos = self.sf_hand_mat.value.get_translate()
# travers all scenegraph nodes
for _node in self.scene_root.Children.value:
_name = _node.Name.value
if _name.count("monkey") > 0: # identify a monkey node
_pos = _node.Transform.value.get_translate(
) # a monkey position
_dist = (_hand_pos - _pos).length() # hand-object distance
_node_col = _node.CurrentColor.value
### toggle object highlight
if _dist < 0.02 and self.is_default_material(_node_col):
_node.CurrentColor.value = avango.gua.Vec4(
0.0, 1.0, 0.0, 1.0)
_node.Material.value.set_uniform(
"Color", _node.CurrentColor.value
) # switch to highlight material
elif _dist > 0.025 and self.is_highlight_material(_node_col):
_node.CurrentColor.value = avango.gua.Vec4(
1.0, 1.0, 1.0, 1.0)
_node.Material.value.set_uniform(
"Color",
_node.CurrentColor.value) # switch to default material
self.object_dragging() # evtl. drag object with hand input
# calc hand velocity
_distance = (_hand_pos - self.lf_hand_mat.get_translate()).length()
_velocity = _distance * 60.0 # application loop runs with 60Hz
_velocity = round(_velocity, 2) # round to 2nd decimal place
#print(_velocity, "m/s")
self.lf_hand_mat = self.sf_hand_mat.value
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, MF_BUTTONS, ATTENUATION_FACTOR=1.0):
self.mf_dof.connect_from(MF_DOF)
self.attenuation_factor = ATTENUATION_FACTOR
### 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()
if _trans_input or _rot_input > 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)
### 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 MultiDofDevice(avango.script.Script):
# output fields
## @var mf_dof
# The input values of the input device.
mf_dof = avango.MFFloat()
mf_dof.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # init 8 channels
## @var mf_buttons
# The button values of the input device.
mf_buttons = avango.MFBool()
mf_buttons.value = [False, False, False, False, False, False, False] # init 7 buttons
## @var sf_station_mat
# Tracking position and rotation.
sf_station_mat = avango.gua.SFMatrix4()
sf_station_mat.value = avango.gua.make_identity_mat()
# factors for amplifying
## @var translation_factor
# Factor to modify the device's translation input.
translation_factor = 1.0
## @var rotation_factor
# Factor to modify the device's rotation input.
rotation_factor = 1.0
## Default constructor.
def __init__(self):
self.super(MultiDofDevice).__init__()
## Initializes a tracking reader for the device's position and rotation
# @param TRACKING_TARGET_NAME The tracking name as chosen in daemon, None if no tracking is available.
# @param NO_TRACKING_MAT Tracking matrix to be used if no tracking is available.
def init_station_tracking(self, TRACKING_TARGET_NAME, NO_TRACKING_MAT):
## @var tracking_reader
# Tracking Reader to process the tracking input of the device.
if TRACKING_TARGET_NAME != None:
self.tracking_reader = TrackingTargetReader()
self.tracking_reader.my_constructor(TRACKING_TARGET_NAME)
self.sf_station_mat.connect_from(self.tracking_reader.sf_abs_mat)
else:
self.tracking_reader = TrackingDefaultReader()
self.tracking_reader.set_no_tracking_matrix(NO_TRACKING_MAT)
self.sf_station_mat.connect_from(self.tracking_reader.sf_abs_mat)
## Map an input value to a certain interval.
# @param VALUE The value to be mapped.
# @param OFFSET The offset to be applied to VALUE, MIN and MAX
# @param MIN The minimum value of the new interval.
# @param MAX The maximum value of the old interval.
# @param NEG_THRESHOLD The negative threshold to be used.
# @param POS_THRESHOLD The positive threshold to be used.
def filter_channel(self, VALUE, OFFSET, MIN, MAX, NEG_THRESHOLD, POS_THRESHOLD):
VALUE = VALUE - OFFSET
MIN = MIN - OFFSET
MAX = MAX - OFFSET
#print "+", VALUE, MAX, POS_THRESHOLD
#print "-", VALUE, MIN, NEG_THRESHOLD
if VALUE > 0:
_pos = MAX * POS_THRESHOLD * 0.01
if VALUE > _pos: # above positive threshold
VALUE = min( (VALUE - _pos) / (MAX - _pos), 1.0) # normalize interval
else: # beneath positive threshold
VALUE = 0
elif VALUE < 0:
_neg = MIN * NEG_THRESHOLD * 0.01
if VALUE < _neg:
VALUE = max( (VALUE - _neg) / abs(MIN - _neg), -1.0)
else: # above negative threshold
VALUE = 0
return VALUE
class SteeringNavigation(NavigationTechnique):
### fields ###
## input fields
mf_dof = avango.MFFloat()
mf_dof.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # init 6 channels
### constructor
def __init__(self):
NavigationTechnique.__init__(self) # call base class constructor
def my_constructor(self, NAVIGATION_MANAGER):
### external references ###
self.NAVIGATION_MANAGER = NAVIGATION_MANAGER
### parameters ###
self.translation_factor = 0.2
self.rotation_factor = 1.0
## init field connections
self.mf_dof.connect_from(
self.NAVIGATION_MANAGER.INPUTS.mf_dof_steering)
### callback functions ###
def evaluate(self): # implement respective base-class function
if self.enable_flag == False:
return
## 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)
_trans_input = _trans_vec.length()
#print(_trans_input)
if _trans_input > 0.0: # guard
# transfer function for translation
_factor = pow(min(_trans_input, 1.0), 2)
_trans_vec.normalize()
_trans_vec *= _factor * self.translation_factor
## 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)
_rot_input = _rot_vec.length()
if _rot_input > 0.0: # guard
# transfer function for rotation
_factor = pow(_rot_input, 2) * self.rotation_factor
_rot_vec.normalize()
_rot_vec *= _factor
_input_mat = avango.gua.make_trans_mat(_trans_vec) * \
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)
## transform input into head orientation (required for HMD setup)
_head_rot_mat = avango.gua.make_rot_mat(
self.NAVIGATION_MANAGER.get_head_matrix().get_rotate())
_input_mat = self.NAVIGATION_MANAGER.transform_mat_in_ref_mat(
_input_mat, _head_rot_mat)
## accumulate input to navigation coordinate system
_new_mat = self.NAVIGATION_MANAGER.get_navigation_matrix() * \
_input_mat
self.NAVIGATION_MANAGER.set_navigation_matrix(_new_mat)
class Inputs(avango.script.Script):
## input fields
sf_pointer_button = avango.SFBool()
sf_pointer_tracking_mat = avango.gua.SFMatrix4()
mf_dof_steering = avango.MFFloat()
sf_technique_button0 = avango.SFBool()
sf_technique_button1 = avango.SFBool()
sf_technique_button2 = avango.SFBool()
sf_technique_button3 = avango.SFBool()
sf_toggle_technique_button = avango.SFBool()
sf_reset_button = avango.SFBool()
## Default constructor.
def __init__(self):
self.super(Inputs).__init__()
def init_projection_setup(self, POINTER_TRACKING_STATION,
POINTER_DEVICE_STATION, KEYBOARD_STATION):
## init sensors
self.pointer_tracking_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.pointer_tracking_sensor.Station.value = POINTER_TRACKING_STATION
self.pointer_tracking_sensor.ReceiverOffset.value = avango.gua.make_trans_mat(
0.0, 0.0, -0.05)
self.pointer_device_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.pointer_device_sensor.Station.value = POINTER_DEVICE_STATION
self.keyboard_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.keyboard_sensor.Station.value = KEYBOARD_STATION
self.steeringInput = SpacemouseSteeringInput()
self.steeringInput.my_constructor("gua-device-spacemouse")
### init field connections
self.sf_pointer_button.connect_from(self.pointer_device_sensor.Button0)
self.sf_pointer_tracking_mat.connect_from(
self.pointer_tracking_sensor.Matrix)
self.mf_dof_steering.connect_from(self.steeringInput.mf_dof)
self.sf_technique_button0.connect_from(self.keyboard_sensor.Button16)
self.sf_technique_button1.connect_from(self.keyboard_sensor.Button17)
self.sf_technique_button2.connect_from(self.keyboard_sensor.Button18)
self.sf_technique_button3.connect_from(self.keyboard_sensor.Button19)
self.sf_reset_button.connect_from(self.keyboard_sensor.Button14)
def init_hmd_setup(self, TRACKING_NAME_BASE, VIEWING_SETUP):
## init sensors
self.hmd_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.hmd_sensor.Station.value = TRACKING_NAME_BASE + "-0"
self.controller1_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.controller1_sensor.Station.value = TRACKING_NAME_BASE + "-1"
self.steeringInput = ViveSteeringInput()
self.steeringInput.my_constructor(TRACKING_NAME_BASE + "-1")
### init field connections
self.sf_pointer_button.connect_from(self.controller1_sensor.Button4)
self.sf_pointer_tracking_mat.connect_from(
self.controller1_sensor.Matrix)
self.mf_dof_steering.connect_from(self.steeringInput.mf_dof)
self.sf_toggle_technique_button.connect_from(
self.controller1_sensor.Button2)
self.sf_reset_button.connect_from(self.controller1_sensor.Button1)
## init Vive-Controller visulization
_loader = avango.gua.nodes.TriMeshLoader()
self.controller_geometry = _loader.create_geometry_from_file(
"controller_geometry",
"data/objects/vive_controller/vive_controller.obj",
avango.gua.LoaderFlags.LOAD_MATERIALS)
self.controller_geometry.Material.value.set_uniform("Roughness", 0.8)
self.controller_geometry.Material.value.set_uniform(
"ColorMap",
"data/objects/vive_controller/onepointfive_texture.png")
self.controller_geometry.Transform.connect_from(
self.controller1_sensor.Matrix)
VIEWING_SETUP.navigation_node.Children.value.append(
self.controller_geometry)
class SteeringNavigation(avango.script.Script):
### 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__()
# variables
self.rot_center_offset = avango.gua.Vec3(0.0,0.0,0.0)
def my_constructor(self, MF_DOF, MF_BUTTONS):
self.mf_dof.connect_from(MF_DOF)
### callbacks
@field_has_changed(mf_dof)
def mf_dof_changed(self):
_input_flag = False
_mat = self.sf_nav_mat.value
# 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)
_trans_input = _trans_vec.length()
if _trans_input > 0.0: # map translation input
_input_flag = True
#_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
_mat = _mat * avango.gua.make_trans_mat(_trans_vec)
# 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)
_rot_input = _rot_vec.length()
if _rot_input > 0.0: # map rotation input
_input_flag = True
_mat = _mat * \
avango.gua.make_trans_mat(self.rot_center_offset) * \
avango.gua.make_rot_mat(_ry,0,1,0) * \
avango.gua.make_rot_mat(_rx,1,0,0) * \
avango.gua.make_rot_mat(_rz,0,0,1) * \
avango.gua.make_trans_mat(self.rot_center_offset * -1)
# apply input (once)
if _input_flag == True:
self.sf_nav_mat.value = _mat
### 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 NavidgetNavigation(NavigationTechnique):
### fields ###
## input fields
sf_pointer_button = avango.SFBool()
pressed = False
mf_dof = avango.MFFloat()
mf_dof.value = [0.0,0.0,0.0,0.0,0.0,0.0]
### constructor
def __init__(self):
NavigationTechnique.__init__(self) # call base class constructor
def my_constructor(self, NAVIGATION_MANAGER, SCENEGRAPH):
### external references ###
self.NAVIGATION_MANAGER = NAVIGATION_MANAGER
self.SCENEGRAPH = SCENEGRAPH
### parameters ###
self.navidget_duration = 8.0 # in seconds
self.camera_distance = 1.8 #in meters
self.sphere_diameter = 1.5 # in meters
self.camera_size = 5.0
self.sphere_intersect_diameter = 0.03
self.global_reference_point = avango.gua.Vec3(0.0,0.0,0.0)
self.initial = round(time.time())
self.time_flag = False
self.sf_pointer_button.connect_from(self.NAVIGATION_MANAGER.INPUTS.sf_pointer_button)
self.init_ref_point = None
self.always_evaluate(True) # change global evaluation policy
_loader = avango.gua.nodes.TriMeshLoader()
self.sphere_node = avango.gua.nodes.TransformNode(Name = "sphere_node")
self.SCENEGRAPH.Root.value.Children.value.append(self.sphere_node)
self.sphere_geometry = _loader.create_geometry_from_file("sphere_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS)
self.sphere_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(0.5,0.5,0.7,0.2))
self.sphere_geometry.Tags.value = ["invisible"]
self.sphere_node.Children.value.append(self.sphere_geometry)
self.camera_geometry = _loader.create_geometry_from_file("camera_geometry", "data/objects/camera.obj", avango.gua.LoaderFlags.DEFAULTS)
self.camera_geometry.Tags.value = ["invisible"]
self.sphere_node.Children.value.append(self.camera_geometry)
self.sphere_intersect_geometry = _loader.create_geometry_from_file("sphere_intersect_geometry", "data/objects/sphere.obj", avango.gua.LoaderFlags.DEFAULTS)
self.sphere_intersect_geometry.Material.value.set_uniform("Color", avango.gua.Vec4(1.0,0.0,0.0,1.0))
self.sphere_intersect_geometry.Tags.value = ["invisible"]
self.sphere_node.Children.value.append(self.sphere_intersect_geometry)
self.camera_geometry.Transform.value = avango.gua.make_trans_mat(self.camera_distance, 0.0, 0.0) * avango.gua.make_rot_mat(90,0,1,0) * avango.gua.make_scale_mat(self.camera_size)
self.sphere_intersect_geometry.Transform.value = avango.gua.make_trans_mat(self.sphere_diameter/2.0 ,0.0,0.0) * avango.gua.make_scale_mat(self.sphere_intersect_diameter)
### functions ###
def get_rotation_matrix_between_vectors(self, VEC1, VEC2): # helper function to calculate rotation matrix to rotate one vector (VEC3) on another one (VEC2)
VEC1.normalize()
VEC2.normalize()
_z_axis = VEC2
_y_axis = VEC1
_x_axis = _y_axis.cross(_z_axis)
_y_axis = _z_axis.cross(_x_axis)
_x_axis.normalize()
_y_axis.normalize()
# create new rotation matrix
_mat = avango.gua.make_identity_mat()
_mat.set_element(0,0, _x_axis.x)
_mat.set_element(1,0, _x_axis.y)
_mat.set_element(2,0, _x_axis.z)
_mat.set_element(0,1, _y_axis.x)
_mat.set_element(1,1, _y_axis.y)
_mat.set_element(2,1, _y_axis.z)
_mat.set_element(0,2, _z_axis.x)
_mat.set_element(1,2, _z_axis.y)
_mat.set_element(2,2, _z_axis.z)
_mat = _mat * avango.gua.make_rot_mat(180.0,0,1,0)
return _mat
#def stopwatch(seconds):
# start = time.time()
# time.clock()
# elapsed = 0
# while elapsed < seconds:
# elapsed = time.time() - start
# print "loop cycle time: %f, seconds count: %02d" % (time.clock() , elapsed)
# time.sleep(1)
# return elapsed
### callback functions ###
def evaluate(self): # implement respective base-class function
if self.enable_flag == False:
self.sphere_geometry.Tags.value = ["invisible"]
self.camera_geometry.Tags.value = ["invisible"]
self.sphere_intersect_geometry.Tags.value = ["invisible"]
return
## To-Do: realize Navidget navigation here
self.NAVIGATION_MANAGER.calc_pick_result()
self.NAVIGATION_MANAGER.update_ray_visualization()
self.NAVIGATION_MANAGER.ray_geometry.Tags.value = []
sphere_rotation = self.NAVIGATION_MANAGER.pointer_node.WorldTransform.value.get_rotate()
ct = round(time.time())
if self.sf_pointer_button.value == True and self.pressed == False: #and not self.pressed:
self.sphere_geometry.Tags.value = []
self.camera_geometry.Tags.value = []
self.sphere_intersect_geometry.Tags.value = []
reference_point = self.NAVIGATION_MANAGER.intersection_geometry.WorldTransform.value.get_translate()
self.init_ref_point = reference_point
self.sphere_node.Transform.value = avango.gua.make_trans_mat(reference_point) *avango.gua.make_scale_mat(self.sphere_diameter)
#self.camera_geometry.Transform.value = avango.gua.make_trans_mat(self.camera_distance, 0.0, 0.0) * avango.gua.make_scale_mat(self.camera_size)
#self.sphere_intersect_geometry.Transform.value = avango.gua.make_trans_mat(self.sphere_diameter/2.0 ,0.0,0.0) * avango.gua.make_scale_mat(self.sphere_intersect_diameter)
self.pressed = True
#print ('translation when pressed once')
#print(reference_point)
# now we save the translated values here
elif not self.sf_pointer_button.value and self.pressed:
print('rotation when pressed once')
print(sphere_rotation)
self.sphere_node.Transform.value = avango.gua.make_trans_mat(self.init_ref_point) * avango.gua.make_rot_mat(sphere_rotation) * avango.gua.make_scale_mat(self.sphere_diameter)
#self.camera_geometry.Transform.value = avango.gua.make_trans_mat(self.camera_distance, 0.0, 0.0) * avango.gua.make_scale_mat(self.camera_size)
#self.sphere_intersect_geometry.Transform.value = avango.gua.make_trans_mat(self.sphere_diameter/2.0 ,0.0,0.0) * avango.gua.make_scale_mat(self.sphere_intersect_diameter)
self.time_flag = True
#print('time')
#print (ct - self.initial)
if self.time_flag and (ct - self.initial > self.navidget_duration):
#print('i am here')
self.sphere_geometry.Tags.value = ["invisible"]
self.camera_geometry.Tags.value = ["invisible"]
self.sphere_intersect_geometry.Tags.value = ["invisible"]
self.pressed = False
self.time_flag = False
self.initial = round(time.time())
_trans_vec = self.sphere_node.WorldTransform.value.get_translate()
_rot_vec = self.NAVIGATION_MANAGER.pointer_node.WorldTransform.value.get_rotate()
_input_mat = avango.gua.make_trans_mat(_trans_vec) * avango.gua.make_rot_mat(_rot_vec)
#_head_rot_mat = avango.gua.make_rot_mat(self.NAVIGATION_MANAGER.get_head_matrix().get_rotate())
#_input_mat = self.NAVIGATION_MANAGER.transform_mat_in_ref_mat(_input_mat, _head_rot_mat)
#self.NAVIGATION_MANAGER.set_navigation_matrix(_input_mat)
self.NAVIGATION_MANAGER.set_navigation_matrix(_input_mat)
class InputMapping(avango.script.Script):
## @var mf_rel_input_values
# The relative input values of the device.
mf_rel_input_values = avango.MFFloat()
## @var sf_station_mat
# The absolute matrix indicating where the device is placed in space.
sf_station_mat = avango.gua.SFMatrix4()
sf_station_mat.value = avango.gua.make_identity_mat()
# internal fields
## @var sf_abs_uncorrected_mat
# The absolute matrix to accumulate the relative inputs on. Will be corrected by GroundFollowing instance.
sf_abs_uncorrected_mat = avango.gua.SFMatrix4()
sf_abs_uncorrected_mat.value = avango.gua.make_identity_mat()
## @var sf_scale
# The current scaling factor of this input mapping.
sf_scale = avango.SFFloat()
sf_scale.value = 1.0
# output field
## @var sf_abs_mat
# The absolute matrix after GroundFollowing correction.
sf_abs_mat = avango.gua.SFMatrix4()
sf_abs_mat.value = avango.gua.make_identity_mat()
## Default constructor.
def __init__(self):
self.super(InputMapping).__init__()
# attributes
## @var realistic
# Boolean value to indicate if the user is navigating in 3-DOF (realistic) or 6-DOF (unrealistic) mode.
self.realistic = True
## @var blocked
# Boolean variable indicating if the device input is blocked (e.g. when in coupling animation)
self.blocked = False
## @var lf_quat_angle
# Quaternion angle of last frame to prevent blackscreen. Used if new angle is nan.
self.lf_quat_angle = 0.0
# factors for input amplifying
## @var input_trans_factor
# Factor to modify the translation input.
self.input_trans_factor = 1.0
## @var input_rot_factor
# Factor to modify the rotation input.
self.input_rot_factor = 1.0
## @var min_scale
# The minimum scaling factor that can be applied.
self.min_scale = 0.001
## @var max_scale
# The maximum scaling factor that can be applied.
self.max_scale = 1000.0
## @var scale_stop_time
# Time at which a scaling process stopped at a fixed step.
self.scale_stop_time = None
## @var scale_stop_duration
# Time how long a scaling process is stopped at a fixed step in seconds.
self.scale_stop_duration = 1.0
## Custom constructor.
# @param NAVIGATION The navigation instance from which this input mapping is created.
# @param DEVICE_INSTANCE Instance of Device class to take the input values from.
# @param GROUND_FOLLOWING_INSTANCE Instance of GroundFollowing to be used for matrix correction.
# @param STARTING_MATRIX Initial matrix to accumulate the relative inputs on.
# @param INVERT Boolean indicating if the input values should be inverted.
def my_constructor(self, NAVIGATION, DEVICE_INSTANCE,
GROUND_FOLLOWING_INSTANCE, STARTING_MATRIX, INVERT):
## @var NAVIGATION
# Reference to the Navigation instance from which this InputMapping is created.
self.NAVIGATION = NAVIGATION
## @var GROUND_FOLLOWING_INSTANCE
# Reference to the GroundFollowing instance used by this InputMapping.
self.GROUND_FOLLOWING_INSTANCE = GROUND_FOLLOWING_INSTANCE
## @var DEVICE_INSTANCE
# Reference to Device instance used by this InputMapping.
self.DEVICE_INSTANCE = DEVICE_INSTANCE
## @var invert
# Boolean indicating if the input values should be inverted.
self.invert = INVERT
# connect device fields
self.mf_rel_input_values.connect_from(DEVICE_INSTANCE.mf_dof)
self.sf_station_mat.connect_from(DEVICE_INSTANCE.sf_station_mat)
# connect ground following fields
GROUND_FOLLOWING_INSTANCE.sf_abs_input_mat.connect_from(
self.sf_abs_uncorrected_mat)
GROUND_FOLLOWING_INSTANCE.sf_scale.connect_from(self.sf_scale)
self.sf_abs_mat.connect_from(
GROUND_FOLLOWING_INSTANCE.sf_abs_output_mat)
# create feedback loop
self.sf_abs_uncorrected_mat.connect_weak_from(self.sf_abs_mat)
# set the starting position
self.set_abs_mat(STARTING_MATRIX)
## Evaluated when device input values change.
@field_has_changed(mf_rel_input_values)
def mf_rel_input_values_changed(self):
if self.blocked == False:
# map scale
_scale = self.mf_rel_input_values.value[6]
if _scale != 0.0:
self.set_scale(self.sf_scale.value * (1.0 + _scale * 0.015))
_x = self.mf_rel_input_values.value[0]
_y = self.mf_rel_input_values.value[1]
_z = self.mf_rel_input_values.value[2]
_rx = self.mf_rel_input_values.value[3]
_ry = self.mf_rel_input_values.value[4]
_rz = self.mf_rel_input_values.value[5]
# invert movement if activated
if self.invert:
_x = -_x
_y = -_y
_z = -_z
_rx = -_rx
_ry = -_ry
_rz = -_rz
# delete certain values that create an unrealistic movement
if self.realistic:
_y = 0.0
_rx = 0.0
_rz = 0.0
# get translation values from input device
_trans_vec = avango.gua.Vec3(_x, _y, _z)
_trans_input = _trans_vec.length()
# get rotation values from input device
_rot_vec = avango.gua.Vec3(_rx, _ry, _rz) * self.input_rot_factor
_rot_input = _rot_vec.length()
# only accumulate inputs on absolute matrix when the device values change
if _trans_input != 0.0 or _rot_input != 0.0:
# transfer function for translation
if _trans_input != 0.0:
_trans_vec.normalize()
_trans_vec *= math.pow(
min(_trans_input, 1.0),
3) * self.input_trans_factor * self.sf_scale.value
# global platform rotation in the world
_platform_quat = self.sf_abs_mat.value.get_rotate()
# Fix if quaternion angle is nan
_quat_angle = _platform_quat.get_angle()
if math.isnan(_quat_angle) == False:
_platform_rot_mat = avango.gua.make_rot_mat(
_quat_angle, _platform_quat.get_axis())
self.lf_quat_angle = _quat_angle
else:
_platform_rot_mat = avango.gua.make_rot_mat(
self.lf_quat_angle, _platform_quat.get_axis())
# global rotation of the device in the world
_device_forward_yaw = Utilities.get_yaw(
self.sf_station_mat.value)
_device_rot_mat = avango.gua.make_rot_mat(
math.degrees(_device_forward_yaw), 0, 1, 0)
# combined platform and device rotation
_combined_rot_mat = _platform_rot_mat * _device_rot_mat
# rotation center of the device
_rot_center = self.sf_station_mat.value.get_translate(
) * self.sf_scale.value
# transformed translation, rotation and rotation center
_transformed_trans_vec = self.transform_vector_with_matrix(
_trans_vec, _combined_rot_mat)
_transformed_rot_vec = self.transform_vector_with_matrix(
_rot_vec, _combined_rot_mat)
_transformed_rot_center = self.transform_vector_with_matrix(
_rot_center, _platform_rot_mat)
# create new transformation matrix
_new_mat = avango.gua.make_trans_mat(_transformed_trans_vec) * \
self.sf_abs_mat.value * \
avango.gua.make_trans_mat(_rot_center) * \
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(_rot_center * -1)
# update matrix on coupled navigations
_global_rot_center = self.sf_abs_mat.value * _rot_center
_global_rot_center = avango.gua.Vec3(_global_rot_center.x,
_global_rot_center.y,
_global_rot_center.z)
#for _navigation in self.NAVIGATION.coupled_navigations:
# _navigation.inputmapping.modify_abs_uncorrected_mat(_transformed_trans_vec, _transformed_rot_vec, _global_rot_center)
else:
# the device values are all equal to zero
_new_mat = self.sf_abs_mat.value
# save the computed new matrix
self.sf_abs_uncorrected_mat.value = _new_mat
## Modify the uncorrected matrix of this input mapping with specific values. Used for coupling purposes.
# @param TRANSFORMED_TRANS_VECTOR The translation vector to be applied.
# @param TRANSFORMED_ROT_VECTOR The vector containing the rotation values to be applied.
# @param ROTATION_CENTER The center to rotate around.
def modify_abs_uncorrected_mat(self, TRANSFORMED_TRANS_VECTOR,
TRANSFORMED_ROT_VECTOR, ROTATION_CENTER):
# compute new translation
_new_pos = TRANSFORMED_TRANS_VECTOR + self.sf_abs_mat.value.get_translate(
)
# compute offset to rotation center
_rot_center_offset = ROTATION_CENTER - _new_pos
# create new transformation matrix
_quat = self.sf_abs_mat.value.get_rotate()
_new_mat = avango.gua.make_trans_mat(_new_pos) * \
avango.gua.make_trans_mat(_rot_center_offset) * \
avango.gua.make_rot_mat( TRANSFORMED_ROT_VECTOR.y, 0, 1, 0) * \
avango.gua.make_rot_mat( TRANSFORMED_ROT_VECTOR.x, 1, 0, 0) * \
avango.gua.make_rot_mat( TRANSFORMED_ROT_VECTOR.z, 0, 0, 1) * \
avango.gua.make_trans_mat(_rot_center_offset * -1) * \
avango.gua.make_rot_mat(_quat.get_angle(), _quat.get_axis())
# save the computed new matrix
self.sf_abs_mat.value = _new_mat
## Transforms a vector using a transformation matrix.
# @param VECTOR The vector to be transformed.
# @param MATRIX The matrix to be applied for transformation.
def transform_vector_with_matrix(self, VECTOR, MATRIX):
_trans_vec = MATRIX * VECTOR
return avango.gua.Vec3(_trans_vec.x, _trans_vec.y, _trans_vec.z)
## Set a value for sf_abs_mat.
# @param MATRIX The matrix to be set to.
def set_abs_mat(self, MATRIX):
self.sf_abs_mat.value = MATRIX
## Sets the translation and rotation input factors.
# @param TRANSLATION_FACTOR Translation modification factor to be set. 1.0 by default.
# @param ROTATION_FACTOR Rotation modification factor to be set. 1.0 by default.
def set_input_factors(self, TRANSLATION_FACTOR=1.0, ROTATION_FACTOR=1.0):
self.input_trans_factor = TRANSLATION_FACTOR
self.input_rot_factor = ROTATION_FACTOR
## Activates the realistic mode (only 3 DOF navigation, GroundFollowing enabled)
def activate_realistic_mode(self):
self.realistic = True
self.GROUND_FOLLOWING_INSTANCE.activate()
## Activates the unrealistic mode (6 DOF navigation, GroundFollowing disabled)
def deactivate_realistic_mode(self):
self.realistic = False
self.GROUND_FOLLOWING_INSTANCE.deactivate()
## Applies a new scaling to this input mapping.
# @param SCALE The new scaling factor to be applied.
# @param CONSIDER_SNAPPING Boolean saying if the scaling should snap at powers of ten.
def set_scale(self, SCALE, CONSIDER_SNAPPING=True):
if CONSIDER_SNAPPING == False:
self.sf_scale.value = SCALE
return
if self.scale_stop_time == None:
_old_scale = self.sf_scale.value
_old_scale = round(_old_scale, 6)
_new_scale = max(min(SCALE, self.max_scale), self.min_scale)
_new_scale = round(_new_scale, 6)
# stop at certain scale levels
if (_old_scale < 100.0
and _new_scale > 100.0) or (_new_scale < 100.0
and _old_scale > 100.0):
#print "snap 100:1"
_new_scale = 100.0
self.scale_stop_time = time.time()
elif (_old_scale < 10.0
and _new_scale > 10.0) or (_new_scale < 10.0
and _old_scale > 10.0):
#print "snap 10:1"
_new_scale = 10.0
self.scale_stop_time = time.time()
elif (_old_scale < 1.0
and _new_scale > 1.0) or (_new_scale < 1.0
and _old_scale > 1.0):
#print "snap 1:1"
_new_scale = 1.0
self.scale_stop_time = time.time()
elif (_old_scale < 0.1
and _new_scale > 0.1) or (_new_scale < 0.1
and _old_scale > 0.1):
#print "snap 1:10"
_new_scale = 0.1
self.scale_stop_time = time.time()
elif (_old_scale < 0.01
and _new_scale > 0.01) or (_new_scale < 0.01
and _old_scale > 0.01):
#print "snap 1:100"
_new_scale = 0.01
self.scale_stop_time = time.time()
self.sf_scale.value = _new_scale
else:
if (time.time() - self.scale_stop_time) > self.scale_stop_duration:
self.scale_stop_time = None
class MultiDofDevice(avango.script.Script):
# output fields
## @var mf_dof
# The input values of the input device.
mf_dof = avango.MFFloat()
mf_dof.value = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
] # init 7 channels: [trans_x, trans_y, trans_z, pitch, head, roll, scale]
## @var sf_reset_trigger
# Boolean indicating if a reset of the platform is to be triggered.
sf_reset_trigger = avango.SFBool()
## @var sf_coupling_trigger
# Boolean indicating if a coupling with other platforms is to be triggered.
sf_coupling_trigger = avango.SFBool()
## @var sf_dof_trigger
# Boolean indicating if a change of dof mode is to be triggered.
sf_dof_trigger = avango.SFBool()
## @var sf_station_mat
# Tracking position and rotation.
sf_station_mat = avango.gua.SFMatrix4()
sf_station_mat.value = avango.gua.make_identity_mat()
## Default constructor.
def __init__(self):
self.super(MultiDofDevice).__init__()
## @var input_bindings
# List of bindings between input values / button values and events. Code form.
self.input_bindings = []
# factors for amplifying
## @var translation_factor
# Factor to modify the device's translation input.
self.translation_factor = 1.0
## @var rotation_factor
# Factor to modify the device's rotation input.
self.rotation_factor = 1.0
# init trigger callback
## @var frame_trigger
# Triggers framewise evaluation of frame_callback method
self.frame_trigger = avango.script.nodes.Update(
Callback=self.frame_callback, Active=True)
## Initializes a tracking reader for the device's position and rotation
# @param TRACKING_TARGET_NAME The tracking name as chosen in daemon, None if no tracking is available.
# @param NO_TRACKING_MAT Tracking matrix to be used if no tracking is available.
def init_station_tracking(self, TRACKING_TARGET_NAME, NO_TRACKING_MAT):
## @var tracking_reader
# Tracking Reader to process the tracking input of the device.
if TRACKING_TARGET_NAME != None:
self.tracking_reader = TrackingTargetReader()
self.tracking_reader.my_constructor(TRACKING_TARGET_NAME)
self.sf_station_mat.connect_from(self.tracking_reader.sf_abs_mat)
else:
self.tracking_reader = TrackingDefaultReader()
self.tracking_reader.set_no_tracking_matrix(NO_TRACKING_MAT)
self.sf_station_mat.connect_from(self.tracking_reader.sf_abs_mat)
## Map an input value to a certain interval.
# @param VALUE The value to be mapped.
# @param OFFSET The offset to be applied to VALUE, MIN and MAX.
# @param MIN The minimum value of the old interval.
# @param MAX The maximum value of the old interval.
# @param NEG_THRESHOLD The negative threshold to be used.
# @param POS_THRESHOLD The positive threshold to be used.
def filter_channel(self, VALUE, OFFSET, MIN, MAX, NEG_THRESHOLD,
POS_THRESHOLD):
#print VALUE
VALUE = VALUE - OFFSET
MIN = MIN - OFFSET
MAX = MAX - OFFSET
#print "+", VALUE, MAX, POS_THRESHOLD
#print "-", VALUE, MIN, NEG_THRESHOLD
if VALUE > 0:
_pos = MAX * POS_THRESHOLD * 0.01
if VALUE > _pos: # above positive threshold
VALUE = min((VALUE - _pos) / (MAX - _pos),
1.0) # normalize interval
else: # beneath positive threshold
VALUE = 0
elif VALUE < 0:
_neg = MIN * NEG_THRESHOLD * 0.01
if VALUE < _neg:
VALUE = max((VALUE - _neg) / abs(MIN - _neg), -1.0)
else: # above negative threshold
VALUE = 0
return VALUE
## Sets given values as input channel filtering parameters.
# @param INPUT_CHANNEL_PARAMETERS List on which the following values will be set.
# @param OFFSET The offset to be applied to VALUE, MIN and MAX.
# @param MIN The minimum value of the old interval.
# @param MAX The maximum value of the old interval.
# @param NEG_THRESHOLD The negative threshold to be used.
# @param POS_THRESHOLD The positive threshold to be used.
def set_input_channel_parameters(self, INPUT_CHANNEL_PARAMETERS, OFFSET,
MIN, MAX, NEG_THRESHOLD, POS_THRESHOLD):
INPUT_CHANNEL_PARAMETERS[0] = OFFSET
INPUT_CHANNEL_PARAMETERS[1] = MIN
INPUT_CHANNEL_PARAMETERS[2] = MAX
INPUT_CHANNEL_PARAMETERS[3] = NEG_THRESHOLD
INPUT_CHANNEL_PARAMETERS[4] = POS_THRESHOLD
## Adds an input binding to the list of bindings for this device.
# @param INSTRUCTION The binding in code form to be set.
def add_input_binding(self, INSTRUCTION):
self.input_bindings.append(INSTRUCTION)
## Callback: evaluated every frame
def frame_callback(self):
## @var dofs
# Temporary list of degrees of freedom to process input bindings.
self.dofs = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# evaluate input bindings
for _input_binding in self.input_bindings:
try:
eval(_input_binding)
except Exception as e:
print_error(
"Error parsing input binding " + _input_binding + "(" + e +
")", False)
self.mf_dof.value = self.dofs
## Sets a specific degree of freedom to a value which is filtered before.
# @param ID ID Number of the degree of freedom to be set.
# @param VALUE The value to be filtered.
# @param OFFSET The offset to be applied to VALUE, MIN and MAX.
# @param MIN The minimum value of the old interval.
# @param MAX The maximum value of the old interval.
# @param NEG_THRESHOLD The negative threshold to be used.
# @param POS_THRESHOLD The positive threshold to be used.
def set_and_filter_dof(self, ID, VALUE, OFFSET, MIN, MAX, NEG_THRESHOLD,
POS_THRESHOLD):
self.dofs[ID] += self.filter_channel(VALUE, OFFSET, MIN, MAX,
NEG_THRESHOLD, POS_THRESHOLD)
## Sets a specific degree of freedom to a value.
# @param ID ID Number of the degree of freedom to be set.
# @param VALUE The value to be set.
def set_dof(self, ID, VALUE):
self.dofs[ID] += VALUE
## Sets the reset trigger.
# @param VALUE The value to be set.
def set_reset_trigger(self, VALUE):
if VALUE != self.sf_reset_trigger.value: # only propagate changes
self.sf_reset_trigger.value = VALUE
## Sets the coupling trigger.
# @param VALUE The value to be set.
def set_coupling_trigger(self, VALUE):
if VALUE != self.sf_coupling_trigger.value: # only propagate changes
self.sf_coupling_trigger.value = VALUE
## Sets the dof trigger.
# @param VALUE The value to be set.
def set_dof_trigger(self, VALUE): # only propagate changes
if VALUE != self.sf_dof_trigger.value:
self.sf_dof_trigger.value = VALUE
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__() # call base-class constructor
### parameters ###
self.rot_center_offset = avango.gua.Vec3(0.0, 0.0, 0.0)
self.translation_factor = 1.0
self.rotation_factor = 1.0
def my_constructor(self,
MF_DOF,
MF_BUTTONS,
TRANSLATION_FACTOR=1.0,
ROTATION_FACTOR=1.0):
self.mf_dof.connect_from(MF_DOF)
self.translation_factor = TRANSLATION_FACTOR
self.rotation_factor = ROTATION_FACTOR
### callback functions ###
@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.translation_factor
_trans_input = _trans_vec.length()
#print(_trans_input)
if _trans_input > 0.0:
## transfer-function for translation
_factor = pow(min(_trans_input, 1.0), 2)
_trans_vec.normalize()
_trans_vec *= _factor
## 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.rotation_factor
_rot_input = _rot_vec.length()
if _rot_input > 0.0:
## transfer-function for rotation
_factor = pow(_rot_input, 2)
_rot_vec.normalize()
_rot_vec *= _factor
if _trans_input or _rot_input > 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)
### functions ###
def set_start_matrix(self, MAT4):
self.sf_nav_mat.value = MAT4
def set_rotation_center_offset(self, VEC3):
self.rot_center_offset = VEC3
class Manipulation(avango.script.Script):
### 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
mf_buttons = avango.MFBool()
mf_buttons.value = [False, False] # init 2 channels
### output_fields
sf_mat = avango.gua.SFMatrix4()
sf_mat.value = avango.gua.make_identity_mat()
sf_action_trigger = avango.SFBool()
### constructor
def __init__(self):
self.super(Manipulation).__init__()
### variables ###
self.type = ""
self.enable_flag = False
### callback functions ###
def evaluate(self): # evaluated every frame if any input field has changed
if self.enable_flag == True:
self.manipulate()
@field_has_changed(mf_buttons)
def mf_buttons_changed(self):
if self.enable_flag == True:
_left_button = self.mf_buttons.value[0]
_right_button = self.mf_buttons.value[1]
self.sf_action_trigger.value = _left_button ^ _right_button # button left XOR button right
### functions ###
def enable_manipulation(self, FLAG):
self.enable_flag = FLAG
if self.enable_flag == True:
print(self.type + " enabled")
self.reset()
def manipulate(self):
raise NotImplementedError("To be implemented by a subclass.")
def reset(self):
raise NotImplementedError("To be implemented by a subclass.")
def clamp_matrix(self, MATRIX):
# clamp translation to certain range (within screen space)
_x_range = 0.3 # in meter
_y_range = 0.15 # in meter
_z_range = 0.15 # in meter
MATRIX.set_element(0, 3,
min(_x_range,
max(-_x_range,
MATRIX.get_element(0, 3)))) # clamp x-axis
MATRIX.set_element(1, 3,
min(_y_range,
max(-_y_range,
MATRIX.get_element(1, 3)))) # clamp y-axis
MATRIX.set_element(2, 3,
min(_z_range,
max(-_z_range,
MATRIX.get_element(2, 3)))) # clamp z-axis
return MATRIX
class SteeringNavigation(NavigationTechnique):
### fields ###
maneuvering = False
## 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_button = avango.SFBool()
### constructor
def __init__(self):
NavigationTechnique.__init__(self) # call base class constructor
def my_constructor(self, NAVIGATION_MANAGER, MF_DOF):
### external references ###
self.NAVIGATION_MANAGER = NAVIGATION_MANAGER
self.mf_dof.connect_from(MF_DOF)
### additional parameters ###
self.translation_factor = 0.3
self.rotation_factor = 1.0
self.sf_button.connect_from(self.NAVIGATION_MANAGER.pointer_device_sensor.Button0)
self._intersection_transform = avango.gua.nodes.TransformNode(Name = "intersection_transform")
### functions ###
def enable(self, BOOL): # extend respective base-class function
NavigationTechnique.enable(self, BOOL) # call base-class function
if self.enable_flag == True:
self.NAVIGATION_MANAGER.ray_geometry.Tags.value = [] # set ray visible
else:
self.NAVIGATION_MANAGER.intersection_geometry.Tags.value = ["invisible"]
self.NAVIGATION_MANAGER.ray_geometry.Tags.value = ["invisible"]
### callback functions ###
def evaluate(self): # implement respective base-class function
if self.enable_flag == False:
return
## 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.translation_factor
_trans_input = _trans_vec.length()
#print(_trans_input)
if _trans_input > 0.0: # guard
# transfer-function for translation
_factor = pow(min(_trans_input,1.0), 2)
_trans_vec.normalize()
_trans_vec *= _factor
## 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.rotation_factor
_rot_input = _rot_vec.length()
if _rot_input > 0.0: # guard
# transfer-function for rotation
_factor = pow(_rot_input, 2)
_rot_vec.normalize()
_rot_vec *= _factor
if not self.maneuvering:
## accumulate input
_new_mat = self.NAVIGATION_MANAGER.get_navigation_matrix() * \
avango.gua.make_trans_mat(_trans_vec) * \
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)
self.NAVIGATION_MANAGER.set_navigation_matrix(_new_mat)
self.NAVIGATION_MANAGER.calc_pick_result()
self.NAVIGATION_MANAGER.update_ray_visualization()
# self.pick_result
elif self.maneuvering:
## accumulate input
_old_mat = self.NAVIGATION_MANAGER.get_navigation_matrix()
# _old_rotate = avango.gua.make_rot_mat(_old_mat.get_rotate())
# _intersection_transform.Transform.value = avango.gua.make_trans_mat(self.NAVIGATION_MANAGER.intersection_geometry.WorldTransform.value.get_translate())
_new_mat = _old_mat * \
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)
# print(_spacemouse_rotate)
# initial translation, rotation of navigation point, intersection point
# _camera_rotate = avango.gua.make_rot_mat(self.NAVIGATION_MANAGER.NAVIGATION_NODE.WorldTransform.value.get_rotate())
# print(_intersection_translate)
# print(_old_mat)
# _pointer_rotate = avango.gua.make_rot_mat(self.NAVIGATION_MANAGER.pointer_node.WorldTransform.value.get_rotate())
# print(_pointer_rotate)
# _camera_translate = avango.gua.make_trans_mat(self.NAVIGATION_MANAGER.NAVIGATION_NODE.WorldTransform.value.get_translate())
# _new_mat_rotated = _camera_translate * _spacemouse_rotate
# print(avango.gua.make_rot_mat(_new_mat_rotated.get_rotate()))
# _new_mat = _old_mat * _new_mat_rotated
# print(avango.gua.make_trans_mat(_old_mat.get_translate()))
# print(avango.gua.make_trans_mat(_new_mat.get_translate()))
# _vec = avango.gua.make_rot_mat(self.pointer_node.WorldTransform.value.get_rotate()) * avango.gua.Vec3(0.0,0.0,-1.0)
# _vec = avango.gua.Vec3(_vec.x,_vec.y,_vec.z)
self.NAVIGATION_MANAGER.set_navigation_matrix(_new_mat)
self.NAVIGATION_MANAGER.calc_pick_result()
self.NAVIGATION_MANAGER.update_ray_visualization()
# self.intersection_geometry.Transform
# _old_mat = self.NAVIGATION_MANAGER.get_navigation_matrix()
# print(_old_mat)
# print(self.NAVIGATION_MANAGER.NAVIGATION_NODE.WorldTransform.value)
# self.offset_mat = avango.gua.make_inverse_mat(self.NAVIGATION_MANAGER.NAVIGATION_NODE.WorldTransform.value) \
# * self.NAVIGATION_MANAGER.intersection_geometry.WorldTransform.value
# _new_mat = self.NAVIGATION_MANAGER.NAVIGATION_NODE.WorldTransform.value * self.offset_mat # new object position in world coodinates
# _new_mat = avango.gua.make_inverse_mat(self.NAVIGATION_MANAGER.NAVIGATION_NODE.WorldTransform.value) \
# * _new_mat # transform new object matrix from global to local space
# self.NAVIGATION_MANAGER.set_navigation_matrix(_new_mat)
@field_has_changed(sf_button)
def sf_button_changed(self):
if self.enable_flag == False:
return
## ToDo: enable/disable maneuvering here
# print(self.sf_button.value)
if self.sf_button.value:
self.maneuvering = True
self._intersection_transform.Transform.value = self.NAVIGATION_MANAGER.intersection_geometry.Transform.value
_nav_mat = self.NAVIGATION_MANAGER.NAVIGATION_NODE.WorldTransform.value
self.NAVIGATION_MANAGER.SCENEGRAPH.Root.value.Children.value.remove(self.NAVIGATION_MANAGER.NAVIGATION_NODE)
self._intersection_transform.Children.value.append(self.NAVIGATION_MANAGER.NAVIGATION_NODE)
self.NAVIGATION_MANAGER.NAVIGATION_NODE.Transform.value = avango.gua.make_inverse_mat(_nav_mat) \
* self.NAVIGATION_MANAGER.NAVIGATION_NODE.Transform.value
class InputMapping(avango.script.Script):
## @var mf_rel_input_values
# The relative input values of the device.
mf_rel_input_values = avango.MFFloat()
mf_rel_input_values.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
## @var sf_station_mat
# The absolute matrix indicating where the device is placed in space.
sf_station_mat = avango.gua.SFMatrix4()
sf_station_mat.value = avango.gua.make_identity_mat()
# internal field
## @var sf_abs_uncorrected_mat
# The absolute matrix to accumulate the relative inputs on. Will be corrected by GroundFollowing instance.
sf_abs_uncorrected_mat = avango.gua.SFMatrix4()
sf_abs_uncorrected_mat.value = avango.gua.make_identity_mat()
# output field
## @var sf_abs_mat
# The absolute matrix after GroundFollowing correction.
sf_abs_mat = avango.gua.SFMatrix4()
sf_abs_mat.value = avango.gua.make_identity_mat()
# factors for input amplifying
## @var input_trans_factor
# Factor to modify the translation input.
input_trans_factor = 1.0
## @var input_rot_factor
# Factor to modify the rotation input.
input_rot_factor = 1.0
## Default constructor.
def __init__(self):
self.super(InputMapping).__init__()
# attributes
## @var realistic
# Boolean value to indicate if the user is navigating in 3-DOF (realistic) or 6-DOF (unrealistic) mode.
self.realistic = True
## Custom constructor.
# @param NAVIGATION The navigation instance from which this input mapping is created.
# @param DEVICE_INSTANCE Instance of Device class to take the input values from.
# @param GROUND_FOLLOWING_INSTANCE Instance of GroundFollowing to be used for matrix correction.
# @param STARTING_MATRIX Initial matrix to accumulate the relative inputs on.
def my_constructor(self, NAVIGATION, DEVICE_INSTANCE,
GROUND_FOLLOWING_INSTANCE, STARTING_MATRIX):
## @var NAVIGATION
# Reference to the Navigation instance from which this InputMapping is created.
self.NAVIGATION = NAVIGATION
## @var GROUND_FOLLOWING_INSTANCE
# Reference to the GroundFollowing instance used by this InputMapping.
self.GROUND_FOLLOWING_INSTANCE = GROUND_FOLLOWING_INSTANCE
# connect device fields
self.mf_rel_input_values.connect_from(DEVICE_INSTANCE.mf_dof)
self.sf_station_mat.connect_from(DEVICE_INSTANCE.sf_station_mat)
# connect ground following fields
GROUND_FOLLOWING_INSTANCE.sf_abs_input_mat.connect_from(
self.sf_abs_uncorrected_mat)
self.sf_abs_mat.connect_from(
GROUND_FOLLOWING_INSTANCE.sf_abs_output_mat)
# create feedback loop
self.sf_abs_uncorrected_mat.connect_weak_from(self.sf_abs_mat)
# set the starting position
self.set_abs_mat(STARTING_MATRIX)
## @var lf_quat_angle
# Quaternion angle of last frame to prevent blackscreen. Used if new angle is nan.
self.lf_quat_angle = 0.0
## Evaluated when device input values change.
@field_has_changed(mf_rel_input_values)
def mf_rel_input_values_changed(self):
# get translation values from input device
_trans_vec = avango.gua.Vec3(0, 0, 0)
_trans_vec.x = self.mf_rel_input_values.value[0]
_trans_vec.y = self.mf_rel_input_values.value[1]
_trans_vec.z = self.mf_rel_input_values.value[2]
_trans_vec *= math.pow(_trans_vec.length() / math.sqrt(3),
3) * self.input_trans_factor
# get rotation values from input device
_rot_vec = avango.gua.Vec3(0, 0, 0)
_rot_vec.x = self.mf_rel_input_values.value[3] * self.input_rot_factor
_rot_vec.y = self.mf_rel_input_values.value[4] * self.input_rot_factor
_rot_vec.z = self.mf_rel_input_values.value[5] * self.input_rot_factor
# delete certain values that create an unrealistic movement
if self.realistic:
_trans_vec.y = 0.0
_rot_vec.x = 0.0
_rot_vec.z = 0.0
# only accumulate inputs on absolute matrix when the device values change
if _trans_vec.length() != 0.0 or _rot_vec.length() != 0.0:
# global platform rotation in the world
_platform_quat = self.sf_abs_mat.value.get_rotate()
# Fix if quaternion angle is nan
_quat_angle = _platform_quat.get_angle()
if math.isnan(_quat_angle) == False:
_platform_rot_mat = avango.gua.make_rot_mat(
_quat_angle, _platform_quat.get_axis())
self.lf_quat_angle = _quat_angle
else:
_platform_rot_mat = avango.gua.make_rot_mat(
self.lf_quat_angle, _platform_quat.get_axis())
# global rotation of the device in the world
_device_forward_yaw = Tools.get_yaw(self.sf_station_mat.value)
_device_rot_mat = avango.gua.make_rot_mat(
math.degrees(_device_forward_yaw), 0, 1, 0)
# combined platform and device rotation
_combined_rot_mat = _platform_rot_mat * _device_rot_mat
# rotation center of the device
_rot_center = self.sf_station_mat.value.get_translate()
# transformed translation, rotation and rotation center
_transformed_trans_vec = self.transform_vector_with_matrix(
_trans_vec, _combined_rot_mat)
_transformed_rot_vec = self.transform_vector_with_matrix(
_rot_vec, _combined_rot_mat)
_transformed_rot_center = self.transform_vector_with_matrix(
_rot_center, _platform_rot_mat)
# create new transformation matrix
_new_mat = avango.gua.make_trans_mat(_transformed_trans_vec) * \
self.sf_abs_mat.value * \
avango.gua.make_trans_mat(_rot_center) * \
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(_rot_center * -1)
# update matrix on coupled navigations
_global_rot_center = self.sf_abs_mat.value * _rot_center
_global_rot_center = avango.gua.Vec3(_global_rot_center.x,
_global_rot_center.y,
_global_rot_center.z)
for _navigation in self.NAVIGATION.coupled_navigations:
_navigation.inputmapping.modify_abs_uncorrected_mat(
_transformed_trans_vec, _transformed_rot_vec,
_global_rot_center)
else:
# the device values are all equal to zero
_new_mat = self.sf_abs_mat.value
# save the computed new matrix
self.sf_abs_uncorrected_mat.value = _new_mat
## Modify the uncorrected matrix of this input mapping with specific values. Used for coupling purposes.
# @param TRANSFORMED_TRANS_VECTOR The translation vector to be applied.
# @param TRANSFORMED_ROT_VECTOR The vector containing the rotation values to be applied.
# @param ROTATION_CENTER The center to rotate around.
def modify_abs_uncorrected_mat(self, TRANSFORMED_TRANS_VECTOR,
TRANSFORMED_ROT_VECTOR, ROTATION_CENTER):
# compute new translation
_new_pos = TRANSFORMED_TRANS_VECTOR + self.sf_abs_mat.value.get_translate(
)
# compute offset to rotation center
_rot_center_offset = ROTATION_CENTER - _new_pos
# create new transformation matrix
_quat = self.sf_abs_mat.value.get_rotate()
_new_mat = avango.gua.make_trans_mat(_new_pos) * \
avango.gua.make_trans_mat(_rot_center_offset) * \
avango.gua.make_rot_mat( TRANSFORMED_ROT_VECTOR.y, 0, 1, 0) * \
avango.gua.make_rot_mat( TRANSFORMED_ROT_VECTOR.x, 1, 0, 0) * \
avango.gua.make_rot_mat( TRANSFORMED_ROT_VECTOR.z, 0, 0, 1) * \
avango.gua.make_trans_mat(_rot_center_offset * -1) * \
avango.gua.make_rot_mat(_quat.get_angle(), _quat.get_axis())
# save the computed new matrix
self.sf_abs_mat.value = _new_mat
## Transforms a vector using a transformation matrix.
# @param VECTOR The vector to be transformed.
# @param MATRIX The matrix to be applied for transformation.
def transform_vector_with_matrix(self, VECTOR, MATRIX):
_trans_vec = MATRIX * VECTOR
return avango.gua.Vec3(_trans_vec.x, _trans_vec.y, _trans_vec.z)
## Set a value for sf_abs_mat.
# @param MATRIX The matrix to be set to.
def set_abs_mat(self, MATRIX):
self.sf_abs_mat.value = MATRIX
## Sets the translation and rotation input factors.
# @param TRANSLATION_FACTOR Translation modification factor to be set. 1.0 by default.
# @param ROTATION_FACTOR Rotation modification factor to be set. 1.0 by default.
def set_input_factors(self, TRANSLATION_FACTOR=1.0, ROTATION_FACTOR=1.0):
self.input_trans_factor = TRANSLATION_FACTOR
self.input_rot_factor = ROTATION_FACTOR
## Activates the realistic mode (only 3 DOF navigation, GroundFollowing enabled)
def activate_realistic_mode(self):
self.realistic = True
self.GROUND_FOLLOWING_INSTANCE.activate()
## Activates the unrealistic mode (6 DOF navigation, GroundFollowing disabled)
def deactivate_realistic_mode(self):
self.realistic = False
self.GROUND_FOLLOWING_INSTANCE.deactivate()
class Action(avango.script.Script):
# Input
Keyframes = avango.gua.MFMatrix4()
TimeStamps = avango.MFFloat()
Time = avango.SFFloat()
# Output
OutTransform = avango.gua.SFMatrix4()
def __init__(self):
self.super(Action).__init__()
self.Time.value = 0.0
#Stuff
self.frame_count = 0
self.duration = 0.0
self.loop = False
def constructor(self, json_keyframes):
for frame in json_keyframes:
matrix_list = frame["transform"]
matrix = load_transform_matrix(matrix_list)
time_stamp = frame["time"]
self.Keyframes.value.append(matrix)
self.TimeStamps.value.append(time_stamp)
self.frame_count = len(self.Keyframes.value)
self.duration = self.TimeStamps.value[self.frame_count - 1]
def evaluate(self):
frame = self.find_key_frame()
if not frame == -1:
self.OutTransform.value = self.interpolate(frame)
else:
self.OutTransform.value = avango.gua.make_identity_mat()
def get_time(self):
if self.loop:
return self.Time.value % self.duration
return self.Time.value
def find_key_frame(self):
time = self.get_time()
for i in range(self.frame_count):
if time < self.TimeStamps.value[i]:
return i
elif time > self.duration:
return self.frame_count - 1
return -1
def interpolate(self, frame):
time = self.get_time()
if frame == 0 or time > self.duration:
return self.Keyframes.value[frame]
else:
time_prev = self.TimeStamps.value[frame - 1]
time_next = self.TimeStamps.value[frame]
delta = time_next - time_prev
current = time - time_prev
factor = current / delta
return self.Keyframes.value[frame-1] * (1-factor) +\
self.Keyframes.value[frame] * (factor)
