The Nature of Code閱讀心得與Python實作：5.4 Path Following

ysf

更新於 2024/12/30發佈於 2024/12/30閱讀時間約 48 分鐘

不同於用來找出兩點間最短距離演算法的路徑搜尋(path finding)，路徑循行(path following)，指的是依循已經設定好的路徑來移動的轉向行為。這一節就要來研究Reynolds所設計的路徑循行轉向行為。

在進入主題之前，要先來看一下需要用到的數學工具：向量內積(inner product; dot product)。

The Dot Product

假設有兩個向量

A = (a_x, a_y)
B = (b_x, b_y)

則其內積，記為A⋅B，之計算公式為

A⋅B = a_xb_x+ a_yb_y

向量內積還有另一個計算公式：

A⋅B = ‖A‖ ‖B‖ cosθ

這裡的θ是A和B間的夾角。所以，利用向量內積，我們可以很容易就算出兩個向量間的夾角。由計算內積的兩條公式可以得到

cosθ = (a_xb_x+ a_yb_y) / ‖A‖ ‖B‖

因此，A和B之間的夾角θ為

θ = cos^-1(a_xb_x+ a_yb_y) / ‖A‖ ‖B‖

從上述的公式很容易就可以得到向量內積的兩個比較特別的性質：

如果兩向量正交(orthogonal)，也就是互相垂直，則其內積為0；反之亦然。
兩單位向量的內積，會等於其夾角的餘弦值。

寫程式計算向量內積時，可以直接利用公式來計算，也可以利用pygame中的dot()方法來計算。假設A和B都是pygame.Vector2物件，則其內積可以用下列任一方式來計算：

A.x*B.x + A.y*B.y    
A.dot(B)
B.dot(A)

至於A和B間的夾角，使用pygame中的angle_to()方法來計算，會是比較省時省力的方式；使用方式為：

A.angle_to(B)

或

B.angle_to(A)

在使用angle_to()時要注意的是，算出來的角度單位是「度」，而且會以不越過負x軸的方式來計算角度。詳細的說明見pygame的使用文件。

Exercise 5.9

# python version 3.10.9
import math
import sys

import pygame  # version 2.3.0


def draw_vector(pt_start, pt_end):
    vec = pygame.Vector2(pt_end) - pygame.Vector2(pt_start)
    pygame.draw.line(screen, (0, 0, 0), pt_start, pt_end, 2)
    
    s1 = -vec.rotate(30)
    if s1.length() > 0:
        s1.scale_to_length(s1.length()/5)
    
    s2 = -vec.rotate(-30)
    if s2.length() > 0:    
        s2.scale_to_length(s2.length()/5)
        
    pygame.draw.line(screen, (0, 0, 0), pt_end, pygame.Vector2(pt_end)+s1, 2)
    pygame.draw.line(screen, (0, 0, 0), pt_end, pygame.Vector2(pt_end)+s2, 2)

        
pygame.init()

pygame.display.set_caption("Exercise 5.9")

BLACK = (0, 0, 0)
WHITE = (255, 255, 255)

screen_size = WIDTH, HEIGHT = 640, 360
screen = pygame.display.set_mode(screen_size)

FPS = 60
frame_rate = pygame.time.Clock()

pt_start = (WIDTH/2, HEIGHT/2)
vector_length = 150
vec1 = pygame.Vector2(vector_length, 0)

font = pygame.font.SysFont("consolas", 25)

while True:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            pygame.quit()
            sys.exit()
    
    screen.fill(WHITE)    

    draw_vector(pt_start, (pt_start[0]+150, pt_start[1]))
    
    vec2 = pygame.mouse.get_pos() - pygame.Vector2(pt_start)
    vec2.scale_to_length(vector_length)
    
    pt_end = tuple(pygame.Vector2(pt_start)+vec2)
    draw_vector(pt_start, pt_end)
    
    theta_rad = math.acos(vec1.dot(vec2)/(vec1.length()*vec2.length()))
    if vec2.y >= vec1.y:
        theta_rad = -theta_rad
        
    theta_deg = math.degrees(theta_rad)
    
    text_deg = font.render(f"{theta_deg:.0f} degrees", True, BLACK)
    text_rad = font.render(f"{theta_rad:.2f} radians", True, BLACK)
    screen.blit(text_deg, (15, 270))
    screen.blit(text_rad, (15, 300))
    
    pygame.display.update()
    frame_rate.tick(FPS)

