当两点之间有一条视线时，如何计算父节点与邻居节点之间的距离？

Question

I have completed my Astar algorithm in Python and now I need to convert it to a Theta star algorithm, I have built my line of sight algorithm below, but when I come to my Theta star algorithm I face some problems when there is a line of sight by calculating the distance and how could I make it jump the points that have line of sight. After running my code, I see there is no any effect, I see it's working as Astar algorithm. Any assistance, please?

The snippet that I have a problem with it:

sight = lineOfsight(grid, y, x, y2, x2)
if sight == True:

        g2 = g + delta[i][2] + math.sqrt((x2 - x)**2 + (y2 - y)**2)

         h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)

         f2 = g2 + h2
  else:

         g2 = g + delta[i][2]             
         h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)

         f2 = g2 + h2

 open.append([f2,g2,h2,x2,y2])

My line of sight code:

def lineOfsight(grid, y1, x1, y2, x2):
    y_size = len(grid)
    x_size = len(grid)

    #Distance
    dy=y2-y1
    dx=x2-x1

    if dy < 0:
        dy = -dy
        sy = -1
    else:
        sy = 1

    if dx < 0:
        dx = -dx
        sx = -1
    else:
        sx = 1

    f = 0

    if dx >= dy:
        while x1 != x2:
            f = f + dy
            if f >= dx and 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:

                    return False
                y1 = y1 + sy
                f  = f  - dx

            elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if f != 0 and grid[x1+(sx-1)/2][y1+(sy-1)/2]:

                    return False

            elif 1<y1 and y1<y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if dy==0 and grid[x1+int((sx-1)/2)][y1] and grid[x1+int((sx-1)/2)][y1-1] :

                    return False
            x1 = x1 + sx

    else:

        while y1 != y2:
            f = f + dx
            if f >= dy and 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0< x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:

                    return False
                x1 = x1 + sx
                f = f - dy
            elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if f !=0 and grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:

                    return False

            elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 1 < x1 and x1 < x_size:       
                if dx == 0 and grid[x1][y1+ int((sy-1)/2)] and grid[x1-1][y1+int((sy-1)/2)]:

                    return False

            y1=y1+sy

    return True

My theta star code:

import matplotlib.pyplot as plt
from lineofsightss import *
#grid format
# 0 = navigable space
# 1 = occupied space

import random
import math

grid = [[0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
        [0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0],
        [0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]



init = [0,0]                            #Start location is (5,5) which we put it in open list.
goal = [len(grid)-1,len(grid[0])-1]     

heuristic = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
for i in range(len(grid)):    
    for j in range(len(grid[0])):            
        heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])

plt.plot(0,10)
plt.plot(0,-len(grid)-10)
plt.grid(True)
plt.axis("equal")

plt.plot([-1, len(grid[0])],[[-x/2 for x in range(-1,len(grid)*2+1)], [-y/2 for y in range(-1,len(grid)*2+1)]], ".k")
plt.plot([[x/2 for x in range(-2,len(grid[0])*2+1)],[x/2 for x in range(-2,len(grid[-1])*2+1)]],[1, -len(grid)],".k")

plt.plot(init[1],-init[0],"og")
plt.plot(goal[1],-goal[0],"ob")


#Below the four potential actions to the single field


delta =      [[1, 0, 1],
              [0, 1, 1],
              [-1, 0, 1],
              [0, -1, 1],
              [-1, -1, math.sqrt(2)],
              [-1, 1, math.sqrt(2)],
              [1, -1, math.sqrt(2)],
              [1, 1, math.sqrt(2)]]



delta_name = ['V','>','<','^','//','\\','\\','//']


def search():

    pltx,plty=[],[]
    #open list elements are of the type [g,x,y]

    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    action = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
    #We initialize the starting location as checked
    closed[init[0]][init[1]] = 1
    expand=[[-1 for row in range(len(grid[0]))] for col in range(len(grid))]

    # we assigned the cordinates and g value
    x = init[0]
    y = init[1]
    g = 0

    h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)

    f = g + h

    #our open list will contain our initial value
    open = [[f, g, h, x, y]]


    found  = False   #flag that is set when search complete
    resign = False   #Flag set if we can't find expand
    count = 0

    #print('initial open list:')
    #for i in range(len(open)):
            #print('  ', open[i])
    #print('----')

    while found is False and resign is False:

        #Check if we still have elements in the open list
        if len(open) == 0:    #If our open list is empty, there is nothing to expand.
            resign = True
            print('Fail')
            print('############# Search terminated without success')
            print()
        else:
            #if there is still elements on our list
            #remove node from list
            open.sort()             #sort elements in an increasing order from the smallest g value up
            open.reverse()          #reverse the list
            next = open.pop()       #remove the element with the smallest g value from the list
            #print('list item')
            #print('next')

            #Then we assign the three values to x,y and g. Which is our expantion.
            x = next[3]
            y = next[4]
            g = next[1]
            #elvation[x][y] = np.random.randint(100, size=(5,6))
            expand[x][y] = count
            count+=1

            #Check if we are done
            if x == goal[0] and y == goal[1]:
                found = True
                print(next) #The three elements above this "if".
                print('############## Search is success')
                print()

            else:
                #expand winning element and add to new open list
                for i in range(len(delta)):       #going through all our actions the four actions
                    #We apply the actions to x and y with additional delta to construct x2 and y2
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]


                    #if x2 and y2 falls into the grid
                    if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 <= len(grid[0])-1:

                        #if x2 and y2 not checked yet and there is not obstacles
                        if closed[x2][y2] == 0 and grid[x2][y2]==0:

                            sight = lineOfsight(grid, y, x, y2, x2)
                            if sight == True:

                                g2 = g + delta[i][2] + math.sqrt((x2 - x)**2 + (y2 - y)**2)

                                h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)

                                f2 = g2 + h2
                            else:

                                g2 = g + delta[i][2]             
                                h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)

                                f2 = g2 + h2

                            open.append([f2,g2,h2,x2,y2])
                            #we add them to our open list
                            pltx.append(y2)
                            plty.append(-x2)
                            #print('append list item')
                            #print([g2,x2,y2])
                            #Then we check them to never expand again
                            closed[x2][y2] = 1
                            action[x2][y2] = i

    for i in range(len(expand)):
        print(expand[i])
    print()
    policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
    x=goal[0]
    y=goal[1]
    policy[x][y]='*'
    visx = [y]
    visy = [-x]
    while x !=init[0] or y !=init[1]:
        x2=x-delta[action[x][y]][0]
        y2=y-delta[action[x][y]][1]
        policy[x2][y2]= delta_name[action[x][y]]
        x=x2
        y=y2
        visx.append(y)
        visy.append(-x)
    for i in range(len(policy)):
        print(policy[i])
    print()





    plt.plot(visx,visy, "-r")

    plt.show()

