def draw(self, painter, color):
coord = self.rotate()
x0 = round(self.robot.posX + coord[0] * self.relDist)
y0 = round(self.robot.posY + coord[1] * self.relDist)
dx = coord[0] * RangeSensor.MAX_DISTANCE
dy = coord[1] * RangeSensor.MAX_DISTANCE
x1 = round(x0 + dx)
y1 = round(y0 + dy)
#color = QtGui.QColor(0xFF0000)
painter.fillRect(x0, y0, 1, 1, color)
dx = abs(x1 - x0)
dy = abs(y1 - y0)
if x0 < x1:
sx = 1
else:
sx = -1
if y0 < y1:
sy = 1
else:
sy = -1
err = dx - dy
while True:
#print('range_sensor.RangeSensor.draw x0: ' + str(x0) + ' y0: ' + str(y0) + ' x1: ' + str(x1) + ' y1: ' + str(y1))
if x0 < 0 or y0 < 0 or x0 >= ImageMap.width() or y0 >= ImageMap.height():
return
if ImageMap.pixel(x0, y0) != 0xFFFFFFFF:
return
painter.fillRect(x0, y0, 1, 1, color)
if x0 == x1 and y0 == y1:
return
e2 = 2 * err
if e2 > -dy:
err = err - dy
x0 = x0 + sx
if x0 == x1 and y0 == y1:
painter.fillRect(x0, y0, 1, 1, color)
self.distance = RangeSensor.MAX_DISTANCE
return
if e2 < dx:
err = err + dx
y0 = y0 + sy
def getDistance(self):
coord = self.rotate()
x0 = round(self.robot.posX + coord[0] * self.relDist)
y0 = round(self.robot.posY + coord[1] * self.relDist)
sX0 = x0
sY0 = y0
dx = coord[0] * RangeSensor.MAX_DISTANCE
dy = coord[1] * RangeSensor.MAX_DISTANCE
x1 = round(x0 + dx)
y1 = round(y0 + dy)
#color = QtGui.QColor(0xFF0000)
dx = abs(x1 - x0)
dy = abs(y1 - y0)
if x0 < x1:
sx = 1
else:
sx = -1
if y0 < y1:
sy = 1
else:
sy = -1
err = dx - dy
px = x0
py = y0
while True:
#TODO cleanup
#print('range_sensor.RangeSensor.draw x0: ' + str(x0) + ' y0: ' + str(y0) + ' x1: ' + str(x1) + ' y1: ' + str(y1))
#print('%08X' % (ImageMap.image.pixel(x0, y0),))
#print('%d - %d' % (x0, y0))
if x0 < 0 or y0 < 0 or x0 >= ImageMap.width() or y0 >= ImageMap.height():
d = math.sqrt(math.pow(sX0 - px, 2.0) + math.pow(sY0 - py, 2.0))
if d < 0 or d > 1000:
print('A: %d - %d # %f' % (px, py, d))
return d
if ImageMap.pixel(x0, y0) != 0xFFFFFFFF:
d = math.sqrt(math.pow(sX0 - x0, 2.0) + math.pow(sY0 - y0, 2.0))
if d < 0 or d > 1000:
print('B: %d - %d # %f' % (px, py, d))
return d
if x0 == x1 and y0 == y1:
return RangeSensor.MAX_DISTANCE
e2 = 2 * err
if e2 > -dy:
err = err - dy
px = x0
x0 = x0 + sx
if x0 == x1 and y0 == y1:
return RangeSensor.MAX_DISTANCE
if e2 < dx:
err = err + dx
py = y0
y0 = y0 + sy
