def _calc_robot_polygon(self, x, y, theta):
"""
Prepare a QPolygonF representing the robot by a triangle, given a pose
@return: a QPolygonF object.
"""
# angle coordinates translation
theta = math.radians(theta)
# 3 points around the gravity center (x, y)
# all oriented following the theta angle
x1 = (self.ROBOT_WIDTH_MM // 2) * math.cos(theta)
y1 = (self.ROBOT_WIDTH_MM // 2) * math.sin(theta)
x2 = (self.ROBOT_WIDTH_MM // 2) * math.cos(theta + 2.5 *
(math.pi / 3.))
y2 = (self.ROBOT_WIDTH_MM // 2) * math.sin(theta + 2.5 *
(math.pi / 3.))
x3 = (self.ROBOT_WIDTH_MM // 2) * math.cos(theta + 3.5 *
(math.pi / 3.))
y3 = (self.ROBOT_WIDTH_MM // 2) * math.sin(theta + 3.5 *
(math.pi / 3.))
points = [(x + x1, y + y1), (x + x2, y + y2), (x + x3, y + y3)]
qpoints = [QtCore.QPointF(x, y) for (x, y) in points]
qpoly = QtGui.QPolygonF(qpoints)
return qpoly
def draw_picture(self, painter, headcolors):
face_index = 0
for i, node_count in enumerate(self.nodes_per_face):
polygon = QtGui.QPolygonF()
for j in range(node_count):
face_node = self.face_nodes[face_index + j]
polygon.append(
QtCore.QPointF(self.grid_x[face_node],
self.grid_y[face_node]))
face_index += node_count + 1
painter.setBrush(pg.mkBrush(headcolors[i]))
painter.drawPolygon(polygon)
def createPoly(self, n, r, s, xx, yy, act):
polygon = QtGui.QPolygonF()
w = 360 / n # angle per step
for i in range(n): # add the points of polygon
t = w * i + s
y = r * math.sin(math.radians(t))
x = r * math.cos(math.radians(t))
if i == 0:
if act > 0:
polygon.append(QtCore.QPointF(xx + x, 2 + (yy + y)))
elif act < 0:
polygon.append(QtCore.QPointF(xx + x, (yy + y) - 2))
else:
polygon.append(QtCore.QPointF(xx + x, yy + y))
return polygon
def setData(self, *args):
"""
Set the data to be drawn.
Parameters
----------
x, y : np.ndarray, optional, default None
2D array containing the coordinates of the polygons
z : np.ndarray
2D array containing the value which will be maped into the polygons
colors.
If x and y is None, the polygons will be displaced on a grid
otherwise x and y will be used as polygons vertices coordinates as::
(x[i+1, j], y[i+1, j]) (x[i+1, j+1], y[i+1, j+1])
+---------+
| z[i, j] |
+---------+
(x[i, j], y[i, j]) (x[i, j+1], y[i, j+1])
"ASCII from: <https://matplotlib.org/3.2.1/api/_as_gen/
matplotlib.pyplot.pcolormesh.html>".
"""
# Prepare data
cd = self._prepareData(args)
# Has the view bounds changed
shapeChanged = False
if self.qpicture is None:
shapeChanged = True
elif len(args) == 1:
if args[0].shape[0] != self.x[:, 1][-1] or args[0].shape[
1] != self.y[0][-1]:
shapeChanged = True
elif len(args) == 3:
if np.any(self.x != args[0]) or np.any(self.y != args[1]):
shapeChanged = True
self.qpicture = QtGui.QPicture()
p = QtGui.QPainter(self.qpicture)
# We set the pen of all polygons once
if self.edgecolors is None:
p.setPen(fn.mkPen(QtGui.QColor(0, 0, 0, 0)))
else:
p.setPen(fn.mkPen(self.edgecolors))
if self.antialiasing:
p.setRenderHint(QtGui.QPainter.RenderHint.Antialiasing)
## Prepare colormap
# First we get the LookupTable
pos = [i[0] for i in Gradients[self.cmap]['ticks']]
color = [i[1] for i in Gradients[self.cmap]['ticks']]
cmap = ColorMap(pos, color)
lut = cmap.getLookupTable(0.0, 1.0, 256)
# Second we associate each z value, that we normalize, to the lut
vmin, vmax = self.z.min(), self.z.max()
if self.levels is not None:
vmin, vmax = self.levels
vmin = max(vmin, self.z.min())
vmax = min(vmax, self.z.max())
norm = self.z - vmin
norm_max = vmax - vmin
norm = norm / norm_max
norm = (norm * (len(lut) - 1)).astype(int)
norm[norm < 0] = 0
norm[norm > 255] = 255
# Go through all the data and draw the polygons accordingly
for xi in range(self.z.shape[0]):
for yi in range(self.z.shape[1]):
# Set the color of the polygon first
c = lut[norm[xi][yi]]
p.setBrush(fn.mkBrush(QtGui.QColor(c[0], c[1], c[2])))
polygon = QtGui.QPolygonF([
QtCore.QPointF(self.x[xi][yi], self.y[xi][yi]),
QtCore.QPointF(self.x[xi + 1][yi], self.y[xi + 1][yi]),
QtCore.QPointF(self.x[xi + 1][yi + 1],
self.y[xi + 1][yi + 1]),
QtCore.QPointF(self.x[xi][yi + 1], self.y[xi][yi + 1])
])
# DrawConvexPlygon is faster
p.drawConvexPolygon(polygon)
p.end()
self.update()
self.prepareGeometryChange()
if shapeChanged:
self.informViewBoundsChanged()
