Added code at line 235 and a heuristic function, seem to work fine.
Code: Select all
#include once "fbgfx.bi"
#define heuristic2(ax, ay, bx, by) (abs(ax - bx ) + abs(ay - by)) '# Manhattan distance on a square grid
const as integer ALLOW_DIAGONAL_MOVEMENT = 1
'-------------------------------------------------------------------------------
type int2d
as integer x, y
end type
type intxyd
dim as integer x,y
dim as single dist
end type
#define dll_data_type intxyd
type dll_node_type
dim as dll_data_type value
dim as dll_node_type ptr pNext
dim as dll_node_type ptr pPrev
end type
sub dll_delete(byval pNode as dll_node_type ptr)
if pNode then
(pNode->pPrev)->pNext = pNode->pNext
if pNode->pNext then
(pNode->pNext)->pPrev = pNode->pPrev
end if
delete pNode
end if
end sub
sub dll_deleteFirst(byval pRootNode as dll_node_type ptr)
dim as dll_node_type ptr pNode = pRootNode->pNext
if pNode then
(pNode->pPrev)->pNext = pNode->pNext
if pNode->pNext then
(pNode->pNext)->pPrev = pNode->pPrev
end if
delete pNode
end if
end sub
sub dll_deleteAll(byval pRootNode as dll_node_type ptr)
dim as dll_node_type ptr pDelNode
while (pRootNode->pNext)
pDelNode = pRootNode->pNext
pRootNode->pNext = pDelNode->pNext
delete pDelNode
wend
end sub
function dll_count(byval pNode as dll_node_type ptr) as integer
dim as integer count = 0
while (pNode->pNext)
pNode = pNode->pNext
count += 1
wend
return count
end function
'low values at root node
sub dll_insertSorted(byval pNode as dll_node_type ptr, byval value as dll_data_type)
dim as dll_node_type ptr pNewNode = new dll_node_type
while (pNode->pNext)
pNode = pNode->pNext
if pNode->value.dist >= value.dist then
pNode = pNode->pPrev
exit while
end if
wend
if pNode->pNext then
pNewNode->pNext = pNode->pNext
(pNewNode->pNext)->pPrev = pNewNode
end if
pNewNode->pPrev = pNode
pNode->pNext = pNewNode
pNewNode->value = value
end sub
'-------------------------------------------------------------------------------
const as ubyte BOX_FREE = 0
const as ubyte BOX_BLOCKED = 1
const as ubyte BOX_START = 2
const as ubyte BOX_TARGET = 3
const as ubyte BOX_VISITED = 4
const as ubyte BOX_ROUTE = 5
const as integer SCRN_W = 1920, SCRN_H = SCRN_W * (9 / 16)
const as integer BOX_SIZE = 5
const as integer MAP_SIZE_X = SCRN_W \ BOX_SIZE
const as integer MAP_SIZE_Y = SCRN_H \ BOX_SIZE
dim shared as ubyte map(MAP_SIZE_X, MAP_SIZE_Y)
'predefine colors (argb), 32-bit unsigned
dim shared as ulong BOX_COLOR(0 to 5) = _
{&h00444444, &h00707070, &h0000a000, &h00d00000, &h00c0c000, &h00f0f0f0}
function rndIntBetween(min as integer, max as integer) as integer
return int(rnd*(max-min+1))+min
end function
sub drawBox(x as integer, y as integer)
if map(x, y) = BOX_FREE then
'show grid / empty box
line(x*BOX_SIZE, y*BOX_SIZE)-_
step(BOX_SIZE-1, BOX_SIZE-1), BOX_COLOR(BOX_FREE), b
else
'show boxes
line(x*BOX_SIZE, y*BOX_SIZE)-_
step(BOX_SIZE-1, BOX_SIZE-1), BOX_COLOR(map(x, y)), bf
end if
end sub
sub drawMap()
dim as integer x, y
For y = 0 to MAP_SIZE_Y-1
for x = 0 to MAP_SIZE_X-1
drawBox(x, y)
next
next
end sub
'-------------------------------------------------------------------------------
dim as integer x, y, i
dim as integer xStart, yStart
dim as integer xTarget, yTarget
dim as string key
const as single D_INF = 1e6
dim shared as single dist(MAP_SIZE_X, MAP_SIZE_Y) 'distance form start position
dim shared as int2d par(MAP_SIZE_X, MAP_SIZE_Y) 'x,y position of parent
dim as dll_node_type ptr pRoot = new dll_node_type 'priority queue
dim as integer xCur, yCur, xNbr, yNbr
dim as single curDist
dim as integer iDelta, routeFound
dim as double tSTart, tEnd
dim as integer allowDir
dim as intxyd delta(0 to 7) = _
{_
type(0, 1, 1.0), type(0, -1, 1.0), type(1, 0, 1.0), type(-1, 0, 1.0), _
type(+1, +1, 1.41), type(+1, -1, 1.41), type(-1, +1, 1.41), type(-1, -1, 1.41) _
}
if (ALLOW_DIAGONAL_MOVEMENT = 1) then allowDir = 8 else allowDir = 4
screenres SCRN_W, SCRN_H, 32
randomize timer
'randomize 1242
do
'create random map
For y = 0 to MAP_SIZE_Y-1
for x = 0 to MAP_SIZE_X-1
map(x, y) = BOX_FREE
'make box / borders
if (x = 0) or (x = MAP_SIZE_X-1) then map(x, y) = BOX_BLOCKED
if (y = 0) or (y = MAP_SIZE_Y-1) then map(x, y) = BOX_BLOCKED
'fill with random blocks
if (rnd >= .7) then map(x, y) = BOX_BLOCKED
next
next
'random start and target position
xStart = rndIntBetween(1, MAP_SIZE_X-2)
yStart = rndIntBetween(1, MAP_SIZE_Y-2)
xTarget = rndIntBetween(1, MAP_SIZE_X-2)
yTarget = rndIntBetween(1, MAP_SIZE_Y-2)
map(xStart, yStart) = BOX_START
map(xTarget, yTarget) = BOX_TARGET
cls()
drawMap()
'-------------------------------------------------------------------------------
'init Dijkstra stuff
tSTart = timer()
routeFound = 0
For y = 0 to MAP_SIZE_Y-1
for x = 0 to MAP_SIZE_X-1
if map(x, y) <> BOX_BLOCKED then
dist(x, y) = D_INF
else
dist(x, y) = -D_INF
end if
par(x, y) = type(-1, -1)
next
next
'set start point, add to list
dist(xStart, yStart) = 0
par(xStart, yStart) = type(-1, -1)
dll_insertSorted(pRoot, type(xStart, yStart, 0))
'zolang iets in queue:
'x,y = pop from queue
while(pRoot->pNext)
xCur = pRoot->pNext->value.x
yCur = pRoot->pNext->value.y
curDist = dist(xCur, yCur)
dll_deleteFirst(pRoot)
if (xCur = xTarget) and (yCur = yTarget) then
routeFound = 1
exit while
end if
'debug
map(xCur, yCur) = BOX_VISITED
'drawBox(xCur, yCur)
for iDelta = 0 to allowDir-1
'check not outside map
xNbr = xCur + delta(iDelta).x
if (xNbr < 0) or (xNbr >= MAP_SIZE_X) then continue for
yNbr = yCur + delta(iDelta).y
if (yNbr < 0) or (yNbr >= MAP_SIZE_Y) then continue for
'skip blocked
if dist(xNbr, yNbr) < 0 then continue for
'check diagonal move allowed
if iDelta > 3 then
if (dist(xCur, yNbr) < 0) and (dist(xNbr, yCur) < 0) then continue for
end if
'current dist to start + neighbour moveto cost < neighbour dist to start
if curDist + delta(iDelta).dist < dist(xNbr, yNbr) then
'update neighbour
dist(xNbr, yNbr) = curDist + delta(iDelta).dist
par(xNbr, yNbr) = type(xCur, yCur)
dll_insertSorted(pRoot, type(xNbr, yNbr, dist(xNbr, yNbr) + heuristic2(xNbr, yNbr, xTarget, yTarget))) ' A* implant
end if
next
if (inkey() = "q") then exit while
wend
dll_deleteAll(pRoot)
tEnd = timer() - tStart
map(xStart, yStart) = BOX_START
drawMap()
locate 1,1: print "Time: " & tEnd
print
if (routeFound = 1) then
print "Route found :-)"
'Route for end to start
xCur = xTarget
yCur = yTarget
while (1)
x = par(xCur, yCur).x
y = par(xCur, yCur).y
xCur = x
yCur = y
if ((xCur = xStart) and (yCur = yStart)) then exit while
if (xCur < 0) then print "(xCur < 0)": sleep 10000, 0
if (yCur < 0) then print "(xCur < 0)": sleep 10000, 0
if (xCur >= MAP_SIZE_X-1) then print "(xCur >= MAP_SIZE_X-1)": sleep 10000, 0
if (yCur >= MAP_SIZE_Y-1) then print "(yCur >= MAP_SIZE_Y-1)": sleep 10000, 0
map(xCur, yCur) = BOX_ROUTE
drawBox(xCur, yCur)
Sleep 20,1
wend
else
print "NO route found!"
end if
print
print "Press <any> for next"
print "Press <q> to quit"
key = inkey
while (key ="")
key = inkey
sleep 1, 0
wend
loop until key = "q"