這裡是使用python的math.acos()來計算角度。要注意的是，acos()算出的角度，單位是「弳度」，而且範圍是在0～π。如果是使用pygame的angle_to()來寫，可以寫成

theta_deg = vec2.angle_to(vec1)

Simple Path Following

既然叫路徑循行，那路徑顯然扮演著非常重要的角色。不過，在虛擬世界中，「路徑」指的到底是什麼呢？有許多技術可以在虛擬世界中實作出路徑來，其中一個做法非常簡單，就是把一串連續的點給連接起來，這樣就可以構成一條路徑了；而這也是接下來我們會採用的方法。

根據上述實作路徑的方法，最簡單的路徑，就是上圖中連接兩個點的直線。不過在實際使用時，我們會把路徑看成是具有寬度的直線，並把這個寬度叫做「半徑(radius)」；以道路來比擬的話，路徑就是道路，而半徑就是路寬。所以，當vehicle進行路徑循行時，路徑的半徑就是他可以偏離路徑中心線的最大距離，當偏離的距離超出這個範圍時，就必須產生轉向力來回到路徑內。

在下面這個例子中，我們設計了一個Path類別來描述一段路徑。

Example 5.5: Creating a Path Object

class Path:
    def __init__(self, start, end, radius):
        # 取得顯示畫面
        self.screen = pygame.display.get_surface()
            
        self.start = pygame.Vector2(start)
        self.end = pygame.Vector2(end)
        self.radius = radius
           
    def show(self):
        pygame.draw.line(self.screen, (200, 200, 200), self.start, self.end, 2*self.radius)
        pygame.draw.line(self.screen, (0, 0, 0), self.start, self.end, 2)
            

# python version 3.10.9
import sys

import pygame  # version 2.3.0


pygame.init()

pygame.display.set_caption("Example 5.5: Creating a Path Object")

WHITE = (255, 255, 255)

screen_size = WIDTH, HEIGHT = 640, 360
screen = pygame.display.set_mode(screen_size)

FPS = 60
frame_rate = pygame.time.Clock()

start, end = (0, HEIGHT/3), (WIDTH, 2*HEIGHT/3)
radius = 50
path = Path(start, end, radius)

while True:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            pygame.quit()
            sys.exit()

    screen.fill(WHITE)    
    
    path.show()
    
    pygame.display.update()
    frame_rate.tick(FPS)

這裡要注意的是，pygame在畫線時，線的寬度的定義和我們所要的路徑半徑的定義方式是不同的。寫個小程式畫個圖就很清楚了。

pygame.draw.line(screen, BLACK, (100, 100), (150, 100), 50)
pygame.draw.line(screen, RED, (150, 100), (350, 250), 50)
pygame.draw.line(screen, BLACK, (350, 250), (400, 250), 50)

這三條線的寬度都是50，結果畫出來的圖長這樣：

很顯然的，pygame畫線時，線的寬度是水平或垂直的量出來的，這和我們量路徑半徑時的做法是不同的。所以，使用pygame.draw.line()來畫路徑時，會有誤差存在；這點應該要謹記在心，以免錯以為是程式有問題。

前面提到，當vehicle進行路徑循行時，如果偏離路徑中心線的距離超過路徑半徑，就必須產生轉向力來回到路徑內；這種事情已經發生了才處理的應變方式，實在是不怎麼樣。那該怎麼做比較好呢？其實就跟開車一樣，當發現依照這個方向繼續開下去就會開到路外邊去時，就應該要預先調整方向來避免。所以，vehicle在路徑循行時，應該要去「預測」未來是不是會偏離路徑中心線太遠，如果是的話，就要馬上產生轉向力來避免事情發生。接下來就來看看，vehicle要怎麼去「預測」未來離路徑的中心線會有多遠。

