View.Set ("graphics:1280:768")
% Pathfinding
type node :
record
x, y, parentX, parentY : int
end record
var nodeFirst, nodeLast, nodeNextLast, numNodes : int := 1
var nodeX, nodeY : int
var endNode : node
var retVal, pathVal : int
var reachEnd : boolean := false
var start, goal, parent : node
var atNode : array 1 .. 625 of node
var numTiles : int := 25
var map : array 1 .. numTiles, 1 .. numTiles of int
var mapVals : array 1 .. numTiles, 1 .. numTiles of int
var expandedNode : array 1 .. numTiles, 1 .. numTiles of boolean
var val, pathMinVal : int
var nodeNbr : int := 1
var pathToFollow : array 1 .. 401 of node
var foundPath : boolean := false
var moveCost := 2
var font := Font.New ("verdana:8")
for i : 1 .. upper (atNode)
atNode (i).x := 0
atNode (i).y := 0
end for
for i : 1 .. numTiles
for j : 1 .. numTiles
map (i, j) := 2
mapVals (i, j) := 1
expandedNode (i, j) := false
end for
end for
function findCombinations (point : node, var arrayStart : int) : int
if point.x - 1 = 0 or point.x + 1 = (numTiles + 1) or point.y - 1 = 0 or point.y + 1 = (numTiles + 1) then
result 0
end if
if (point.x - 1 >= 1) and expandedNode (point.x - 1, point.y) = false then
atNode (nodeNbr).x := point.x - 1
atNode (nodeNbr).y := point.y
nodeNbr += 1
expandedNode (point.x - 1, point.y) := true
atNode (nodeNbr).parentX := point.x
atNode (nodeNbr).parentY := point.y
end if
if (point.y - 1 >= 1) and expandedNode (point.x, point.y - 1) = false then
atNode (nodeNbr).x := point.x
atNode (nodeNbr).y := point.y - 1
expandedNode (point.x, point.y - 1) := true
nodeNbr += 1
atNode (nodeNbr).parentX := point.x
atNode (nodeNbr).parentY := point.y
end if
if (point.x + 1 <= numTiles) and expandedNode (point.x + 1, point.y) = false then
atNode (nodeNbr).x := point.x + 1
atNode (nodeNbr).y := point.y
expandedNode (point.x + 1, point.y) := true
nodeNbr += 1
atNode (nodeNbr).parentX := point.x
atNode (nodeNbr).parentY := point.y
end if
if (point.y + 1 <= numTiles) and expandedNode (point.x, point.y + 1) = false then
atNode (nodeNbr).x := point.x
atNode (nodeNbr).y := point.y + 1
expandedNode (point.x, point.y + 1) := true
nodeNbr += 1
atNode (nodeNbr).parentX := point.x
atNode (nodeNbr).parentY := point.y
end if
result nodeNbr - arrayStart
end findCombinations
function isValid (num : int) : boolean
if num >= 1 then
result true
end if
result false
end isValid
function getNodeValue (x, y : int) : int
pathMinVal := 500
% Check if it is an impossible tile...
if x = 0 or y = 0 then
result - 2
end if
if map (x, y) = -1 then
result - 1
end if
if (x - 1) >= 1 then
val := mapVals (x - 1, y)
if isValid (val) then
if val <= pathMinVal or pathMinVal = 0 then
pathMinVal := val
end if
end if
end if
if (x + 1) <= numTiles then
val := mapVals (x + 1, y)
if isValid (val) then
if val <= pathMinVal or pathMinVal = 0 then
pathMinVal := val
end if
end if
end if
if (y - 1) >= 1 then
val := mapVals (x, y - 1)
if isValid (val) then
if val <= pathMinVal or pathMinVal = 0 then
pathMinVal := val
end if
end if
end if
if (y + 1) <= numTiles then
val := mapVals (x, y + 1)
if isValid (val) then
if val <= pathMinVal or pathMinVal = 0 then
pathMinVal := val
end if
end if
end if
result pathMinVal
end getNodeValue
function tracePath (point, start, goal : node) : node
var xFactor, yFactor, absVal, absTestVal, lowestAbsVal : int := 0
pathMinVal := 500
lowestAbsVal := 500
var resultPt : node
resultPt.x := point.x
resultPt.y := point.y
% X VALUES
% Is it in the bounds?
if point.x - 1 = goal.x and point.y = goal.y then
result goal
elsif (point.x - 1) >= 1 and expandedNode (point.x - 1, point.y) = true then
% Find value
val := mapVals (point.x - 1, point.y)
locate (3, maxcol - 2)
if isValid (val) and val > 1 then
put val
absTestVal := abs ((start.x - (point.x - 1)) ** 2) + abs ((start.y - point.y) ** 2)
if val < pathMinVal or pathMinVal = 0 then
pathMinVal := val
xFactor := -1
yFactor := 0
lowestAbsVal := absTestVal
elsif val = pathMinVal and absTestVal <= lowestAbsVal then
pathMinVal := val
xFactor := -1
yFactor := 0
lowestAbsVal := absTestVal
end if
end if
end if
if point.x + 1 = goal.x and point.y = goal.y then
result goal
elsif (point.x + 1) <= numTiles and expandedNode (point.x + 1, point.y) = true then
% Find value
val := mapVals (point.x + 1, point.y)
if isValid (val) and val > 1 then
absTestVal := abs ((start.x - (point.x + 1)) ** 2) + abs ((start.y - point.y) ** 2)
if val < pathMinVal or pathMinVal = 0 then
pathMinVal := val
xFactor := 1
yFactor := 0
elsif val = pathMinVal and absTestVal <= lowestAbsVal then
pathMinVal := val
xFactor := 1
yFactor := 0
lowestAbsVal := absTestVal
end if
end if
end if
if point.x = goal.x and point.y - 1 = goal.y then
result goal
elsif (point.y - 1) >= 1 and expandedNode (point.x, point.y - 1) = true then
% Find value
val := mapVals (point.x, point.y - 1)
if isValid (val) and val > 1 then
absTestVal := abs ((start.x - point.x) ** 2) + abs ((start.y - (point.y - 1)) ** 2)
if val < pathMinVal or pathMinVal = 0 then
pathMinVal := val
xFactor := 0
yFactor := -1
elsif val = pathMinVal and absTestVal <= lowestAbsVal then
pathMinVal := val
xFactor := 1
yFactor := 0
lowestAbsVal := absTestVal
end if
end if
end if
if point.x = goal.x and point.y + 1 = goal.y then
result goal
elsif (point.y + 1) <= numTiles and expandedNode (point.x, point.y + 1) = true then
% Find value
val := mapVals (point.x, point.y + 1)
if isValid (val) and val < 1 then
absTestVal := abs ((start.x - point.x) ** 2) + abs ((start.y - (point.y + 1)) ** 2)
if val < pathMinVal or pathMinVal = 0 then
pathMinVal := val
xFactor := 0
yFactor := 1
elsif val = pathMinVal and absTestVal <= lowestAbsVal then
pathMinVal := val
xFactor := 1
yFactor := 0
lowestAbsVal := absTestVal
end if
end if
end if
if pathMinVal not= 0 then
resultPt.x += xFactor
resultPt.y += yFactor
end if
result resultPt
end tracePath
proc choosePath (start, goal : node, points : array 1 .. * of node)
var currentPoint : node
var count := 1
currentPoint := start
loop
if foundPath = false then
locate (1, maxcol)
drawfillbox (1 + currentPoint.x * 16, 1 + currentPoint.y * 16, 15 + currentPoint.x * 16, 15 + currentPoint.y * 16, blue)
drawfillbox (goal.x * 16, goal.y * 16, 16 + goal.x * 16, 16 + goal.y * 16, red)
currentPoint := tracePath (currentPoint, start, goal)
drawfillbox (1 + currentPoint.x * 16, 1 + currentPoint.y * 16, 15 + currentPoint.x * 16, 15 + currentPoint.y * 16, green)
exit when currentPoint.x = goal.x and currentPoint.y = goal.y
end if
end loop
end choosePath
proc pathSearch (start, goal : node)
% Prepare
endNode.x := goal.x
endNode.y := goal.y
% Initialize first node (starting location)
atNode (1).x := start.x
atNode (1).y := start.y
atNode (1).parentX := start.x
atNode (1).parentY := start.y
nodeFirst := 1
nodeLast := findCombinations (start, numNodes)
mapVals (start.x, start.y) := map (start.x, start.y)
parent := atNode (1)
loop
nodeNextLast := nodeLast
for a : nodeFirst .. nodeLast
% Store the x and y values for speed
nodeX := atNode (a).x
nodeY := atNode (a).y
pathVal := getNodeValue (nodeX, nodeY)
if pathVal = -1 then
mapVals (nodeX, nodeY) := -1
elsif pathVal > 0 then
% Adds the lowest value of closest tile to the current value of my tile
mapVals (nodeX, nodeY) := mapVals (atNode (a).parentX, atNode (a).parentY) + pathVal
% Draw it in
Font.Draw (intstr (mapVals (nodeX, nodeY)), nodeX * 16 + 2, nodeY * 16 + 2, font, black)
% Find the next 4 nodes
retVal := findCombinations (atNode (a), nodeNextLast)
nodeNextLast := nodeNextLast + retVal
% Destination reached?
if nodeX = endNode.x and nodeY = endNode.y then
reachEnd := true
end if
end if
end for
nodeFirst := nodeLast + 1
nodeLast := nodeNextLast
exit when reachEnd = true
exit when nodeFirst > nodeLast
end loop
if reachEnd = true then
choosePath (goal, start, atNode)
end if
end pathSearch
var temp, mx, my, mb : int
start.x := 15
start.y := 2
goal.x := 7
goal.y := 8
for i : 1 .. numTiles
for j : 1 .. numTiles
drawbox (i * 16, j * 16, 16 + i * 16, 16 + j * 16, black)
end for
end for
buttonchoose ("multibutton")
loop
mousewhere (mx, my, mb)
if mx < (16 * numTiles) and my < (16 * numTiles) and mb = 1 then
map (mx div 16, my div 16) := -1
drawfillbox ((mx div 16) * 16, (my div 16) * 16, 16 + (mx div 16) * 16, 16 + (my div 16) * 16, red)
end if
exit when mb = 100
end loop
pathSearch (start, goal)
% for i : 1 .. 10
% put pathToFollow (i).x, " ", pathToFollow (i).y
% end for