def make_visual(self, physical, position, thrust_indicators=False): visual = self.rendering_world.add_mesh(Sub8Visual, position, orientation=None, color=(20, 20, 20, 1), shininess=20) self.rendering_world.add_entity(rendering.Indicator, physical, radius=0.05) self.rendering_world.add_entity(rendering.Indicator, physical, get_param=lambda o: np.array(o.last_force), radius=0.025, color=(200, 10, 0), scaling_factor=0.01) class ThrustGetter(object): '''This is a pretty garbage thing, and will be replaced by the scenegraph system ''' def __init__(self, thruster_name, rdir): self.thruster_name = thruster_name self.rdir = rdir def __call__(self, entity): return entity.thrust_dict.get(self.thruster_name, 0.0) * np.array([0.0, 0.0, 1.0]) if thrust_indicators: # add the thrust indicators for name, rel_direction, rel_position in self.thruster_list: R = make_rotation(np.array([0.0, 0.0, 1.0]), rel_direction) transformation = compose_transformation(np.transpose(R), rel_position) t = ThrustGetter(name, rel_direction) self.rendering_world.add_entity( rendering.Indicator, physical, get_param=t, rigid=True, offset=transformation, radius=0.01, scaling_factor=0.01, color=(0, 40, 200) ) return visual
def make_visual(self, physical, position, thrust_indicators=False): visual = self.rendering_world.add_mesh(Sub8Visual, position, orientation=None, color=(20, 20, 20, 1), shininess=20) self.rendering_world.add_entity(rendering.Indicator, physical, radius=0.05) self.rendering_world.add_entity( rendering.Indicator, physical, get_param=lambda o: np.array(o.last_force), radius=0.025, color=(200, 10, 0), scaling_factor=0.01) class ThrustGetter(object): '''This is a pretty garbage thing, and will be replaced by the scenegraph system ''' def __init__(self, thruster_name, rdir): self.thruster_name = thruster_name self.rdir = rdir def __call__(self, entity): return entity.thrust_dict.get(self.thruster_name, 0.0) * np.array([0.0, 0.0, 1.0]) if thrust_indicators: # add the thrust indicators for name, rel_direction, rel_position in self.thruster_list: R = make_rotation(np.array([0.0, 0.0, 1.0]), rel_direction) transformation = compose_transformation( np.transpose(R), rel_position) t = ThrustGetter(name, rel_direction) self.rendering_world.add_entity(rendering.Indicator, physical, get_param=t, rigid=True, offset=transformation, radius=0.01, scaling_factor=0.01, color=(0, 40, 200)) return visual
def test_make_rotation(self): '''Test several random vector pairs, and see if we can generate a valid alignment''' scaling = 10 for k in range(10): p = (np.random.random(3) - 0.5) * scaling q = (np.random.random(3) - 0.5) * scaling R = make_rotation(p, q) p_rotated = R.dot(p) # Test that the matrix actually aligns p with q np.testing.assert_allclose([0.0, 0.0, 0.0], np.cross(p_rotated, q), atol=1e-5, err_msg="The generated rotation matrix did not align the input vectors, {} to {}".format( p, q) ) self.assertGreater(np.dot(p_rotated, q), 0.0, msg="The rotation did wacky inversion")
def draw(self): # pose = self.physics_entity.pose pos = self.physics_entity.pos directed_quantity = self.get_param_func(self.physics_entity) norm = np.linalg.norm(directed_quantity) if np.isclose(norm, 0.0, atol=1e-3): return R = make_rotation(np.array([0.0, 0.0, 1.0]), directed_quantity) if not self.rigid: model = compose_transformation(np.transpose(R), pos) else: model = compose_transformation(R, (0, 0, 0)).dot(self.physics_entity.pose) self.program['u_model'] = self.offset.dot(model) self.program['u_length_scale'] = norm * self.scaling_factor # Draw self.program.draw('triangles', self.faces)
def draw(self): # pose = self.physics_entity.pose pos = self.physics_entity.pos directed_quantity = self.get_param_func(self.physics_entity) norm = np.linalg.norm(directed_quantity) if np.isclose(norm, 0.0, atol=1e-3): return R = make_rotation(np.array([0.0, 0.0, 1.0]), directed_quantity) if not self.rigid: model = compose_transformation(np.transpose(R), pos) else: model = compose_transformation(R, (0, 0, 0)).dot( self.physics_entity.pose) self.program['u_model'] = self.offset.dot(model) self.program['u_length_scale'] = norm * self.scaling_factor # Draw self.program.draw('triangles', self.faces)
def test_make_rotation(self): '''Test several random vector pairs, and see if we can generate a valid alignment''' scaling = 10 for k in range(10): p = (np.random.random(3) - 0.5) * scaling q = (np.random.random(3) - 0.5) * scaling R = make_rotation(p, q) p_rotated = R.dot(p) # Test that the matrix actually aligns p with q np.testing.assert_allclose( [0.0, 0.0, 0.0], np.cross(p_rotated, q), atol=1e-5, err_msg= "The generated rotation matrix did not align the input vectors, {} to {}" .format(p, q)) self.assertGreater(np.dot(p_rotated, q), 0.0, msg="The rotation did wacky inversion")
def add_sub(self, position): ''' TODO: This should be some kind of seperate widget, but alas, it is currently just jammed in here ''' visual = self.rendering_world.add_mesh(Sub8Visual, position, orientation=None, color=(20, 20, 20, 1), shininess=20) physical = self.physics_world.add_entity(Sub8, position) self.rendering_world.add_entity(rendering.Indicator, physical, radius=0.05) self.rendering_world.add_entity( rendering.Indicator, physical, get_param=lambda o: np.array(o.last_force), radius=0.025, color=(200, 10, 0), scaling_factor=0.01) thruster_list = [ ("FLV", np.array([0.000, 0.0, -1]), np.array([0.1583, 0.16900, 0.0142])), # flake8: noqa ("FLL", np.array([-0.866, 0.5, 0]), np.array([0.2678, 0.27950, 0.0000])), # flake8: noqa ("FRV", np.array([0.000, 0.0, -1]), np.array([0.1583, -0.1690, 0.0142])), # flake8: noqa ("FRL", np.array([-0.866, -0.5, 0]), np.array([0.2678, -0.2795, 0.0000])), # flake8: noqa ("BLV", np.array([0.000, 0.0, 1]), np.array([-0.1583, 0.16900, 0.0142])), # flake8: noqa ("BLL", np.array([0.866, 0.5, 0]), np.array([-0.2678, 0.27950, 0.0000])), # flake8: noqa ("BRV", np.array([0.000, 0.0, 1]), np.array([-0.1583, -0.1690, 0.0142])), # flake8: noqa ("BRL", np.array([0.866, -0.5, 0]), np.array([-0.2678, -0.2795, 0.0000])), # flake8: noqa ] # add the thrust indicators for name, rel_direction, rel_position in thruster_list: R = make_rotation(np.array([0.0, 0.0, 1.0]), rel_direction) transformation = compose_transformation(np.transpose(R), rel_position) class ThrustGetter(object): def __init__(self, thruster_name, rdir): self.thruster_name = thruster_name self.rdir = rdir def __call__(self, entity): return entity.thrust_dict.get( self.thruster_name, 0.0) * np.array([0.0, 0.0, 1.0]) t = ThrustGetter(name, rel_direction) self.rendering_world.add_entity(rendering.Indicator, physical, get_param=t, rigid=True, offset=transformation, radius=0.01, scaling_factor=0.01, color=(0, 40, 200)) self.entity_pairs.append((visual, physical))