vehicle要想知道未來自己離路徑的中心線會有多遠，就必須知道未來自己會在哪個位置上。要預測移動中的物體未來會在哪個位置上，最簡單的做法，就是假設物體移動的速度不變，那沿著物體移動的方向在物體前方一段距離的位置，就是未來某個時間點物體所在的位置。假設vehicle現在的位置及速度分別為position及velocity，而他在未來某個時間點的位置是future；這裡的position、velocity、position都是pygame.Vector2物件。根據這樣子的設定，計算future的程式可以這樣寫：

displacement = velocity.copy()
displacement.scale_to_length(25)
future = position + displacement

預測出vehicle未來的位置future之後，再來就是要算出future到路徑中心線的距離。這要怎麼做呢？

稍微檢視一下就可以知道，計算future到路徑中心線的距離，其實就等同於計算一個點到一條直線的距離。如圖，已知P₀和P₁是直線L上的兩個點，而P₂是直線L外的一點，我們要做的，就是利用P₀、P₁、P₂這三個點來算出P₂到L的距離。不過，這P₂到L的距離指的究竟是什麼呢？

如果一個向量垂直於L時，那我們會把這個向量叫做L的法向量(normal vector)；英文有時候會省略vector這個字，而簡稱normal。點P₂到L的距離，就定義成由點P₂到L的這個法向量的長度。假設點P₂到L這個法向量和L的交點是P_normal，那點P₂到L的距離，就是P₂到P_normal這個向量的長度；從下圖中也可以看出來，這也就是點P₂到點P_normal這個線段的長度。

那要怎麼從已知的P₀、P₁、P₂這三個點來算出P₂到P_normal的距離呢？這就要用到先前介紹過的「向量內積」這個數學工具了；畫張圖來看會比較清楚：

在圖中有三個向量，分別是：由P₀到P₂的向量A；由P₀到P₁的向量B；由P₂到P_normal的N；這三個向量當中，已知的是A和B。所以，利用先前介紹向量內積時提到的夾角計算公式，我們就可以算出A和B之間的夾角θ。因為P₀、P₂、P_normal這三個點構成一個直角三角形，所以P₂到P_normal之間的距離，也就是‖N‖，可以這樣算出來：

‖N‖ = ‖A‖ sinθ

上述的做法雖然可以達到我們的目的，不過需要去計算cos的反函數，實在是有點麻煩。接下來，我們用另外一種比較簡潔的方式來處理；這種方式還有一個附帶的好處，那就是可以同時把點P_normal給算出來。

由向量內積的公式可以得到

‖A‖ cosθ = A⋅B/‖B‖ = A⋅u_B

這裡的u_B是B的單位向量。‖A‖ cosθ有個特別的名稱，叫做向量A在向量B上的純量投影(scalar projection)。所以，利用這個式子就可以在不需要算出夾角θ的情況下，算出A在B上的純量投影；而從前面的圖中可以看出來，這個純量投影，就是P₀到P_normal的距離。既然知道P₀到P_normal之間的距離是A⋅u_B，那P_normal的位置向量P_normal就可以用下列方式算出來：

P_normal = P₀+ (A⋅u_B)u_B

這裡的P₀是P₀的位置向量。算出P_normal之後，要算出P₂到P_normal之間的距離，就只是小菜一碟的功夫了。

現在回到計算future到路徑中心線的距離這個問題上來。還是畫圖來看會比較清楚：

在圖中，P_start和P_end為路徑的起點和終點，而P_future是預測的vehicle位置；這些都是已知的點。另外，P_normal是從P_future到路徑中心線的法向量與路徑的交點。

把P_start到P_future的向量叫做A，而P_start到P_end的向量叫做B，利用前面推導出的公式可以得到

P_normal = P_start+ (A⋅u_B)u_B

有了P_normal之後，接下來就可以計算future到路徑中心線的距離了。

在開始計算future到路徑中心線的距離前，有件事情需要先確認一下，不然可能會得到錯誤的結果：點P_normal是位於路徑上嗎？

