title = "A* Search"
GraphicsWindow.Title = title
SB_Workaround()
Init()
While "True"
GenerateMaze()
InitNodes()
n = 0 ' parent of start node
m = startNode
Node2Pos()
Turtle.Speed = 9
Turtle.PenUp()
Turtle_MoveToWorkaround()
open = ""
close = ""
parent = ""
CalcH()
f = h
AddNodeToOpenList()
GraphicsWindow.BrushColor = "Orange"
rMark = rNode
found = "False"
While (open[0] <> "") And Not[found]
GetNearestNode()
If Array.ContainsValue(goalNodes, n) Then
found = "True"
EndIf
If Not[found] Then
m = n
CalcH()
g = fOpen[n] - h
RemoveNodeFromOpenList()
AddNodeToCloseList()
For iDir = 0 To 7
m = n + dir[iDir]
If (node[m] = "passage") Or (node[m] = "goal") Then
Node2Pos()
GraphicsWindow.FillEllipse(x - rMark, y - rMark, 2 * rMark, 2 * rMark)
CalcH()
fDash = g + cost[iDir] + h
CheckNodeInLists()
If Not[inOpen] And Not[inClose] Then
f = fDash
AddNodeToOpenList()
ElseIf inOpen Then
If fDash < fOpen[m] Then
RemoveNodeFromOpenList()
f = fDash
AddNodeToOpenList()
EndIf
ElseIf inClose Then
If fDash < fOpen[m] Then
RemoveNodeFromCloseList()
f = fDash
AddNodeToOpenList()
EndIf
EndIf
EndIf
EndFor
EndIf
EndWhile
If found Then
route = ""
i = 0
While node[n] <> "start"
i = i + 1
route[i] = n
n = parent[n]
EndWhile
GraphicsWindow.PenWidth = 2
GraphicsWindow.PenColor = "Black"
Turtle.PenDown()
For i = Array.GetItemCount(route) To 1 Step -1
m = route[i]
Node2Pos()
Turtle_MoveToWorkaround()
nTL = nTL + 1
EndFor
EndIf
Program.Delay(3000)
ClearTurtleTrails()
EndWhile
Sub AddNodeToCloseList
' param m - to add
close[m] = "True"
EndSub
Sub AddNodeToOpenList
' param n - parent node
' param m - node to add
' param f - f(node)
p = 0
If Array.ContainsValue(open, m) Then
TextWindow.WriteLine("AddNodeToOpenList Error: node " + m + " already exists.")
Else
While (open[p] <> "") And (fOpen[p] <= f)
p = open[p]
EndWhile
_m = open[p]
open[p] = m
fOpen[m] = f
open[m] = _m
parent[m] = n
EndIf
EndSub
Sub AddWallToList
' param col, row - cell
' return nWalls - number of wall list
' return iWalls - index of wall list
' return colWalls[], rowWalls[] - wall list
' return colOpp[], rowOpp[] - cell list (on the opposite side)
For d = 0 To 3
colw = col + colAdj[d]
roww = row + rowAdj[d]
If 1 < colw And colw < cols And 1 < roww And roww < rows And cell[roww][colw] = WALL Then
i = colw + (roww - 1) * cols
' find i in wall list
FindWallInList()
If i = iFound Then ' found
' remove the wall
nWalls = nWalls - 1
iWalls[iPrev] = iWalls[iFound]
Else
' add the wall to wall list
nWalls = nWalls + 1
iNext = iWalls[0]
iWalls[0] = i
iWalls[i] = iNext
colWall[i] = colw ' wall
rowWall[i] = roww
colOpp[i] = colw + colAdj[d] ' cell on the opposite side
rowOpp[i] = roww + rowAdj[d]
colPath[i] = col ' for graph
rowPath[i] = row ' for graph
EndIf
EndIf
EndFor
EndSub
Sub CalcH
' param m - node
' return h - minimum cost from node to goal
Node2Pos()
_x = x
_y = y
Stack.PushValue("local", m)
For iGoal = 1 To nGoal
m = goalNodes[iGoal]
Node2Pos()
xx = x - _x
yy = y - _y
_h = Math.SquareRoot(xx * xx + yy * yy)
If (iGoal = 1) Or (_h < h) Then
h = _h
EndIf
EndFor
m = Stack.PopValue("local")
EndSub
Sub CheckNodeInLists
' param m - node
' return inOpen - "True" if node m is in open list
' return inClose - "True" if node m is in close list
If Array.ContainsValue(open, m) Then
inOpen = "True"
Else
inOpen = "False"
EndIf
If close[m] Then
inClose = "True"
Else
' needed because close[m] is "True" or ""
inClose = "False"
EndIf
EndSub
Sub ClearTurtleTrails
For iTL = sTL To nTL
Shapes.Remove("_turtleLine" + iTL)
EndFor
sTL = nTL + 1
EndSub
Sub DownArrow
' param bc - brush color
' param x, y - top left corner
' param width
' param height
GraphicsWIndow.BrushColor = bc
w = width * 0.45
GraphicsWindow.FillRectangle(x + (width - w) / 2, y, w, height / 2)
x1 = x
y1 = y + height / 2
x2 = x + width
y2 = y1
x3 = x + width / 2
y3 = y + height
GraphicsWindow.FillTriangle(x1, y1, x2, y2, x3, y3)
EndSub
Sub DrawStartAndGoal
x = 33
y = 6
width = 30
height = 60
bc = "Red"
DownArrow()
x = 535
y = 362
bc = "#0AB000"
DownArrow()
EndSub
Sub EraseNodes
For i = 1 To nodes
Shapes.Remove(ell[i])
ell[i] = ""
EndFor
nodes = 0
EndSub
Sub FindWallInList
' param i - index to remove of wall list
' return iPrev - previous index of i
' return iFound - i if found
iFound = 0
c = 1
While i <> iFound And c <= nWalls
c = c + 1
iPrev = iFound
iFound = iWalls[iFound]
EndWhile
EndSub
Sub GenerateMaze
' Generate maze with randomized Prim's algorithm
' param x0, y0
' param colsPassage, rowsPassage
' param width
' 1. Start with a grid full of walls.
ClearMaze()
col = 2
row = 2
colw = 2
roww = 1
KnockDownWall()
col = cols - 1
row = rows - 1
colw = cols - 1
roww = rows
KnockDownWall()
DrawStartAndGoal()
' 2. Pick a cell, mark it as part of the maze. Add the walls of the cell to the wall list.
col = 2 * col0
row = 2 * row0
AddWallToList()
' 3. While there are walls in the list:
While nWalls > 0
' 1. Pick a random wall from the list. If the cell on the opposite side isn't in the maze yet:
GetRandomIndex()
col = colOpp[i]
row = rowOpp[i]
If cell[row][col] = WALL Then
' 1. Make the wall a passage and mark the cell on the opposite side as part of the maze.
colw = colWall[i]
roww = rowWall[i]
RemoveWallFromList()
cell[roww][colw] = PASSAGE ' wall
cell[row][col] = PASSAGE ' cell on the opposite side
colp = colPath[i] ' for graph
rowp = rowPath[i] ' for graph
node1 = (rowp / 2 - 1) * colsPassage + colp / 2 ' node # for graph
node2 = (row / 2 - 1) * colsPassage + col / 2 ' node # for graph
edge[node1][node2] = "True" ' one direction edge to maze graph
edge[node2][node1] = "True" ' opposite direction edge to maze graph
node1 = node2 ' for graph
KnockDownWall()
' 2. Add the neighboring walls of the cell to the wall list.
AddWallToList()
Else
' 2. If the cell on the opposite side already was in the maze, remove the wall from the list.
RemoveWallFromList()
EndIf
EndWhile
DrawStartAndGoal()
EndSub
Sub GetNearestNode
' return n - nearest node
nCand = 1 ' number of candidate nodes
candNodes[nCand] = open[0]
candNodes[nCand + 1] = open[candNodes[nCand]]
While (candNodes[nCand + 1] <> "") And (fOpen[candNodes[nCand]] = fOpen[candNodes[nCand + 1]])
nCand = nCand + 1
candNodes[nCand + 1] = open[candNodes[nCand]]
EndWhile
n = candNodes[Math.GetRandomNumber(nCand)]
EndSub
Sub GetRandomIndex
' get random index of wall list
' return i - index
i = 0
For c = 1 To Math.GetRandomNumber(nWalls)
i = iWalls[i]
EndFor
EndSub
Sub Node2Pos
' param m - node
' return x, y - coordinate of the node position
x = Math.Remainder(m - 1, nCols) * dx
y = Math.Floor((m - 1) / nCols) * dy
EndSub
Sub RemoveNodeFromCloseList
' param m - node to remove
close[m] = ""
EndSub
Sub RemoveNodeFromOpenList
' param m - node to remove
p = 0
While (open[p] <> m)
p = open[p]
EndWhile
If p <> "" Then
open[p] = open[open[p]]
open[m] = ""
Else
TextWindow.WriteLine("RemoveNodeFromOpenList Error: node " + m + " not found.")
EndIf
EndSub
Sub RemoveWallFromList
' param i - index to remove of wall list
' param wall[] - wall list
' param nWalls - number of wall list
FindWallInList()
If i = iFound Then ' found
iWalls[iPrev] = iWalls[iFound]
iWalls[iFound] = ""
nWalls = nWalls - 1
EndIf
EndSub
Sub Scan
If x < 0 Or gw <= x Or y < 0 Or gh <= y Then
color = "#000000"
Else
If silverlight Then
rx = Math.Round(x)
ry = Math.Round(y)
color = GraphicsWindow.GetPixel(rx, ry)
Else
color = GraphicsWindow.GetPixel(x, y)
EndIf
EndIf
Color_ColorToRGB()
totalR = r
totalG = g
totalB = b
totalN = 1
For d = 0 To 7
_x = x + distX[d] / 3
_y = y + distY[d] / 3
If _x < 0 Or gw <=_x Or _y < 0 Or gh <= _y Then
color = "#000000"
Else
If silverlight Then
rx = Math.Round(_x)
ry = Math.Round(_y)
color = GraphicsWindow.GetPixel(rx, ry)
Else
color = GraphicsWindow.GetPixel(_x, _y)
EndIf
EndIf
Color_ColorToRGB()
totalR = totalR + r
totalG = totalG + g
totalB = totalB + b
totalN = totalN + 1
EndFor
r = Math.Round(totalR / totalN)
g = Math.Round(totalG / totalN)
b = Math.Round(totalB / totalN)
If (r = 255) And (g = 255) And (b = 255) Then
color = "Transparent"
node[nodes] = "passage"
ElseIf (g < r) And (b < r) And (160 < r) Then
color = "#FF0000"
node[nodes] = "start"
startNode = nodes
ElseIf (r < g) And (b < g) And (160 < g) Then
color = "#00CC00"
node[nodes] = "goal"
nGoal = nGoal + 1
goalNodes[nGoal] = nodes
Else
color = "#000000"
node[nodes] = "wall"
EndIf
EndSub
Sub Color_ColorToRGB
' Color | Convert color To RGB values
' param color - "#rrggbb" (hexadecimal values)
' return r, g, b - RGB values 0..255
If Text.GetLength(color) = 9 Then
offset = 2
Else
offset = 0
EndIf
sR = Text.GetSubText(color, offset + 2, 2)
sG = Text.GetSubText(color, offset + 4, 2)
sB = Text.GetSubText(color, offset + 6, 2)
hex = sR
Math_Hex2Dec()
r = dec
hex = sG
Math_Hex2Dec()
g = dec
hex = sB
Math_Hex2Dec()
b = dec
EndSub
Sub Math_CartesianToPolar
' Math | convert cartesian coodinate To polar coordinate
' param x, y - cartesian coordinate
' return r, a - polar coordinate
r = Math.SquareRoot(x * x + y * y)
If x = 0 And y > 0 Then
a = 90 ' [degree]
ElseIf x = 0 And y < 0 Then
a = -90
ElseIf x = 0 Then
a = 0
Else
a = Math.ArcTan(y / x) * 180 / Math.Pi
EndIf
If x < 0 Then
a = a + 180
ElseIf x > 0 And y < 0 Then
a = a + 360
EndIf
EndSub
Sub Math_Hex2Dec
' Math | Convert hexadecimal To decimal
' param hex
' return dec
dec = 0
len = Text.GetLength(hex)
For ptr = 1 To len
dec = dec * 16 + Text.GetIndexOf("123456789ABCDEF", Text.GetSubText(hex, ptr, 1))
EndFor
EndSub
Sub SB_Workaround
' Small Basic | Workaround For Silverlight
' returns silverlight - "True" If in remote
color = GraphicsWindow.GetPixel(0, 0)
If Text.GetLength(color) > 7 Then
silverlight = "True"
msWait = 300
Else
silverlight = "False"
EndIf
EndSub
Sub Turtle_MoveToWorkaround
' param x, y - destination point
Stack.PushValue("local", x)
Stack.PushValue("local", y)
x = x - Turtle.X
y = y - Turtle.Y
Math_CartesianToPolar()
a = Math.Remainder(a + 90 - Turtle.Angle, 360)
If a < -180 Then
a = a + 360
ElseIf 180 < a Then
a = a - 360
EndIf
Turtle.Turn(a)
Turtle.Move(r)
EndSub
Program Listing: PRV488-2