def box(min=[0, 0, 0],
max=[1, 1, 1],
filled=False,
color=QColor("blue"),
effect_f=CustomEffects.material,
matrix=np.eye(4, dtype='f4'),
name="box"):
color = ensure_QColor(color)
indices = [0,1,3, 3,2,0, 7,5,4, 4,6,7, 4,5,1, 1,0,4, 5,7,3, 3,1,5, 6,2,3, 3,7,6, 4,0,2, 2,6,4] if filled else\
[0,1, 1,3, 3,2, 2,0, 4,5, 5,7, 7,6, 6,4, 0,4, 1,5, 2,6, 3,7]
vertices = [
[min[0], min[1], min[2]] #0
,
[min[0], min[1], max[2]] #1
,
[min[0], max[1], min[2]] #2
,
[min[0], max[1], max[2]] #3
,
[max[0], min[1], min[2]] #4
,
[max[0], min[1], max[2]] #5
,
[max[0], max[1], min[2]] #6
,
[max[0], max[1], max[2]] #7
]
normals = [
[-0.577350269, -0.577350269, -0.577350269] #0
,
[-0.577350269, -0.577350269, 0.577350269] #1
,
[-0.577350269, 0.577350269, -0.577350269] #2
,
[-0.577350269, 0.577350269, 0.577350269] #3
,
[0.577350269, -0.577350269, -0.577350269] #4
,
[0.577350269, -0.577350269, 0.577350269] #5
,
[0.577350269, 0.577350269, -0.577350269] #6
,
[0.577350269, 0.577350269, 0.577350269] #7
]
return Actors.Actor(
geometry=Geometry.Geometry(
indices=Array.Array(ndarray=np.array(indices, 'u4')),
attribs=Geometry.Attribs(
vertices=Array.Array(ndarray=np.array(vertices, 'f4')),
normals=Array.Array(ndarray=np.array(normals, 'f4'))),
primitive_type=Geometry.PrimitiveType.TRIANGLES
if filled else Geometry.PrimitiveType.LINES),
effect=effect_f(color) if filled else CustomEffects.emissive(color),
transform=ensure_Transform(matrix),
name=name)
def colored_point_cloud(points,
colors,
matrix=np.eye(4, dtype='f4'),
name="pcl"):
#color = ensure_QColor(color)
return Actors.Actor(geometry=Geometry.Geometry(
attribs=CustomAttribs.ColorsAttribs(
vertices=Array.Array(ndarray=points),
colors=Array.Array(ndarray=colors)),
primitive_type=Geometry.PrimitiveType.POINTS),
effect=CustomEffects.point_colors(),
transform=ensure_Transform(matrix),
name=name)
def lines(indices,
vertices,
color=QColor("blue"),
matrix=np.eye(4, dtype='f4'),
name="lines"):
color = ensure_QColor(color)
return Actors.Actor(geometry=Geometry.Geometry(
indices=Array.Array(ndarray=indices),
attribs=Geometry.Attribs(vertices=Array.Array(ndarray=vertices)),
primitive_type=Geometry.PrimitiveType.LINES),
effect=CustomEffects.emissive(color),
transform=ensure_Transform(matrix),
name=name)
def quad(top_left=[0, 0, 0],
bottom_left=[0, 1, 0],
bottom_right=[1, 1, 0],
top_right=None,
texcoords=[[0, 0], [0, 1], [1, 1], [1, 0]],
filled=False,
color=QColor("blue"),
effect_f=CustomEffects.material,
textures=None,
matrix=np.eye(4, dtype='f4'),
name="quad"):
color = ensure_QColor(color)
top_left = utils.to_numpy(top_left)
bottom_left = utils.to_numpy(bottom_left)
bottom_right = utils.to_numpy(bottom_right)
v_top = top_left - bottom_left
v_right = bottom_right - bottom_left
if top_right is None:
top_right = bottom_right + v_top
else:
top_right = utils.to_numpy(top_right)
indices = [0, 1, 2, 3, 0, 2
] if filled else [0, 1, 1, 2, 2, 0, 3, 0, 0, 2, 2, 3]
vertices = np.vstack((top_left, bottom_left, bottom_right, top_right))
normals = np.empty_like(vertices)
n = np.cross(v_right, v_top)
normals[:] = n / np.linalg.norm(n)
if textures is not None:
effect = effect_f(textures=textures, color=color)
else:
effect = effect_f(color=color)
return Actors.Actor(geometry=Geometry.Geometry(
indices=Array.Array(ndarray=np.array(indices, 'u4')),
attribs=CustomAttribs.TexcoordsAttribs(
vertices=Array.Array(ndarray=np.array(vertices, 'f4')),
normals=Array.Array(ndarray=np.array(normals, 'f4')),
texcoords0=Array.Array(ndarray=np.array(texcoords, 'f4'))),
primitive_type=Geometry.PrimitiveType.TRIANGLES
if filled else Geometry.PrimitiveType.LINES),
effect=effect,
transform=ensure_Transform(matrix),
name=name)
def shallow_copy(actor,
effect_f=CustomEffects.material,
color=None,
matrix=None,
name=None,
instance_id=None):
name = actor.objectName() if name is None else name
instance_id = actor.instance_id if instance_id is None else instance_id
if isinstance(actor, Actors.Actor):
return Actors.Actor(
geometry=actor.geometry,
effect=actor.effect if color is None else effect_f(color),
transform=actor.transform
if matrix is None else ensure_Transform(matrix),
name=name,
bbox=actor.bbox,
type_id=actor.type_id,
instance_id=instance_id)
elif isinstance(actor, Actors.Actors):
tf = ensure_Transform(matrix)
actors = Actors.Actors(name=name,
bbox=actor.bbox,
shared_transform=tf,
scale=actor.scale,
all_vertices=actor.all_vertices,
type_id=actor.type_id,
instance_id=instance_id)
for a in actor.children_actors():
actors.addActor(
shallow_copy(a, effect_f, color, tf, a.objectName(),
instance_id))
return actors
def text(text,
font="Arial",
font_size=6,
line_width=1,
color=QColor("blue"),
matrix=np.eye(4, dtype='f4'),
is_billboard=True,
name="text",
scale=0.1,
origin=[0, 0, 0],
u=[1, 0, 0],
v=[0, 1, 0],
w=[0, 0, 1]):
'''
Warning, this function can crash if called before any call to QApplication(sys.argv)
'''
color = ensure_QColor(color)
origin = utils.to_numpy(origin)
u = utils.to_numpy(u)
v = utils.to_numpy(v)
w = utils.to_numpy(w)
indices = []
vertices = []
path = QPainterPath()
path.addText(QPointF(0, 0), QFont(font, font_size), text)
polygons = path.toSubpathPolygons()
for polygon in polygons:
for point in polygon:
indices.append(len(vertices))
p = utils.to_numpy([point.x(), point.y(), 0]) * scale
vertices.append(origin + p[0] * u + p[1] * v + p[2] * w)
indices.append(-1)
return Actors.Actor(geometry=Geometry.Geometry(
indices=Array.Array(ndarray=np.array(indices, 'u4')),
attribs=Geometry.Attribs(vertices=Array.Array(
ndarray=np.array(vertices, 'f4'))),
primitive_type=Geometry.PrimitiveType.LINE_LOOP),
effect=CustomEffects.emissive(
color,
line_width=line_width,
is_billboard=is_billboard),
transform=ensure_Transform(matrix),
name=f"{name}_{text}")
def from_mesh(mesh,
color=QColor("blue"),
effect_f=CustomEffects.material,
scale=1,
matrix=np.eye(4, dtype='f4'),
name="mesh"):
color = ensure_QColor(color)
mesh.add_attribute("vertex_normal")
return Actors.Actor(geometry=Geometry.Geometry(
indices=Array.Array(ndarray=mesh.faces.flatten().astype('u4')),
attribs=Geometry.Attribs(
vertices=Array.Array(ndarray=mesh.vertices.astype('f4') * scale),
normals=Array.Array(ndarray=mesh.get_vertex_attribute(
"vertex_normal").astype('f4'))),
primitive_type=Geometry.PrimitiveType.TRIANGLES),
effect=effect_f(color=color),
transform=ensure_Transform(matrix),
name=name)
def chessboard(nx=4,
ny=3,
dx=0.1,
dy=0.1,
matrix=np.eye(4, dtype='f4'),
name="chessboard",
color1="white",
color2="black"):
actors = Actors.Actors()
actors.chessboard_shared_Transform = ensure_Transform(matrix)
n_squares_x = nx + 1
n_squares_y = ny + 1
for x in range(n_squares_x + 2):
cb_x_i = x - 2
x_even = (cb_x_i) % 2 == 0
x_border = x in (0, n_squares_x + 1)
for y in range(n_squares_y + 2):
cb_y_i = y - 2
y_even = cb_y_i % 2 == 0
y_border = y in (0, n_squares_y + 1)
is_border = x_border or y_border
color_name = color1 if is_border or (x_even != y_even) else color2
actor = actors.addActor(
quad(
top_left=[(x - 2) * dx, (y - 2) * dy,
0] #we want the (0,0) detected point in (0,0,0)
,
bottom_left=[(x - 2) * dx, (y - 1) * dy, 0],
bottom_right=[(x - 1) * dx, (y - 1) * dy, 0],
top_right=[(x - 1) * dx, (y - 2) * dy, 0],
filled=True,
matrix=actors.chessboard_shared_Transform,
color=QColor(color_name),
name=f"{name}_{color_name}_{x}_{y}"))
actor.effect.shader0.uniforms[
'back_color'] = actor.effect.shader0.uniforms['color']
actors.setObjectName(name)
return actors
def axes(size=1, matrix=np.eye(4, dtype='f4'), line_width=1, name="axes"):
indices = [0, 1, 2, 3, 4, 5]
vertices = [
[0, 0, 0] #0
,
[size, 0, 0] #1
,
[0, 0, 0] #2
,
[0, size, 0] #3
,
[0, 0, 0] #4
,
[0, 0, size] #5
]
colors = [
[1, 0, 0] #red
,
[1, 0, 0] #red
,
[0, 1, 0] #green
,
[0, 1, 0] #green
,
[0, 0, 1] #blue
,
[0, 0, 1] #blue
]
return Actors.Actor(
geometry=Geometry.Geometry(
indices=Array.Array(ndarray=np.array(indices, 'u4')),
attribs=CustomAttribs.ColorsAttribs(
vertices=Array.Array(ndarray=np.array(vertices, 'f4')),
colors=Array.Array(ndarray=np.array(colors, 'f4'))),
primitive_type=Geometry.PrimitiveType.LINES),
effect=CustomEffects.point_colors(line_width=line_width),
transform=ensure_Transform(matrix),
name=name)
def bbox(c=[0, 0, 0],
d=[0, 0, 0],
r=[0, 0, 0],
color=QColor("blue"),
effect_f=CustomEffects.material,
matrix=np.eye(4, dtype='f4'),
name="bbox",
return_anchor=False,
draw_orientation=True):
color = ensure_QColor(color)
indices = [
0, 1, 1, 3, 3, 2, 2, 0, 4, 5, 5, 7, 7, 6, 6, 4, 0, 4, 1, 5, 2, 6, 3, 7
]
vertices = linalg.bbox_to_8coordinates(c, d, r)
vertices = np.vstack([vertices, np.mean(vertices[[5, 7]], axis=0)])
if draw_orientation:
# add central front point
# and indices to draw the direction arrow
indices += [1, 8, 3, 8]
text_anchor = vertices[np.argmin(np.sum(vertices, axis=1))]
vertices = vertices.astype('f4')
actor = Actors.Actor(geometry=Geometry.Geometry(
indices=Array.Array(ndarray=np.array(indices, 'u4')),
attribs=Geometry.Attribs(vertices=Array.Array(ndarray=vertices)),
primitive_type=Geometry.PrimitiveType.LINES),
effect=CustomEffects.emissive(color),
transform=ensure_Transform(matrix),
name=name)
if return_anchor:
return actor, vertices[-1]
else:
return actor
def colored_quad_cloud(points,
amplitude,
indices,
colormap="viridis",
log_scale=False,
cm_resolution=256,
matrix=np.eye(4, dtype='f4'),
color=None,
name="quad"):
#color = ensure_QColor(color)
min_amplitude = float(amplitude.min())
max_amplitude = float(amplitude.max())
if log_scale:
norm = mpl_colors.LogNorm(1, cm_resolution)
else:
norm = mpl_colors.Normalize(0, cm_resolution)
colormap_ = getattr(cm, colormap)(norm(np.arange(cm_resolution)))
cm_array = np.ascontiguousarray(colormap_ * 255, dtype=np.uint8)
if color is None:
effect = CustomEffects.color_map(Array.Array(ndarray=cm_array),
amplitude.min(), amplitude.max())
else:
effect = CustomEffects.material(color=color)
return Actors.Actor(geometry=Geometry.Geometry(
indices=Array.Array(ndarray=indices),
attribs=CustomAttribs.ColorsAttribs(
vertices=Array.Array(ndarray=points),
colors=Array.Array(ndarray=amplitude)),
primitive_type=Geometry.PrimitiveType.TRIANGLES),
effect=effect,
transform=ensure_Transform(matrix),
name=name)
def colormap_point_cloud(points,
amplitude,
min_amplitude=None,
max_amplitude=None,
colormap="viridis",
log_scale=False,
cm_resolution=256,
matrix=np.eye(4, dtype='f4'),
name="cmap_pcl"):
"""
resolution: the color map texture resolution, it affect granularity/precision of the color distribution only
"""
if min_amplitude is None:
min_amplitude = float(amplitude.min())
if max_amplitude is None:
max_amplitude = float(amplitude.max())
if log_scale:
norm = mpl_colors.LogNorm(1, cm_resolution)
else:
norm = mpl_colors.Normalize(0, cm_resolution)
colormap_ = getattr(cm, colormap)(norm(np.arange(cm_resolution)))
cm_array = np.ascontiguousarray(colormap_ * 255, dtype=np.uint8)
return Actors.Actor(geometry=Geometry.Geometry(
attribs=CustomAttribs.AmplitudeAttribs(
vertices=Array.Array(ndarray=points),
amplitude=Array.Array(ndarray=amplitude)),
primitive_type=Geometry.PrimitiveType.POINTS),
effect=CustomEffects.color_map(
Array.Array(ndarray=cm_array), min_amplitude,
max_amplitude),
transform=ensure_Transform(matrix),
name=name)
def load_collada(filename,
scale=1,
matrix=np.eye(4, dtype='f4'),
name="collada",
bake_matrix=True,
merge_actors=True,
ignore_non_textured=False,
invert_normals=False,
type_id=-1,
instance_id=-1):
actors = Actors.Actors(shared_transform=ensure_Transform(
np.eye(4, dtype='f4')) if bake_matrix else ensure_Transform(matrix),
name=name,
type_id=type_id,
instance_id=instance_id)
mesh = collada.Collada(filename)
np_matrix = utils.to_numpy(matrix)
textures_cache = {}
actors_cache = {}
bbox = np.full((2, 3), np.finfo('f4').max)
bbox[1, :] = np.finfo('f4').min
actors.all_vertices = []
actors.scale = scale
for coll_geom in tqdm.tqdm(mesh.scene.objects('geometry')):
for coll_prim in coll_geom.primitives():
#FIXME: stop ignoring colladas transforms
if isinstance(coll_prim, collada.triangleset.BoundTriangleSet):
triangles = coll_prim
elif isinstance(coll_prim, collada.polylist.BoundPolylist):
triangles = coll_prim.triangleset()
else:
LoggingManager.instance().warning(
f"{type(coll_prim)} not implementend")
continue
textures = {}
effect_signature = [] #for merging actors
uniforms = {}
for effect_name in triangles.material.effect.supported:
value = getattr(triangles.material.effect, effect_name)
if isinstance(value, collada.material.Map):
texture_image = value.sampler.surface.image
effect_signature.append((effect_name, texture_image.id))
if texture_image.id in textures_cache:
textures[effect_name] = textures_cache[
texture_image.id]
else:
array = textures[effect_name] = textures_cache[
texture_image.id] = Array.Array(
ndarray=utils.load_texture(
texture_image.pilimage))
elif isinstance(value, tuple):
uniforms[effect_name] = QColor.fromRgbF(*value)
effect_signature.append((effect_name, value))
elif isinstance(value, float):
uniforms[effect_name] = value
effect_signature.append((effect_name, value))
elif value is not None:
LoggingManager.instance().warning(
f"Unsupported type {effect_name}: {type(value)}")
if not textures and ignore_non_textured:
continue
effect_signature = frozenset(effect_signature)
triangles.generateNormals()
vertices = triangles.vertex.astype('f4') * scale
normals = triangles.normal.astype('f4')
if invert_normals:
normals = normals * -1
if bake_matrix:
vertices = linalg.map_points(np_matrix, vertices)
normals = linalg.map_vectors(np_matrix, normals)
indices = triangles.vertex_index.flatten().astype('u4')
attributes_ndarrays = {"vertices": vertices, "normals": normals}
indexed_vertices = vertices[triangles.vertex_index.flatten()]
for i in range(3):
bbox[0, i] = min(bbox[0, i], indexed_vertices[:, i].min())
bbox[1, i] = max(bbox[1, i], indexed_vertices[:, i].max())
if textures:
if len(triangles.texcoordset) > 1:
LoggingManager.instance().warning(
f"warning, {type(coll_prim)} not implementend")
orig_tc0 = triangles.texcoordset[0].astype('f4')
tc0_idx = triangles.texcoord_indexset[0].flatten()
if not np.all(tc0_idx == indices):
assert tc0_idx.shape == indices.shape, "texcoord indices must be the same shape as vertex indices"
#this will duplicate shared vertices so that we can have a separate texcoords for each triangle sharing vertices
attributes_ndarrays['vertices'] = indexed_vertices
attributes_ndarrays['normals'] = normals[
triangles.normal_index.flatten()]
indices = np.arange(indices.shape[0], dtype=indices.dtype)
uv = orig_tc0[tc0_idx]
else:
uv = np.empty((vertices.shape[0], 2), 'f4')
uv[indices] = orig_tc0[tc0_idx]
attributes_ndarrays['texcoords0'] = uv
attribs = CustomAttribs.TexcoordsAttribs(
vertices=Array.Array(
ndarray=attributes_ndarrays['vertices']),
normals=Array.Array(
ndarray=attributes_ndarrays['normals']),
texcoords0=Array.Array(
ndarray=attributes_ndarrays['texcoords0']))
#FIXME: bind collada uniforms if present
effect = CustomEffects.textured_material(textures)
else:
attribs = Geometry.Attribs(
vertices=Array.Array(
ndarray=attributes_ndarrays['vertices']),
normals=Array.Array(
ndarray=attributes_ndarrays['normals']))
#FIXME: bind other uniforms if present
effect = CustomEffects.material(color=uniforms['diffuse'],
back_color=uniforms['diffuse'])
if invert_normals:
indices = indices.reshape((indices.shape[0] // 3),
3)[:, [0, 2, 1]].flatten()
if merge_actors and effect_signature in actors_cache:
actor = actors_cache[effect_signature]
actor_attributes = actor.geometry.attribs.get_attributes()
n_vertices_before = actor_attributes['vertices'].shape[0]
for attr_name, value in actor_attributes.items():
value.set_ndarray(
np.vstack(
(value.ndarray, attributes_ndarrays[attr_name])))
actor.geometry.indices.set_ndarray(
np.hstack((actor.geometry.indices.ndarray,
indices + n_vertices_before)))
else:
geometry = Geometry.Geometry(
indices=Array.Array(ndarray=indices), attribs=attribs)
actor = actors.addActor(
Actors.Actor(geometry=geometry,
effect=effect,
transform=actors.shared_transform,
name=f"{name}_{coll_geom.original.id}",
type_id=type_id,
instance_id=instance_id))
actors_cache[effect_signature] = actor
actors.all_vertices.append(
actor.geometry.attribs.vertices
) #if in merge actor mode, vertices are already there
actors.bbox = bbox
return actors
def lane(vertices,
color,
double=False,
dashed=False,
width=0.07,
matrix=np.eye(4, dtype='f4')):
actors = Actors.Actors()
offset = width if double else 0
nb_lanes = 2 if double else 1
dash_lenght = 1
lenght_drew = 0
total_lenght = 0
for n in range(nb_lanes):
ddashed = dashed[n] if double else dashed
if ddashed:
# For dashed lanes, the distance between vertices needs to be shorter than the dash lenght to be properly displayed
# So, we upsample if required
while np.max(((vertices[1:, 0] - vertices[:-1, 0])**2 +
(vertices[1:, 1] - vertices[:-1, 1])**2)**
0.5) > dash_lenght / 2:
new_vertices = (vertices[:-1] + vertices[1:]) / 2
vertices = np.insert(vertices,
np.indices(vertices.shape)[0, 1:, 0],
new_vertices,
axis=0)
for i in range(vertices.shape[0] - 1):
v1x, v1y, v1z = vertices[i, 0], vertices[i, 1], vertices[i, 2]
v2x, v2y, v2z = vertices[i + 1, 0], vertices[i + 1,
1], vertices[i + 1, 2]
# Rotate the lane segment so its aspect is preserved in all orientations
angle = np.arctan(abs(v2y - v1y) / abs(v2x - v1x))
dx = (width / 2) * np.sin(angle)
dy = (width / 2) * np.cos(angle)
doffx = offset * np.sin(angle)
doffy = offset * np.cos(angle)
# Draw half the time only
if ddashed:
lenght = ((v2x - v1x)**2 + (v2y - v1y)**2)**0.5
total_lenght += lenght
if lenght_drew > total_lenght / 2 and int(total_lenght %
(2 * dash_lenght)):
continue
lenght_drew += lenght
actor = actors.addActor(
quad(top_left=[v2x + dx + doffx, v2y + dy + doffy, v2z],
bottom_left=[v1x + dx + doffx, v1y + dy + doffy, v1z],
bottom_right=[v1x - dx + doffx, v1y - dy + doffy, v1z],
top_right=[v2x - dx + doffx, v2y - dy + doffy, v2z],
filled=True,
matrix=matrix,
color=color,
name='lanes',
effect_f=CustomEffects.emissive_both_sides))
offset *= -1 #for double lanes, reverse the offset for the second lane
return actors