先前推導公式時，我們只說P_normal會落在一條通過已知兩點的直線上，並沒有說它會落在這兩個點之間。所以，現在算出來的P_normal，是有可能會落在由P_start到P_end這個線段所構成的路徑之外的。這時候，future到路徑中心線的距離，就不會是future到P_normal的距離了。碰到這種情況，比較直接的做法，是把future到路徑的距離，定義成future到P_start或P_end的距離；但任何其他有助於解決問題的定義方式，也是可以的。

接下來，就來把這一大串的東西寫成程式。在這裡，我們會先假設P_normal一定會落在路徑上；落在路徑外的那種情況，等後面碰到的時候再來處理。

vec_A = future - path.start
vec_B = path.end - path.start

uB = vec_B.normalize()
normal_point = path.start + vec_A.dot(uB)*uB

# future到路徑中心線的距離
distance = (normal_point - future).length()

在pygame中，有個project()方法可以用來計算vec_A在vec_B上的向量投影，也就是vec_A.dot(uB)*uB。所以，可以刪除那行計算uB的程式，並把計算normal_point那行程式改成

normal_point = path.start + vec_A.project(B)

這樣子的寫法，看起來會清爽一點。

算出future到路徑中心線的距離之後，就可以根據這個距離是不是大於路徑的半徑來判斷vehicle是不是有可能會偏離路徑了。根據Renoylds的設計，當vehicle未來有可能偏離路徑時，應該要產生朝向路徑方向的轉向力來避免未來真的出現這種情況。那這個朝向路徑方向的轉向力要怎麼設計呢？這其實挺簡單的，只要在P_normal這個位置前方的路徑上放個目標物，並啟用尋標功能來搜尋這個目標物就可以了；既然目標物是位於路徑上，那vehicle尋標所產生的轉向力，自然就是朝向路徑的方向了。

假設我們讓目標物位於P_normal前方25個像素的路徑上，因為向量B的方向就是路徑的方向，所以這部分的程式可以這樣寫：

displacement = vec_B.copy()
displacement.scale_to_length(25)
target = normal_point + displacement
    
if (normal_point-future).length() > path.radius:
    self.seek(target)

把所有這些一大堆東西整合在一起放進Vehicle類別中，就可以讓vehicle具有路徑循行的能力了。

Example 5.6: Simple Path Following

在這個例子中，我們把路徑循行的功能放進Vehicle類別中，並加入可以顯示future、normal_point，以及目標物的功能；按滑鼠按鍵，就可以切換是否顯示這些細節。新加入用來處理路徑循行的follow_path()方法如下：

def follow_path(self, path, show_details):
    displacement = self.velocity.copy()
    displacement.scale_to_length(25)
    future = self.position + displacement
        
    vec_A = future - path.start        
    vec_B = path.end - path.start
    normal_point = path.start + vec_A.project(vec_B)

    displacement = vec_B.copy()
    displacement.scale_to_length(25)
    target = normal_point + displacement
        
    is_strayed = (normal_point-future).length() > path.radius
    if is_strayed:
        self.seek(target)     
        
    if show_details:
        pygame.draw.line(self.screen, (0, 0, 0), self.position, future)
        pygame.draw.circle(self.screen, (0, 0, 0), future, 3)
          
        pygame.draw.line(self.screen, (0, 0, 0), future, normal_point)
        pygame.draw.circle(self.screen, (0, 0, 0), normal_point, 3)
            
        color = (255, 0, 0) if is_strayed else (0, 0, 0)
        pygame.draw.circle(self.screen, color, target, 5)

另外還新增了當vehicle跑出畫面時的處理方法：

def borders(self, path):
    if self.position.x > path.end.x:
        self.position.x = path.start.x
        self.position.y = path.start.y + (self.position.y - path.end.y)

主程式如下：

# python version 3.10.9
import math
import sys

import pygame  # version 2.3.0


pygame.init()

pygame.display.set_caption("Example 5.6: Simple Path Following")

WHITE = (255, 255, 255)

screen_size = WIDTH, HEIGHT = 640, 360
screen = pygame.display.set_mode(screen_size)

FPS = 60
frame_rate = pygame.time.Clock()

start, end = (0, HEIGHT/3), (WIDTH, 2*HEIGHT/3)
radius = 20
path = Path(start, end, radius)

