Pathfinding in Yabasic

[rain_storm]

This program uses tha A* pathfinding algorithm to find the shortest path between two points. It does find the shortest path but I think the code for adding new nodes to the open list and the code for finding the open node with the lowest cost could be improved apon a lot. Anyway here it goes :

Code: [Select]

open window 640, 512

gridX = 19
gridY = 15
dim grid(gridX, gridY)
dim list(gridX, gridY)
for y = 0 to gridY
  for x = 0 to gridX
    read grid(x, y)
  next
next
posX = 2
posY = 2
tarX = 17
tarY = 13

label main
  astar(posX, posY, tarX, tarY)
  goto main

sub astar(px, py, tx, ty)
  nodes = 1
  closed = 0

  for y = 0 to gridY
    for x = 0 to gridX
      list(x,y) = 0
    next
  next
  dim nodeX(nodes), nodeY(nodes)
  dim homeX(nodes), homeY(nodes)
  dim cost(nodes)
  nodeX(nodes) = px
  nodeY(nodes) = py
  homeX(nodes) = px
  homeY(nodes) = py
  cost(nodes) = abs(px-tx) + abs(py-ty)
  list(px,py) = 1
  cx = px
  cy = py

  while (closed < nodes)
    render()
    cost = (gridX+1)*(gridY+1)*10
    for n = 1 to nodes
      if (list(nodeX(n), nodeY(n)) = 1) and (cost(n) < cost) then
        cost = cost(n)
        cur = n
      endif
    next
    cx = nodeX(cur)
    cy = nodeY(cur)
    cost = cost - (abs(tx-cx)+abs(ty-cy))
    list (cx,cy) = 2
    closed = closed + 1

    for y = cy-1 to cy+1
      for x = cx-1 to cx+1
        if (grid(x, y) = 0) and (list(x, y) = 0) then
          nodes = nodes + 1
          redim nodeX(nodes), nodeY(nodes)
          redim homeX(nodes), homeY(nodes)
          redim cost(nodes)
          nodeX(nodes) = x
          nodeY(nodes) = y
          homeX(nodes) = cx
          homeY(nodes) = cy
          if (abs(x-cx) = 1) and (abs(y-cy) = 1) then
            cost(nodes) = cost + 14 + abs(tx-x) + abs(ty-y)
          else
            cost(nodes) = cost + 10 + abs(tx-x) + abs(ty-y)
          endif
          list(x,y) = 1
        endif
      next
    next
    if (list(tx,ty) <> 0) goto trace
  wend

label trace
  while (inkey$(0) = "")
    render()
    x = tx
    y = ty

    length = 0
    for n = 1 to nodes
      if (nodeX(n) = x) and (nodeY(n) = y) then
        length = length + 1
        redim pathX(length), pathY(length)
        setrgb 1, 255, 255, 000
        fill box x*s+8, y*s+8 to x*s+s-8, y*s+s-8
        setrgb 1, 000, 000, 000
        text x*s+16, y*s+10, str$(length), "cc"

        x = homeX(n)
        y = homeY(n)
        pathY(length) = y
        pathX(length) = x
        if (x <> nodeX(n)) or (y <> nodeY(n)) n = 1
      endif
    next
  wend
end sub

sub render()
  setdispbuf draw
  draw = 1 - draw
  setdrawbuf draw
  clear window
  s = 32
  for y = 0 to gridY
    for x = 0 to gridX
      setrgb 1, 010, 010, 010
      if (grid(x, y) = 1) setrgb 1, 000, 255, 000
      if (list(x, y) = 1) setrgb 1, 000, 000, 100
      if (list(x, y) = 2) setrgb 1, 100, 000, 000
      fill box x*s, y*s to x*s+s, y*s+s
    next
  next
  setrgb 1, 100, 100, 100
  for y = 0 to gridY
    for x = 0 to gridX box x*s, y*s to x*s+s, y*s+s next
  next
  setrgb 1, 000, 000, 255
  fill box posX*s, posY*s to posX*s+s, posY*s+s
  setrgb 1, 255, 000, 000
  fill box tarX*s, tarY*s to tarX*s+s, tarY*s+s
end sub

data 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1
data 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
data 1,0,0,0,1,1,1,1,1,0,1,0,1,1,1,1,1,1,0,1
data 1,0,0,0,1,0,0,0,0,0,1,0,1,0,0,0,0,0,0,1
data 1,0,1,1,1,0,1,1,1,1,1,0,1,0,1,1,1,1,1,1
data 1,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1
data 1,0,1,0,1,1,1,1,1,0,1,0,1,1,1,1,1,1,0,1
data 1,0,1,0,1,0,0,0,1,0,1,0,0,0,0,0,0,0,0,1
data 1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,1,1,1,0,1
data 1,0,1,0,1,0,1,0,1,0,1,0,0,0,1,0,0,0,0,1
data 1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,1
data 1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,0,0,0,1
data 1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,0,0,0,1
data 1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,0,0,0,1
data 1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,1
data 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1



[ninogenio]

excellent post rain,

this stuff holds a great deal of intrest for me as well.
it can be used for lots of stuff from demos to gost's in pacman.

i think im going to have to give you karma!

any chance in a random maze generator using said algo?

[rain_storm]

I was thinking about that myself but I couldnt get anything workin so far.

[ninogenio]

ill have a play around and see it should'nt be too hard .