Make your own free website on Tripod.com
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