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_compose_transformation(self):
''' Test quaternion to euler angle '''
R = np.array(([3, 6, 1], [2, 5, 2], [1, 4, 3]))
t = np.array((1, 2, 3))
truth = np.array(([3, 6, 1, 0], [2, 5, 2, 0], [1, 4, 3,
0], [1, 2, 3, 1]))
test = compose_transformation(R, t)
np.testing.assert_almost_equal(
test, truth, err_msg="Error composing transformation")
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 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_compose_transformation(self):
''' Test quaternion to euler angle '''
R = np.array(([3, 6, 1],
[2, 5, 2],
[1, 4, 3]))
t = np.array((1, 2, 3))
truth = np.array(([3, 6, 1, 0],
[2, 5, 2, 0],
[1, 4, 3, 0],
[1, 2, 3, 1]))
test = compose_transformation(R, t)
np.testing.assert_almost_equal(
test, truth, err_msg="Error composing transformation")
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