vehicle1 = Vehicle(0, 50)
vehicle1.velocity = pygame.Vector2(2, 0)
vehicle1.max_speed = 2
vehicle1.max_force = 0.02

vehicle2 = Vehicle(0, 180)
vehicle2.velocity = pygame.Vector2(2, 0)
vehicle2.max_speed = 3
vehicle2.max_force = 0.05

show_details = True

while True:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            pygame.quit()
            sys.exit()
        elif event.type == pygame.MOUSEBUTTONDOWN:
            show_details = not show_details

    screen.fill(WHITE)    
    
    path.show()
    
    vehicle1.borders(path)
    vehicle1.follow_path(path, show_details)
    vehicle1.update()
    vehicle1.show()
    
    vehicle2.borders(path)
    vehicle2.follow_path(path, show_details)
    vehicle2.update()
    vehicle2.show()    
    
    pygame.display.update()
    frame_rate.tick(FPS)

Path Following with Multiple Segments

到目前為止，我們所設計的路徑循行功能，就只能在單一的一截直線路徑上使用。那如果路徑不是直線而是曲線，又該怎麼辦呢？其實，把許多短短的線段給串在一起，就可以形成一條近似的曲線了；這就像鐵鍊一樣，雖然鐵鍊是由一個一個直挺挺硬梆梆的金屬環節串接而成，但一整條鐵鍊卻可以彎彎曲曲繞成各式各樣的曲線。接下來，就來看看要怎麼讓vehicle能在由許多短路徑串起來的長路徑上循行。

要讓vehicle在由許多短路徑串起來的長路徑上循行，第一件要做的事，當然就是要先升級Path類別，讓它能夠描述由一截一截短路徑串接而成的長路徑。因為短路徑在串接時，兩條連續的短路徑會首尾相連，所以只需要知道這些連接點，以及整條完整路徑的起點和終點就可以了。利用完整路徑的這個特點，Path類別可以這樣修改：

class Path:
    def __init__(self, radius):
        # 取得顯示畫面
        self.screen = pygame.display.get_surface()
            
        self.radius = radius
            
        self.points = []
    
    def add_point(self, x, y):
        self.points.append(pygame.Vector2(x, y))
            
    def show(self):
        pygame.draw.lines(self.screen, (200, 200, 200), False, self.points, 2*self.radius)
        pygame.draw.lines(self.screen, (0, 0, 0), False, self.points, 2)

利用這個修改過後的Path類別，就可以完整地描述一條由短路徑串接而成的長路徑了。

Example 5.7: Path Made of Multiple Line Segments

# python version 3.10.9
import sys

import pygame  # version 2.3.0


pygame.init()

pygame.display.set_caption("Example 5.7: Path Made of Multiple Line Segments")

WHITE = (255, 255, 255)

screen_size = WIDTH, HEIGHT = 640, 360
screen = pygame.display.set_mode(screen_size)

FPS = 60
frame_rate = pygame.time.Clock()

radius = 50
path = Path(radius)
path.add_point(0, 80)
path.add_point(150, 150)
path.add_point(450, 70)
path.add_point(640, 300)

while True:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            pygame.quit()
            sys.exit()

    screen.fill(WHITE)    
    
    path.show()
    
    pygame.display.update()
    frame_rate.tick(FPS)

在這個例子中，完整的路徑是由三條短路徑所構成。在圖中可以看到，第二、三條短路徑的接點上，有個明顯的缺口。之所以會有這樣子的情況發生，是因為pygame在畫線的時候，會根據直線不同的走向而把端點垂直或水平切齊；那個缺口形成的原因，就是因為第二條短路徑的端點是垂直切齊，而第三條短路徑的端點是水平切齊的緣故。不過，這種接點不良的情形，就只會影響畫出來的路徑美觀與否，並不會影響其他計算的結果；如果真的很在乎，那就要另外寫程式來處理了。

升級好Path類別之後，接下來就可以來升級Vehicle類別的follow_up()方法，好讓vehicle可以在由多條短路徑所組成的長路徑上循行。

