3d a* pathfinding

And while I'm at it... Here's some ready to use heap function.s

;HEAP FUNCTIONS
Global HEAPSIZE=0
Global HMAXSIZE=P_MAXNODES
Dim HEAPITEM%(HMAXSIZE)
Dim HEAPKEY#(HMAXSIZE)



Function HEAP_INSERT(n,key#)
HEAPSIZE=HEAPSIZE+1
HEAPITEM(HEAPSIZE) = n

m = HEAPSIZE
HEAPKEY(HEAPITEM(m))=key

  While m <> 1 ;While item hasn't bubbled to the top (m=1)

     ;Check if child is <= parent. If so, swap them.
     If HEAPKEY(HEAPITEM(m)) <= HEAPKEY(HEAPITEM(m/2)) Then
        temp = HEAPITEM(m/2)
        HEAPITEM(m/2) = HEAPITEM(m)
        HEAPITEM(m) = temp
        m = m/2 
     Else

        Exit ;exit the while/wend loop
     End If
  Wend
End Function




Function HEAP_REMOVELOWEST()
;If HEAP_EMPTY()=0
L = HEAPITEM(1)

;GET THE LAST ITEM AND PUT IT IN THE FIRST POSITION
;(FIRST GET*S DELETED)
HEAPITEM(1) = HEAPITEM(HEAPSIZE)
HEAPSIZE = HEAPSIZE - 1

v = 1
;Repeat the following until the item sinks to its proper spot in the binary heap.
Repeat
  u = v
  If 2*u+1 <= HEAPSIZE;if both children exist
    ;Select the lowest of the two children.
    If HEAPKEY(HEAPITEM(u)) >= HEAPKEY(HEAPITEM(2*u)) Then v = 2*u 
    If HEAPKEY(HEAPITEM(v)) >= HEAPKEY(HEAPITEM(2*u+1)) Then v = 2*u+1

  Else If 2*u <= HEAPSIZE ;if only child #1 exists
    ;Check if the F cost is greater than the child
    If HEAPKEY(HEAPITEM(u)) >= HEAPKEY(HEAPITEM(2*u))Then v = 2*u
  End If

  If u <> v Then ; If parent's F > one or both of its children, swap them
    temp = HEAPITEM(u)
    HEAPITEM(u) = HEAPITEM(v)
    HEAPITEM(v) = temp
  Else
    Exit ;if item <= both children, exit repeat/forever loop
  End If
Forever

Return L
;End If
End Function



Function UPDATE_HEAPITEM(item,newkey#,HEAPPOS=0)
If HEAPPOS=0
For i=1 To HEAPSIZE
If HEAPITEM(i) = item Then m=i:Exit
Next
Else 
m=HEAPPOS
End If

HEAPKEY(HEAPITEM(m)) = newkey

  While m <> 1 ;While item hasn't bubbled to the top (m=1)
     ;Check if child is <= parent. If so, swap them.
     If HEAPKEY(HEAPITEM(m)) <= HEAPKEY(HEAPITEM(m/2)) Then
        temp = HEAPITEM(m/2)
        HEAPITEM(m/2) = HEAPITEM(m)
        HEAPITEM(m) = temp
        m = m/2 
     Else
        Exit ;exit the while/wend loop
     End If
  Wend
End Function



Function HEAP_EMPTY()
If HEAPSIZE>0 Return 0 Else Return 1
End Function
Function CLEAR_HEAP()
Dim HEAPITEM%(HMAXSIZE)
Dim HEAPKEY#(HMAXSIZE)
HEAPSIZE=0
End Function