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)