要讓vehicle可以在由多條短路徑所組成的長路徑上循行，最關鍵的地方在於：目標物要放在哪裡？根據Reynolds提出的方法，目標物應該放在距離未來vehicle所在位置，也就是future，最近的那條短路徑上。換句話說，我們必須算出future到各條短路徑的距離，然後找出距離最短的那條路徑，並依據先前的方法，把目標物放在法線和短路徑中心線交點前方一段距離的位置。依照這樣子的做法，Vehicle類別的follow_up()方法，可以修改成這樣：

def follow_path(self, path, show_details):
    displacement = self.velocity.copy()
    displacement.scale_to_length(50)
    future = self.position + displacement
    
    # 找出距離future最近的短路徑
    distance = float('inf')    
    for i in range(len(path.points)-1):
        # 路徑起點與終點
        pt_start = path.points[i]
        pt_end = path.points[i+1]
    
        vec_A = future - pt_start       
        vec_B = pt_end - pt_start
        normal_pt = pt_start + vec_A.project(vec_B)
        
        # 交點不在路徑上，把路徑終點當作交點
        if not (pt_start.x <= normal_pt.x <= pt_end.x):    
            normal_pt = pt_end.copy()

        d = future.distance_to(normal_pt)
        if d < distance:
            distance = d
            normal_point = normal_pt.copy()
            displacement = vec_B.copy()
    
    # 把目標物放在交點前方25像素處
    displacement.scale_to_length(25)
    target = normal_point + displacement

    is_strayed = distance > path.radius
    if is_strayed:
        self.seek(target)     
    
    if show_details:
        pygame.draw.line(self.screen, (0, 0, 0), self.position, future)
        pygame.draw.circle(self.screen, (0, 0, 0), future, 3)
        
        pygame.draw.line(self.screen, (0, 0, 0), future, normal_point)
        pygame.draw.circle(self.screen, (0, 0, 0), normal_point, 3)
        
        color = (255, 0, 0) if is_strayed else (0, 0, 0)
        pygame.draw.circle(self.screen, color, target, 5)

Exercise 5.10

線段的起點是a、終點是b，判斷normal_point是否位於線段上，程式可以這樣寫：

# normal_point到a的距離
da = normal_point.distance_to(a)
# normal_point到b的距離
db = normal_point.distance_to(b)
# a到b的距離
dab = b.distance_to(a)
if (da + db - dab) > 0.001:
    # normal_point沒有位於線段上

這裡要注意的是，因為da、db、dab都是浮點數，所以在比較大小時，需考慮由精確度所引起的誤差；這也是為什麼當normal_point到兩端點的距離和要比線段長度大上0.001時，才會判定normal_point沒有位於線段上的原因。

修改好Path類別以及follow_up()方法之後，還要再修改一下Vehicle類別中的borders()方法，以因應完整路徑是由多條短路徑所組成的狀況：

def borders(self, path):
    if self.position.x > path.points[-1].x:
        self.position.x = path.points[0].x
        self.position.y = path.points[0].y + (self.position.y - path.points[-1].y)

這裡頭，path.points[0]是完整路徑的起點；而path.points[-1]則是完整路徑的終點。

Example 5.8: Path Following

按滑鼠按鍵，可以切換是否顯示future、normal_point、目標物等細節。

# python version 3.10.9
import math
import sys

import pygame  # version 2.3.0


pygame.init()

pygame.display.set_caption("Example 5.8: Path Following")

WHITE = (255, 255, 255)

screen_size = WIDTH, HEIGHT = 640, 360
screen = pygame.display.set_mode(screen_size)

FPS = 60
frame_rate = pygame.time.Clock()

radius = 50
path = Path(radius)
path.add_point(0, 80)
path.add_point(150, 150)
path.add_point(450, 170)
path.add_point(640, 230)

vehicle1 = Vehicle(0, 50)
vehicle1.velocity = pygame.Vector2(2, 0)
vehicle1.max_speed = 2
vehicle1.max_force = 0.02

vehicle2 = Vehicle(0, 180)
vehicle2.velocity = pygame.Vector2(2, 0)
vehicle2.max_speed = 3
vehicle2.max_force = 0.05

