当移动物体偏离路径特定幅度时如何发出警告？
How to give a warning when a moving object deviates from a path by a specific margin?
在我的 pygame 代码中，我有一架无人机应该遵循飞行路径。
我用 pygame.draw.lines 在指定点之间画线。现在，我有一个包含 10 个点的飞行路径，每个点之后路径角度都会发生变化（有点像锯齿形）。玩家可以通过按键移动无人机。
我的目标是在无人机偏离路径时打印警告，例如+/-30。绞尽脑汁两天也想不出检测偏差的条件。我只是想不出如何解决这个问题。
我可以随时确定无人机的 x 坐标，但如何确定与路径的偏移量？我附上了一张图片来形象化我的问题。
编辑：
由于我是初学者，我的代码一团糟，但在复制粘贴时，我猜只有第 35-91 行很有趣。 感谢您提前提出任何建议！！
import pygame
import pygame.gfxdraw
import random
import sys
import math
pygame.init()
# Define some colors
black = (0,0,0)
white = (255,255,255)
red = (255,0,0)
red_transp = (255,0,0, 150)
BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
BLUE = (0, 0, 255)
GREEN = (0, 255, 0)
RED = (255, 0, 0)
X = 0
Y = 250
#Display
display_width, display_height = 1200, 700
h_width, h_height = display_width/2, display_height/2
gameDisplay = pygame.display.set_mode((display_width,display_height))
pygame.display.set_caption('Game Display')
#Drone Sprite Image Load Function
droneImg_interim = pygame.image.load('drone.png')
droneImg = pygame.transform.scale(droneImg_interim, [50,50])
drone_width, drone_height = droneImg.get_rect().size
#Create 11 Waypoints with the same coordinates
p1=[X, Y]
p2=[X, Y]
p3=[X, Y]
p4=[X, Y]
p5=[X, Y]
p6=[X, Y]
p7=[X, Y]
p8=[X, Y]
p9=[X, Y]
p10=[X, Y]
p11=[X, Y]
pointlist = [p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11]
x_min=drone_width
x_max=100
#Setting new x-coordinate for each point
for i in pointlist:
i[0] = random.randrange(x_min, x_max)
x_min+=250
x_max+=250
#Setting new y-coordinate for each point
for i in range(len(pointlist)):
if i == 0:
pointlist[i][1] = random.randrange(200, 400)
else:
prev = pointlist[i-1][1]
pointlist[i][1] = random.randrange(200, prev+100)
#Plotting pointlist on gameDisplay and connecting dots
def flightpath(pointlist):
pygame.draw.lines(gameDisplay, (255, 0, 0), False, pointlist, 2)
def margin(x):
for i in range(len(pointlist)-1):
p1_x = pointlist[i][0]
p2_x = pointlist[i+1][0]
p1_y = pointlist[i][1]
p2_y = pointlist[i+1][1]
distance_x = p2_x - p1_x
distance = math.sqrt((p2_x-p1_x)**2+(p2_y-p1_y)**2)
halfwaypoint_x = math.sqrt((p2_x - p1_x)**2)/2 + p1_x
halfwaypoint_y = math.sqrt((p2_y - p1_y)**2)/2 + p1_y
if p2_y < p1_y:
angle_rad = math.acos(distance_x/distance)
elif p2_y > p1_y:
angle_rad = 0 - math.acos(distance_x/distance)
angle_deg = math.degrees(angle_rad)
rect_width = distance
rect_height = 60
"""
This part of the code is meant for displaying the margins (the rectangles) around the flight path on the display.
marginSize = (rect_width, rect_height)
surface = pygame.Surface(marginSize, pygame.SRCALPHA)
surface.fill((255,0,0,25))
rotated_surface = pygame.transform.rotate(surface, angle_deg)
#new_rect = rotated_surface.get_rect(center = surface.get_rect(center = ((pointlist[i][0], pointlist[i][1]))).center)
new_rect = rotated_surface.get_rect(center = surface.get_rect(center = ((halfwaypoint_x, halfwaypoint_y))).center)
#gameDisplay.blit(rotated_surface, new_rect)
"""
#Placing drone on the screen
def drone(x,y):
rect = droneImg.get_rect ()
rect.center=(x, y)
gameDisplay.blit(droneImg,rect)
def displayMSG(value,ttext,posx,posy):
myFont = pygame.font.SysFont("Verdana", 12)
Label = myFont.render(ttext, 1, black)
Value = myFont.render(str(value), 1, black)
gameDisplay.blit(Label, (posx, posy))
gameDisplay.blit(Value, (posx + 100, posy))
#Main Loop Object
def game_loop():
global X, Y, FThrustX, FThrustY, FDragY, Time
FThrustY = 0
gameExit = False
while not gameExit:
#Event Checker (Keyboard, Mouse, etc.)
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
pygame.quit()
sys.exit()
keys = pygame.key.get_pressed() #checking pressed keys
if keys[pygame.K_LEFT]:
X -= 1
if keys[pygame.K_RIGHT]:
X +=1
if keys[pygame.K_DOWN]:
Y += 1
if keys[pygame.K_UP]:
Y -=1
#Display Background Fill
gameDisplay.fill(white)
#Plot flightpath
flightpath(pointlist)
#YS: Determine the position of the mouse
current_pos_x, current_pos_y = pygame.mouse.get_pos()
displayMSG(current_pos_x,'x:',20,665)
displayMSG(current_pos_y,'y:',20,680)
#Plot margin
margin(5)
#Move Drone Object
drone(X,Y)
#Determine the position of the mouse
current_pos_x, current_pos_y = pygame.mouse.get_pos()
#No exceeding of display edge
if X > display_width - drone_width: X = display_width - drone_width
if Y > display_height - drone_height: Y = display_height - drone_height
if X < drone_width: X = drone_width
if Y < drone_height: Y = drone_height
pygame.display.update()
#MAIN
game_loop()
pygame.quit()
sys.exit()
问题的有趣部分当然是在所需路径上找到最近的 点 到实际位置；距离很容易。其中最困难的部分是依次识别路径中最近的 元素 （线段）；投影到它上面也很简单。
如果路径足够简单（特别是，如果它没有分支并且 impossible/disallowed 可以跳过自相交的部分），您可以通过保持 current 变量中的元素，并在其中一个元素的投影比当前元素的投影更近时将其更新为上一个或下一个元素。这是赛车游戏用来确定赛车手瞬时顺序的典型算法。
一种方法是找到无人机中心与直线之间的最小距离。
编写计算点到线段的最小距离的函数。为此，请使用 pygame.math.Vector2 and the Dot product:
def distance_point_linesegment(pt, l1, l2):
LV = pygame.math.Vector2(l2[0] - l1[0], l2[1] - l1[1])
PV = pygame.math.Vector2(pt[0] - l1[0], pt[1]- l1[1])
dotLP = LV.dot(PV)
if dotLP < 0:
return PV.length()
if dotLP > LV.length_squared():
return pygame.math.Vector2(pt[0] - l2[0], pt[1]- l2[1]).length()
NV = pygame.math.Vector2(l1[1] - l2[1], l2[0] - l1[0])
return abs(NV.normalize().dot(PV))
求循环中距离最短的线段：
def minimum_distance(pt, pointlist):
min_dist = -1
for i in range(len(pointlist)-1):
dist = distance_point_linesegment(pt, pointlist[i], pointlist[i+1])
if i == 0 or dist < min_dist:
min_dist = dist
return min_dist
当距离超过特定阈值时创建警报：
def game_loop():
# [...]
while not gameExit:
# [...]
dist_to_path = minimum_distance((X, Y), pointlist)
if dist_to_path > 25:
pygame.draw.circle(gameDisplay, (255, 0, 0), (X, Y), 25, 4)
drone(X,Y
# [...]
另一种可能的解决方案是使用pygame.Rect.clipline并检测线段与无人机周围矩形的碰撞：
def intersect_rect(rect, pointlist):
for i in range(len(pointlist)-1):
if rect.clipline(pointlist[i], pointlist[i+1]):
return True
return False
def game_loop():
# [...]
while not gameExit:
# [...]
if not intersect_rect(droneImg.get_rect(center = (X, Y)), pointlist):
pygame.draw.circle(gameDisplay, (255, 0, 0), (X, Y), 25, 4)
drone(X,Y
# [...]
在我的 pygame 代码中，我有一架无人机应该遵循飞行路径。
我用 pygame.draw.lines 在指定点之间画线。现在，我有一个包含 10 个点的飞行路径，每个点之后路径角度都会发生变化（有点像锯齿形）。玩家可以通过按键移动无人机。
我的目标是在无人机偏离路径时打印警告，例如+/-30。绞尽脑汁两天也想不出检测偏差的条件。我只是想不出如何解决这个问题。
我可以随时确定无人机的 x 坐标，但如何确定与路径的偏移量？我附上了一张图片来形象化我的问题。
编辑： 由于我是初学者，我的代码一团糟，但在复制粘贴时，我猜只有第 35-91 行很有趣。 感谢您提前提出任何建议！！
import pygame
import pygame.gfxdraw
import random
import sys
import math
pygame.init()
# Define some colors
black = (0,0,0)
white = (255,255,255)
red = (255,0,0)
red_transp = (255,0,0, 150)
BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
BLUE = (0, 0, 255)
GREEN = (0, 255, 0)
RED = (255, 0, 0)
X = 0
Y = 250
#Display
display_width, display_height = 1200, 700
h_width, h_height = display_width/2, display_height/2
gameDisplay = pygame.display.set_mode((display_width,display_height))
pygame.display.set_caption('Game Display')
#Drone Sprite Image Load Function
droneImg_interim = pygame.image.load('drone.png')
droneImg = pygame.transform.scale(droneImg_interim, [50,50])
drone_width, drone_height = droneImg.get_rect().size
#Create 11 Waypoints with the same coordinates
p1=[X, Y]
p2=[X, Y]
p3=[X, Y]
p4=[X, Y]
p5=[X, Y]
p6=[X, Y]
p7=[X, Y]
p8=[X, Y]
p9=[X, Y]
p10=[X, Y]
p11=[X, Y]
pointlist = [p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11]
x_min=drone_width
x_max=100
#Setting new x-coordinate for each point
for i in pointlist:
i[0] = random.randrange(x_min, x_max)
x_min+=250
x_max+=250
#Setting new y-coordinate for each point
for i in range(len(pointlist)):
if i == 0:
pointlist[i][1] = random.randrange(200, 400)
else:
prev = pointlist[i-1][1]
pointlist[i][1] = random.randrange(200, prev+100)
#Plotting pointlist on gameDisplay and connecting dots
def flightpath(pointlist):
pygame.draw.lines(gameDisplay, (255, 0, 0), False, pointlist, 2)
def margin(x):
for i in range(len(pointlist)-1):
p1_x = pointlist[i][0]
p2_x = pointlist[i+1][0]
p1_y = pointlist[i][1]
p2_y = pointlist[i+1][1]
distance_x = p2_x - p1_x
distance = math.sqrt((p2_x-p1_x)**2+(p2_y-p1_y)**2)
halfwaypoint_x = math.sqrt((p2_x - p1_x)**2)/2 + p1_x
halfwaypoint_y = math.sqrt((p2_y - p1_y)**2)/2 + p1_y
if p2_y < p1_y:
angle_rad = math.acos(distance_x/distance)
elif p2_y > p1_y:
angle_rad = 0 - math.acos(distance_x/distance)
angle_deg = math.degrees(angle_rad)
rect_width = distance
rect_height = 60
"""
This part of the code is meant for displaying the margins (the rectangles) around the flight path on the display.
marginSize = (rect_width, rect_height)
surface = pygame.Surface(marginSize, pygame.SRCALPHA)
surface.fill((255,0,0,25))
rotated_surface = pygame.transform.rotate(surface, angle_deg)
#new_rect = rotated_surface.get_rect(center = surface.get_rect(center = ((pointlist[i][0], pointlist[i][1]))).center)
new_rect = rotated_surface.get_rect(center = surface.get_rect(center = ((halfwaypoint_x, halfwaypoint_y))).center)
#gameDisplay.blit(rotated_surface, new_rect)
"""
#Placing drone on the screen
def drone(x,y):
rect = droneImg.get_rect ()
rect.center=(x, y)
gameDisplay.blit(droneImg,rect)
def displayMSG(value,ttext,posx,posy):
myFont = pygame.font.SysFont("Verdana", 12)
Label = myFont.render(ttext, 1, black)
Value = myFont.render(str(value), 1, black)
gameDisplay.blit(Label, (posx, posy))
gameDisplay.blit(Value, (posx + 100, posy))
#Main Loop Object
def game_loop():
global X, Y, FThrustX, FThrustY, FDragY, Time
FThrustY = 0
gameExit = False
while not gameExit:
#Event Checker (Keyboard, Mouse, etc.)
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
pygame.quit()
sys.exit()
keys = pygame.key.get_pressed() #checking pressed keys
if keys[pygame.K_LEFT]:
X -= 1
if keys[pygame.K_RIGHT]:
X +=1
if keys[pygame.K_DOWN]:
Y += 1
if keys[pygame.K_UP]:
Y -=1
#Display Background Fill
gameDisplay.fill(white)
#Plot flightpath
flightpath(pointlist)
#YS: Determine the position of the mouse
current_pos_x, current_pos_y = pygame.mouse.get_pos()
displayMSG(current_pos_x,'x:',20,665)
displayMSG(current_pos_y,'y:',20,680)
#Plot margin
margin(5)
#Move Drone Object
drone(X,Y)
#Determine the position of the mouse
current_pos_x, current_pos_y = pygame.mouse.get_pos()
#No exceeding of display edge
if X > display_width - drone_width: X = display_width - drone_width
if Y > display_height - drone_height: Y = display_height - drone_height
if X < drone_width: X = drone_width
if Y < drone_height: Y = drone_height
pygame.display.update()
#MAIN
game_loop()
pygame.quit()
sys.exit()
问题的有趣部分当然是在所需路径上找到最近的 点 到实际位置；距离很容易。其中最困难的部分是依次识别路径中最近的 元素 （线段）；投影到它上面也很简单。
如果路径足够简单（特别是，如果它没有分支并且 impossible/disallowed 可以跳过自相交的部分），您可以通过保持 current 变量中的元素，并在其中一个元素的投影比当前元素的投影更近时将其更新为上一个或下一个元素。这是赛车游戏用来确定赛车手瞬时顺序的典型算法。
一种方法是找到无人机中心与直线之间的最小距离。
编写计算点到线段的最小距离的函数。为此，请使用 pygame.math.Vector2 and the Dot product:
def distance_point_linesegment(pt, l1, l2):
LV = pygame.math.Vector2(l2[0] - l1[0], l2[1] - l1[1])
PV = pygame.math.Vector2(pt[0] - l1[0], pt[1]- l1[1])
dotLP = LV.dot(PV)
if dotLP < 0:
return PV.length()
if dotLP > LV.length_squared():
return pygame.math.Vector2(pt[0] - l2[0], pt[1]- l2[1]).length()
NV = pygame.math.Vector2(l1[1] - l2[1], l2[0] - l1[0])
return abs(NV.normalize().dot(PV))
求循环中距离最短的线段：
def minimum_distance(pt, pointlist):
min_dist = -1
for i in range(len(pointlist)-1):
dist = distance_point_linesegment(pt, pointlist[i], pointlist[i+1])
if i == 0 or dist < min_dist:
min_dist = dist
return min_dist
当距离超过特定阈值时创建警报：
def game_loop():
# [...]
while not gameExit:
# [...]
dist_to_path = minimum_distance((X, Y), pointlist)
if dist_to_path > 25:
pygame.draw.circle(gameDisplay, (255, 0, 0), (X, Y), 25, 4)
drone(X,Y
# [...]
另一种可能的解决方案是使用pygame.Rect.clipline并检测线段与无人机周围矩形的碰撞：
def intersect_rect(rect, pointlist):
for i in range(len(pointlist)-1):
if rect.clipline(pointlist[i], pointlist[i+1]):
return True
return False
def game_loop():
# [...]
while not gameExit:
# [...]
if not intersect_rect(droneImg.get_rect(center = (X, Y)), pointlist):
pygame.draw.circle(gameDisplay, (255, 0, 0), (X, Y), 25, 4)
drone(X,Y
# [...]