DECLARE SUB rotate (dx%, dy%)
DECLARE SUB rotatec (dx%, dy%)
SCREEN 12
'define the scale and sizes
sc% = 10
maxi% = 120
maxj% = 80
DIM map%(200, 100)
DIM cx%(5000)
DIM cy%(5000)
DIM tx%(5000)
DIM ty%(5000)
DIM tempx%(5000)
DIM tempy%(5000)
'initialize map to empty
FOR i% = 0 TO 100
FOR j% = 0 TO 100
map%(i%, j%) = 0
NEXT j%
NEXT i%
'set start location
x% = 10
y% = 10
map%(x%, y%) = 12
'set goal location
ex% = 62
ey% = 27
map%(ex%, ey%) = 13
FOR j% = 5 TO 20
map%(26, j% + 9) = 3
NEXT j%
FOR j% = 5 TO 20
map%(30, j% + 9) = 3
NEXT j%
FOR j% = 5 TO 25
map%(32, j%) = 3
NEXT j%
map%(31, 25) = 3
map%(0, 0) = 3
FOR j% = 5 TO 25
map%(33, j% + 9) = 3
NEXT j%
FOR j% = 5 TO 25
map%(35, j%) = 3
NEXT j%
'put in an object in the way
FOR j% = 5 TO 25
map%(46 + 1, j% * 2) = 3
NEXT j%
FOR j% = 5 TO 25
map%(47 + 1, j% * 2 + 1) = 3
NEXT j%
FOR j% = 5 TO 25
map%(48 + 1, j% * 2) = 3
NEXT j%
'put in an object in the way
'FOR j% = 5 TO 25
' map%(33, j% + 10) = 3
'NEXT j%
'FOR j% = 5 TO 25
' map%(34, j%) = 3
'NEXT j%
'
'FOR j% = 1 TO 10
' map%(35 + j%, 20 + j%) = 3
'NEXT j%
'
'FOR j% = 1 TO 10
' map%(45 - j%, 20 + j%) = 3
'NEXT j%
'draw the map
FOR i% = 0 TO maxi%
FOR j% = 0 TO maxj%
LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), map%(i%, j%), BF
LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), 7, B
NEXT j%
NEXT i%
'line
'walk around objects
'remove excess travel
'trivial case-end is same as start
'DO
'LOOP UNTIL INKEY$ <> ""
count% = 0 'location in cx%...point alont the path
'line drawing algorithm
dx% = ex% - x%
dy% = ey% - y%
IF ABS(dx%) >= ABS(dy%) THEN 'quadrant 0 or 3
xdir% = SGN(dx%)
ydir% = SGN(dy%)
dx% = ABS(dx%)
dy% = ABS(dy%)
dyx2% = dy% * 2
dyx2mdx2% = dyx2% - dx% * 2
er% = dyx2% - dx%
x0% = x%
y0% = y%
IF map%(x0%, y0%) = 0 THEN map%(x0%, y0%) = 1
cx%(count%) = x0%
cy%(count%) = y0%
count% = count% + 1
FOR i% = dx% - 1 TO 0 STEP -1
IF er% >= 0 THEN
y0% = y0% + ydir%
er% = er% + dyx2mdx2%
IF map%(x0%, y0%) = 0 THEN map%(x0%, y0%) = 2
cx%(count%) = x0%
cy%(count%) = y0%
count% = count% + 1
ELSE
er% = er% + dyx2%
END IF
x0% = x0% + xdir%
IF map%(x0%, y0%) = 0 THEN map%(x0%, y0%) = 1
cx%(count%) = x0%
cy%(count%) = y0%
count% = count% + 1
NEXT i%
ELSE
xdir% = SGN(dx%)
ydir% = SGN(dy%)
dx% = ABS(dx%)
dy% = ABS(dy%)
dxx2% = dx% * 2
dxx2mdy2% = dxx2% - dy% * 2
er% = dxx2% - dy%
x0% = x%
y0% = y%
IF map%(x0%, y0%) = 0 THEN map%(x0%, y0%) = 1
cx%(count%) = x0%
cy%(count%) = y0%
count% = count% + 1
FOR i% = dy% - 1 TO 0 STEP -1
IF er% >= 0 THEN
x0% = x0% + xdir%
er% = er% + dxx2mdy2%
IF map%(x0%, y0%) = 0 THEN map%(x0%, y0%) = 2
cx%(count%) = x0%
cy%(count%) = y0%
count% = count% + 1
ELSE
er% = er% + dxx2%
END IF
y0% = y0% + ydir%
IF map%(x0%, y0%) = 0 THEN map%(x0%, y0%) = 1
cx%(count%) = x0%
cy%(count%) = y0%
count% = count% + 1
NEXT i%
END IF
'FOR i% = 0 TO maxi%
' FOR j% = 0 TO maxj%
' LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), map%(i%, j%), BF
' LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), 7, B
' NEXT j%
'NEXT i%
'find path around objects
'***********************************
tc% = 0
dtx% = 0
dty% = 0
half% = 0
block% = 0
i% = 0
DO
i% = i% + 1
IF map%(cx%(i%), cy%(i%)) > 2 THEN
block% = 1
xs% = cx%(i% - 1)
ys% = cy%(i% - 1)
x% = xs%
y% = ys%
LINE ((x% + dx%) * sc%, (y% + dy%) * sc%)-((x% + dx%) * sc% + sc%, (y% + dy%) * sc% + sc%), 4, BF
dx% = cx%(i%) - cx%(i% - 1)
dy% = cy%(i%) - cy%(i% - 1)
dxflag% = 0
WHILE map%(x% + dx%, y% + dy%) > 2
CALL rotate(dx%, dy%)
LINE ((x% + dx%) * sc%, (y% + dy%) * sc%)-((x% + dx%) * sc% + sc%, (y% + dy%) * sc% + sc%), 4, BF
FOR j = 1 TO 1000: NEXT
LINE ((x% + dx%) * sc%, (y% + dy%) * sc%)-((x% + dx%) * sc% + sc%, (y% + dy%) * sc% + sc%), map%(x% + dx%, y% + dy%), BF
LINE ((x% + dx%) * sc%, (y% + dy%) * sc%)-((x% + dx%) * sc% + sc%, (y% + dy%) * sc% + sc%), 7, B
WEND
dxs% = dx%
dys% = dy%
DO
tx%(tc%) = x% + dx%
ty%(tc%) = y% + dy%
tc% = tc% + 1
LOCATE 20, 1
x% = x% + dx%
y% = y% + dy%
'PRINT tc%, x% - xs%, y% - ys%, dtx%, dty%, map%(x%, y%)
'FOR j = 1 TO 10000: NEXT
'DO
'LOOP UNTIL INKEY$ <> ""
IF map%(x%, y%) = 1 OR map%(x%, y%) = 2 THEN
IF (dtx% * dtx% + dty% * dty%) <= (x% - xs%) * (x% - xs%) + (y% - ys%) * (y% - ys%) THEN
' SOUND 1000, .1
half% = tc% - 1
dtx% = ABS(x% - xs%)
dty% = ABS(y% - ys%)
IF map%(x%, y%) = 2 THEN map%(x%, y%) = 1
END IF
END IF
CALL rotatec(dx%, dy%)
CALL rotatec(dx%, dy%)
WHILE map%(x% + dx%, y% + dy%) > 2
CALL rotate(dx%, dy%)
LINE ((x% + dx%) * sc%, (y% + dy%) * sc%)-((x% + dx%) * sc% + sc%, (y% + dy%) * sc% + sc%), 4, BF
' FOR j = 1 TO 1000: NEXT
LINE ((x% + dx%) * sc%, (y% + dy%) * sc%)-((x% + dx%) * sc% + sc%, (y% + dy%) * sc% + sc%), map%(x% + dx%, y% + dy%), BF
LINE ((x% + dx%) * sc%, (y% + dy%) * sc%)-((x% + dx%) * sc% + sc%, (y% + dy%) * sc% + sc%), 7, B
WEND
LOOP UNTIL x% = xs% AND y% = ys% AND dxs% = dx% AND dys% = dy%
last% = 1
ELSE
IF map%(cx%(i%), cy%(i%)) <= 2 THEN last% = 0 'set last marker to available
END IF
IF (tc% - 1) / 2 >= half% AND block% = 1 THEN
j% = i%
erx% = ABS(tx%(half%) - cx%(j%))
ery% = ABS(ty%(half%) - cy%(j%))
WHILE erx% >= ABS(tx%(half%) - cx%(j%)) AND ery% >= ABS(ty%(half%) - cy%(j%))
erx% = ABS(tx%(half%) - cx%(j%))
ery% = ABS(ty%(half%) - cy%(j%))
j% = j% + 1
WEND
FOR p% = j% TO count% - 1
tempx%(p%) = cx%(p%)
tempy%(p%) = cy%(p%)
NEXT p%
FOR p% = 0 TO half%
cx%(p% + i%) = tx%(p%)
cy%(p% + i%) = ty%(p%)
NEXT p%
PRINT j%, i%
FOR p% = j% TO count% - 1
cx%(p% + half% - (j% - i%) + 1) = tempx%(p%)
cy%(p% + half% - (j% - i%) + 1) = tempy%(p%)
NEXT p%
count% = count% + half% - (j% - i%) + 1
block% = 0
i% = i% + half% - (j% - i%) + 1
tc% = 0
dtx% = 0
dty% = 0
half% = 0
END IF
IF (tc% - 1) / 2 < half% AND block% = 1 THEN
FOR k% = 0 TO tc% - 2
tempx%(k%) = tx%(tc% - k% - 2)
tempy%(k%) = ty%(tc% - k% - 2)
NEXT k%
FOR k% = 0 TO tc% - 2
tx%(k%) = tempx%(k%)
ty%(k%) = tempy%(k%)
NEXT k%
half% = tc% - half% - 2
j% = i%
erx% = ABS(tx%(half%) - cx%(j%))
ery% = ABS(ty%(half%) - cy%(j%))
WHILE erx% >= ABS(tx%(half%) - cx%(j%)) AND ery% >= ABS(ty%(half%) - cy%(j%))
erx% = ABS(tx%(half%) - cx%(j%))
ery% = ABS(ty%(half%) - cy%(j%))
j% = j% + 1
WEND
FOR p% = j% TO count% - 1
tempx%(p%) = cx%(p%)
tempy%(p%) = cy%(p%)
NEXT p%
FOR p% = 0 TO half%
cx%(p% + i%) = tx%(p%)
cy%(p% + i%) = ty%(p%)
NEXT p%
PRINT j%, i%
FOR p% = j% TO count% - 1
cx%(p% + half% - (j% - i%) + 1) = tempx%(p%)
cy%(p% + half% - (j% - i%) + 1) = tempy%(p%)
NEXT p%
count% = count% + half% - (j% - i%) + 1
block% = 0
i% = i% + half% - (j% - i%) + 1
tc% = 0
dtx% = 0
dty% = 0
half% = 0
END IF
LOOP UNTIL i% >= count% - 2
FOR i% = 0 TO maxi%
FOR j% = 0 TO maxj%
LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), map%(i%, j%), BF
LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), 7, B
NEXT j%
NEXT i%
'truncate unused portion of path
count% = count% - 1
'walk along path
FOR i% = 0 TO count%
LINE (cx%(i%) * sc%, cy%(i%) * sc%)-(cx%(i%) * sc% + sc%, cy%(i%) * sc% + sc%), 4, BF
now = TIMER
DO
LOOP UNTIL TIMER > now + .1
LINE (cx%(i%) * sc%, cy%(i%) * sc%)-(cx%(i%) * sc% + sc%, cy%(i%) * sc% + sc%), map%(cx%(i%), cy%(i%)), BF
LINE (cx%(i%) * sc%, cy%(i%) * sc%)-(cx%(i%) * sc% + sc%, cy%(i%) * sc% + sc%), 7, B
NEXT i%
SUB rotate (dx%, dy%)
dxo% = dx%
dx% = (.7 * dx% + .7 * dy%)
dy% = (-.7 * dxo% + .7 * dy%)
END SUB
SUB rotatec (dx%, dy%)
dxo% = dx%
dx% = (.7 * dx% + -.7 * dy%)
dy% = (.7 * dxo% + .7 * dy%)
END SUB