show_details = True

while True:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            pygame.quit()
            sys.exit()
        elif event.type == pygame.MOUSEBUTTONDOWN:
            show_details = not show_details

    screen.fill(WHITE)    
    
    path.show()
    
    vehicle1.borders(path)
    vehicle1.follow_path(path, show_details)
    vehicle1.update()
    vehicle1.show()
    
    vehicle2.borders(path)
    vehicle2.follow_path(path, show_details)
    vehicle2.update()
    vehicle2.show()    
    
    pygame.display.update()
    frame_rate.tick(FPS)

在follow_up()方法中，我們直接寫死，就把目標物放在交點前方距離25像素的地方。當然，除了直接寫死之外，也可以根據Reynolds所指出的做法那樣，動態地根據vehicle的速率，以及他跟路徑之間的距離來計算調整；這種動態調整的方式，效果應該會比較好。

Exercise 5.11

讓每段短路徑的端點具備在Example 5.3中所設計的轉向行為；不過，只有在進入水平邊界時才會產生轉向力。所以，針對Example 5.3中所設計的Vehicle類別的boundaries()方法，把其中「進入垂直邊界，以最大的水平方向速率遠離」部份的程式碼刪除，修改成這樣：

def boundaries(self, offset):
    desired = None
    
    # 進入水平邊界，以最大的垂直方向速率分量遠離
    if self.position.y > self.height - offset:
        desired = pygame.Vector2(self.velocity.x, -self.max_speed)
    elif self.position.y < offset:
        desired = pygame.Vector2(self.velocity.x, self.max_speed) 

    if desired is not None:
        desired.scale_to_length(self.max_speed)
        steer = desired - self.velocity
        if steer.length() > 1.e-5:
            steer.scale_to_length(self.max_force)
            self.apply_force(steer)

至於Path類別，則改成這樣：

class Path:
    def __init__(self, radius):
        # 取得顯示畫面
        self.screen = pygame.display.get_surface()
        
        self.radius = radius
        
        self.points = []
        self.nodes = []
        
    def add_point(self, x, y):
        node = Vehicle(x, y)
        self.nodes.append(node)
        self.points.append(node.position)

    def show(self):
        pygame.draw.lines(self.screen, (200, 200, 200), False, self.points, 2*self.radius)
        pygame.draw.lines(self.screen, (0, 0, 0), False, self.points, 2)

    def update(self):
        for node in self.nodes:
            node.boundaries(100)
            node.update()

主程式：

# python version 3.10.9
import math
import random
import sys

import pygame  # version 2.3.0


pygame.init()

pygame.display.set_caption("Exercise 5.11")

WHITE = (255, 255, 255)

screen_size = WIDTH, HEIGHT = 640, 360
screen = pygame.display.set_mode(screen_size)

FPS = 60
frame_rate = pygame.time.Clock()

radius = 50
path = Path(radius)
path.add_point(0, 80)
path.add_point(150, 150)
path.add_point(450, 170)
path.add_point(640, 230)
for node in path.nodes:
    node.velocity = pygame.Vector2(0, 5*random.random())
    node.max_speed = 0.5
    node.max_force = 0.02

vehicle1 = Vehicle(0, 50)
vehicle1.velocity = pygame.Vector2(2, 0)
vehicle1.max_speed = 2
vehicle1.max_force = 0.02

vehicle2 = Vehicle(0, 180)
vehicle2.velocity = pygame.Vector2(2, 0)
vehicle2.max_speed = 3
vehicle2.max_force = 0.05

show_details = True

while True:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            pygame.quit()
            sys.exit()
        elif event.type == pygame.MOUSEBUTTONDOWN:
            show_details = not show_details

    screen.fill(WHITE)    
    
    path.update()
    path.show()
    
    vehicle1.borders(path)
    vehicle1.follow_path(path, show_details)
    vehicle1.update()
    vehicle1.show()
    
    vehicle2.borders(path)
    vehicle2.follow_path(path, show_details)
    vehicle2.update()
    vehicle2.show()    
    
    pygame.display.update()
    frame_rate.tick(FPS)