How to give a warning when a moving object deviates from a path by a specific margin?

Question

In my pygame-code, I have a drone that is supposed to follow a flight path.

I used pygame.draw.lines to draw lines between specified points. Now, I have a flight path with 10 points where after each point the path angle changes (a bit like a zigzag). The player can move the drone by pressing the keys.

My goal is to print a warning once the drone deviates from the path, e.g. by +/-30. I have been racking my brain for two days but can't come up with a condition to detect a deviation. I just can't figure out how to approach this.

I can determine the drone's x-coordinate at any time but how do I determine the offset from the path? I have attached an image to visualize my problem.

Edit: As I am a beginner my code is a mess but when copy-pasting it, I guess only the lines 35-91 are interesting. Thank you for any kind of advice in advance!!

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()

Drone_Flight_Path_Deviation

Can you add your code please? – taylor.2317 Dec 26 '21 at 23:27 — taylor.2317, Dec 26 '21 at 23:27
@taylor.2317 I just added the code. Thank you for the hint! – Yaso Dec 27 '21 at 00:19 — Yaso, Dec 27 '21 at 00:19

Rabbid76 · Accepted Answer · 2021-12-27T18:44:15.010

One approach is to find the minimum distance between the center of the drone and the line.

Write the function that calculates the minimum distance from a point to a line segment. To do this, use 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))

Find the line segment with the shortest distance in a loop:

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

Create an alert when the distance exceeds a certain threshold:

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

        # [...]

Another possible solution is to use pygame.Rect.clipline and detect the collision of the line segments and the rectangle surrounding the drone:

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

        # [...]

First of all, you are a LIFESAVER and I appreciate your time! Thank you. In order to indicate to the player whether to go up or down to get back to the trajectory, I adjusted the `return abs(NV.normalize().dot(PV))`-line to `return NV.normalize().dot(PV)`. Then I added a message to be displayed in the `if-statement` of the game_loop. However, sometimes it says to go up when, in fact, one has to go down. Any idea what I could be missing? — Yaso, Dec 28 '21 at 20:52
What do you mean by close "before" or "after"? What do I close? — Yaso, Dec 28 '21 at 21:16
@Yaso This probably happens when the drone is close before the line (if dotLP < 0:) or close after the line segment (if dotLP > LV.length_squared()) the line. In this case the function returns the distance to the start or end point of the line segment. — Rabbid76, Dec 28 '21 at 21:19

score 0 · Answer 2 · answered Dec 27 '21 at 01:52

The interesting part of the question is of course finding the nearest point on the desired path to the actual position; distance is easy. The hard part of that is in turn identifying the nearest element (line segment) of the path; projecting onto it is also straightforward.

If the path is simple enough (in particular, if it doesn’t branch and it’s impossible/disallowed to skip sections at a self-intersection), you can finesse that part by just maintaining that current element in a variable and updating it to the previous or next element when the projection onto one of them is closer than the projection onto the current one. This is a typical algorithm used by racing games to determine the instantaneous order of racers.

How to give a warning when a moving object deviates from a path by a specific margin?

2 Answers2