python - 当移动物体偏离路径特定幅度时如何发出警告？

Question

在我的 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()

Answer 1

一种方法是找到无人机中心与直线之间的最小距离。

编写计算点到线段的最小距离的函数。为此，请使用 pygame.math.Vector2和 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

        # [...]