search()

Here is below my output:

Answer 1

In Theta* , when the neighboring node sees the parent, you should try to connect that neighboring node directly to the parent of the current node. This is the process that leads to any-angle , non-grid-aligned paths.

This association of a node to an arbitrary parent (not necessarily one that is a neighbor in the grid) is actually missing from the solution (thus the path will not be properly reconstructed when the search is done).

---

This involves some changes in the code from the question:

• the path reconstruction should be implemented differently, the "action" array containing one of the eight movement directions is not sufficient. One viable alternative would be for it to contain the (x, y) coordinates of the parent node, ie action[x][y] = (parent_x, parent_y).

• in the code inside if sight == True you should calculate the g-score of the neighbor as the path that uses the straight line from parent to neighbor (dashed line in the figure above). At this point, the calculation of the g-score takes into account the current node, which is not necessary.

---

Below is a modification of the posted code that incorporates some of these changes. It is still possible that other issues still exist, but it is a step in the right direction.

def search():
    pltx,plty=[],[]

    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    action = [[(-1, -1) for row in range(len(grid[0]))] for col in range(len(grid))]
    closed[init[0]][init[1]] = 1
    expand = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]

    # we assigned the coordinates and g value
    x = init[0]
    y = init[1]
    g = 0

    h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)

    f = g + h

    open = [[f, g, h, x, y]]

    found = False    # flag that is set when search complete
    resign = False   # flag set if we can't find expand
    count = 0

    while found is False and resign is False:
        # check if we still have elements in the open list
        if len(open) == 0:    # if our open list is empty, there is nothing to expand.
            resign = True
            print('Fail')
            print('############# Search terminated without success')
            print()
        else:
            # if there is still elements on our list
            # remove node from list
            open.sort()             # sort elements in an increasing order from the smallest g value up
            open.reverse()          # reverse the list
            next = open.pop()       # remove the element with the smallest g value from the list

            # then we assign the three values to x,y and g. Which is our expantion.
            x = next[3]
            y = next[4]
            g = next[1]
            # elvation[x][y] = np.random.randint(100, size=(5,6))
            expand[x][y] = count
            count += 1

            # check if we are done
            if x == goal[0] and y == goal[1]:
                found = True
                print(next)     # the three elements above this "if".
                print('############## Search is success')
                print()

            else:
                # expand winning element and add to new open list
                for i in range(len(delta)):       # going through all our actions the four actions
                    # we apply the actions to x and y with additional delta to construct x2 and y2
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]

                    # if x2 and y2 falls into the grid
                    if 0 <= x2 < len(grid) and 0 <= y2 <= len(grid[0]) - 1:
                        #if x2 and y2 not checked yet and there is not obstacles
                        if closed[x2][y2] == 0 and grid[x2][y2] == 0:
                            sight = lineOfsight(grid, y, x, y2, x2)

                            parent_x, parent_y = action[x][y]
                            if sight and parent_x >= 0:
                                g2 = g + math.sqrt((x2 - parent_x)**2 + (y2 - parent_y)**2)
                                h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
                                f2 = g2 + h2
                                action[x2][y2] = (parent_x, parent_y)
                            else:
                                g2 = g + delta[i][2]
                                h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
                                f2 = g2 + h2
                                action[x2][y2] = (x, y)

                            open.append([f2,g2,h2,x2,y2])
                            # we add them to our open list
                            pltx.append(y2)
                            plty.append(-x2)
                            closed[x2][y2] = 1

    for i in range(len(expand)):
        print(expand[i])
    print()
    policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
    x=goal[0]
    y=goal[1]
    visx = [y]
    visy = [-x]
    while x !=init[0] or y !=init[1]:
        x2=action[x][y][0]
        y2=action[x][y][1]
        x=x2
        y=y2
        visx.append(y)
        visy.append(-x)
    print()

    plt.plot(visx,visy, "-r")

    plt.show()

This produces the following path: