FreeBasic

Пример алгоритма поиска кратчайшего пути.

Платформы: Windows, Linux
Автор: coderJeff

'' ------------------------------------------------------------------
'' A* Demonstration using Manhattan Distance Hueristic
'' by coderJeff - November 2007
'' ------------------------------------------------------------------
''
'' The intent of this demonstration was to provide a sample program that 
'' displays results similar to "A* Pathfinding for Beginners" tutorial 
'' by Patrick Lester, found at:
'' http://www.policyalmanac.org/games/aStarTutorial.htm
''
'' Compiled using FreeBASIC 0.18.3 - however, early versions of FreeBASIC
'' should work.
''
''
'' INSTRUCTIONS
'' ------------
'' LEFT mouse button to set the starting tile
'' RIGHT mouse button to set the ending tile
'' MIDDLE mouse button to toggle the solid tiles
'' ESCAPE key to exit
''
'' ------------------------------------------------------------------

#INCLUDE Once "fbgfx.bi"

Const TRUE = -1
Const FALSE = 0
Const NULL = 0

Const SCREEN_W = 640
Const SCREEN_H = 480

Const CELL_SIZE_W = 64
Const CELL_SIZE_H = 64

Const CELL_W = SCREEN_W \ CELL_SIZE_W
Const CELL_H = SCREEN_H \ CELL_SIZE_H

Const CELL_COUNT = CELL_W * CELL_H

#DEFINE CELLINDEX(x,y) ((CELL_W*(y))+(x))

Const STATE_NONE = 0
Const STATE_OPEN = 1
Const STATE_CLOSED = 2

''
Type Cell
    
    '' Cell Properties
    x As Integer
    y As Integer
    IsSolid As Integer

    '' Information needed for A* computation
    parent As Cell Ptr
    state As Integer
    f As Integer
    g As Integer
    h As Integer
End Type

''
Dim Shared Map( 0 To CELL_COUNT - 1 ) As CELL
Dim Shared StartIndex As Integer
Dim Shared StartCell As Cell Ptr
Dim Shared EndIndex As Integer
Dim Shared EndCell As Cell Ptr
Dim Shared fnt As fb.image Ptr

'' ------------------------------------------------------------------
'' CELLS
'' ------------------------------------------------------------------

''
Sub CellClearAll()
    For y As Integer = 0 To CELL_H - 1
        For x As Integer = 0 To CELL_W - 1
            With Map( CELLINDEX(x,y) )
                .x = x
                .y = y
                .IsSolid  = FALSE
            End With
        Next
    Next
End Sub

''
Sub CellSetSolid( Byval x As Integer, Byval y As Integer, Byval flag As Integer )
    Dim n As Integer = CELLINDEX(x,y)
    Map( n ). IsSolid = flag
End Sub

''
Sub CellSetStart( Byval x As Integer, Byval y As Integer )
    StartIndex = CELLINDEX(x,y)
    StartCell = @Map( StartIndex )
End Sub

''
Sub CellSetEnd( Byval x As Integer, Byval y As Integer )
    EndIndex = CELLINDEX(x,y)
    EndCell = @Map( EndIndex )
End Sub

''
Sub CellToggleSolid( Byval x As Integer, Byval y As Integer )
    With Map( CELLINDEX(x,y) )
        If( .IsSolid ) Then
            .IsSolid = FALSE
        Else
            .IsSolid = TRUE
        End If
    End With
End Sub

'' ------------------------------------------------------------------
'' DISPLAY
'' ------------------------------------------------------------------

''
Function CreateNumberFont() As fb.image Ptr

    Dim fontdata As Zstring Ptr = @ _
    " XXXXX  XX   XXXXX XXXXX X   X XXXXX XXXXX XXXXX XXXXX XXXXX" _
    " X   X   X       X     X X   X X     X         X X   X X   X" _
    " X   X   X   XXXXX XXXXX XXXXX XXXXX XXXXX     X XXXXX XXXXX" _
    " X   X   X   X         X     X     X X   X     X X   X     X" _
    " XXXXX XXXXX XXXXX XXXXX     X XXXXX XXXXX     X XXXXX     X"

    fnt = Imagecreate( 6 * 10, 6 )
    Dim p As Byte Ptr = Cast( Byte Ptr, fnt + 1 )
    p[0] = 0
    p[1] = Asc("0")
    p[2] = Asc("9")
    For i As Integer = 0 To 9
        p[i+3] = 6
    Next
    For y As Integer = 1 To 5
        For x As Integer = 0 To 10 * 6 - 1
            If( fontdata[ (y-1)*10 * 6 + x] = Asc("X") ) Then
                Pset fnt,( x, y ), Rgb( 255, 255, 255 )
            Else
                Pset fnt,( x, y ), Rgb( 255, 0, 255 )
            End If
        Next
    Next

    Function = fnt

End Function

''
Sub DrawCell( Byval x As Integer, Byval y As Integer )

    Dim xx As Integer = x * CELL_SIZE_W
    Dim yy As Integer = y * CELL_SiZE_H
    Dim n As Integer = CELLINDEX(x,y)

    If( Map(n).IsSolid ) Then
        Line( xx, yy ) - ( xx + CELL_SIZE_W - 1, yy + CELL_SIZE_H - 1 ), Rgb(0,0,127), bf
    End If

    If( n = StartIndex ) Then
        Line( xx, yy ) - ( xx + CELL_SIZE_W - 1, yy + CELL_SIZE_H - 1 ), Rgb(0,127,0), bf
    End If

    If( n = EndIndex ) Then
        Line( xx, yy ) - ( xx + CELL_SIZE_W - 1, yy + CELL_SIZE_H - 1 ), Rgb(127,0,0), bf
    End If

    If( Map(n).state <> STATE_NONE ) Then

        If( Map(n).state = STATE_OPEN ) Then
            Line( xx + 1, yy + 1 ) - ( xx + CELL_SIZE_W - 1 - 1, yy + CELL_SIZE_H - 1 - 1), Rgb(0,255,0), b

        Elseif( Map(n).state = STATE_CLOSED ) Then
            Line( xx + 1, yy + 1 ) - ( xx + CELL_SIZE_W - 1 - 1, yy + CELL_SIZE_H - 1 - 1), Rgb(0,255,255), b
            Line( xx + 2, yy + 2 ) - ( xx + CELL_SIZE_W - 1 - 2, yy + CELL_SIZE_H - 1 - 2), Rgb(0,255,255), b
            Line( xx + 3, yy + 3 ) - ( xx + CELL_SIZE_W - 1 - 3, yy + CELL_SIZE_H - 1 - 3), Rgb(0,255,255), b

        End If

        If( Map(n).parent ) Then
            Dim x1 As Integer = xx + CELL_SIZE_W \ 2
            Dim y1 As Integer = yy + CELL_SIZE_H \ 2
            Dim dx As Integer = ( Map(n).parent->x * CELL_SIZE_W + CELL_SIZE_W \ 2 - x1 )
            Dim dy As Integer = ( Map(n).parent->y * CELL_SIZE_H + CELL_SIZE_H \ 2 - y1 )
            Dim nn As Integer = Sqr( dx * dx + dy * dy )
            Dim mm As Integer = CELL_SIZE_W \ 3
            Circle( x1, y1 ), 3, Rgb( 191, 191, 191 )
            Line( x1, y1 ) - ( x1 + dx * mm \ nn, y1 + dy * mm \ nn ), Rgb( 191, 191, 191 )

        End If

        Draw String ( xx + 5, yy + 5 ), Str( Map(n).f ), , fnt
        Draw String ( xx + 5, yy + CELL_SIZE_H - 6 - 5), Str( Map(n).g ), , fnt
        Draw String ( xx + CELL_SIZE_W - 5 - Len( Str( Map(n).h )) * 6, yy + CELL_SIZE_H - 6 - 5), Str( Map(n).h ), , fnt

    End If

    Line( xx, yy ) - ( xx + CELL_SIZE_W - 1, yy + CELL_SIZE_H - 1 ), Rgb(31,31,63), b

End Sub

''
Sub DrawMap()

    For y As Integer = 0 To CELL_H - 1
        For x As Integer = 0 To CELL_W - 1
            DrawCell x, y
        Next
    Next

End Sub

''
Sub DrawPath()

    Dim c As CELL Ptr = EndCell

    While( c->parent )

        Dim x1 As Integer = c->x * CELL_SIZE_W + CELL_SIZE_W \ 2
        Dim y1 As Integer = c->y * CELL_SIZE_H + CELL_SIZE_H \ 2
        Circle( x1, y1 ), 9, Rgb( 191, 0, 0 ),,,,f

        c = c->parent

    Wend

End Sub

'' ------------------------------------------------------------------
'' A* Computations
'' ------------------------------------------------------------------

''
Function ASTAR_GetLowestF( ) As CELL Ptr
    
    Dim c As CELL Ptr = NULL

    For i As Integer = 0 To CELL_COUNT - 1

        If( Map( i ).State = STATE_OPEN ) Then
            If( c = NULL ) Then
                c = @Map(i)
            Else
                If( Map(i).f < c->f ) Then
                    c = @Map(i)
                End If
            End If
        End If

    Next

    Function = c
    
End Function

''
Function ASTAR_CheckNeighbour( Byval parent As CELL Ptr, Byval x As Integer, Byval y As Integer, cost As Integer ) As Integer

    Function = FALSE

    If( x < 0 Or x >= CELL_W ) Then
        Exit Function
    End If

    If( y < 0 Or y >= CELL_H ) Then
        Exit Function
    End If

    Dim c As CELL Ptr = @Map( CELLINDEX(x, y) )

    If( c->IsSolid ) Then
        Exit Function
    End If

    If( c->state = STATE_OPEN ) Then
        If( parent->g + cost < c->g ) Then
            c->state = STATE_NONE
        End If
    
    Elseif( c->state = STATE_CLOSED ) Then
        If( parent->g + cost < c->g ) Then
            c->state = STATE_NONE
        End If
    End If

    If( c->state = STATE_NONE ) Then
        c->state = STATE_OPEN
        c->g = parent->g + cost

        '' This is the Manhattan Distance Heuristic
        c->h = Abs( c->x - EndCell->x ) * 10 + Abs( c->y - EndCell->y ) * 10
        c->f = c->g + c->h
        c->parent = parent

    End If

    Function = TRUE

End Function

''
Function ASTAR_CheckNeighbours( Byval parent As CELL Ptr, Byval x As Integer, Byval y As Integer ) As Integer
    
    Const DIR_N = 1
    Const DIR_S = 2
    Const DIR_W = 4
    Const DIR_E = 8

    Dim flag As Integer

    '' Check all orthogonal directions first N S E W
    If( ASTAR_CheckNeighbour( parent, x - 1, y    , 10 ) ) Then
        flag or= DIR_W
    End If
    If( ASTAR_CheckNeighbour( parent, x    , y - 1, 10 ) ) Then
        flag or= DIR_N
    End If
    If( ASTAR_CheckNeighbour( parent, x    , y + 1, 10 ) ) Then
        flag or= DIR_S
    End If
    If( ASTAR_CheckNeighbour( parent, x + 1, y    , 10 ) ) Then
        flag or= DIR_E
    End If

    '' Only allow a diagonal movement if both orthogonal
    '' directions are also allowed 
    If( ( flag And ( DIR_N Or DIR_W )) = ( DIR_N Or DIR_W ) ) Then
        ASTAR_CheckNeighbour( parent, x - 1, y - 1, 14 )
    End If
    If( ( flag And ( DIR_S Or DIR_W )) = ( DIR_S Or DIR_W ) ) Then
        ASTAR_CheckNeighbour( parent, x - 1, y + 1, 14 )
    End If
    If( ( flag And ( DIR_N Or DIR_E )) = ( DIR_N Or DIR_E ) ) Then
        ASTAR_CheckNeighbour( parent, x + 1, y - 1, 14 )
    End If
    If( ( flag And ( DIR_S Or DIR_E )) = ( DIR_S Or DIR_E ) ) Then
        ASTAR_CheckNeighbour( parent, x + 1, y + 1, 14 )
    End If

    Function = 0

End Function

''
Sub ASTAR_Compute()

    Dim c As CELL Ptr

    '' Clear the A* calculations
    For i As Integer = 0 To CELL_COUNT - 1
        Map(i).parent = NULL
        Map(i).state = STATE_NONE
        Map(i).f = 0
        Map(i).g = 0
        Map(i).h = 0
    Next

    '' Set the starting CELL as the only one in the open set
    c = StartCell
    c->State = STATE_OPEN

    Do

        c = ASTAR_GetLowestF()

        If( c = NULL ) Then
            '' No OPEN cells, just quit
            Exit Do
        Elseif( c = EndCell ) Then
            '' Success
            Exit Do
        End If

        '' Add the current cell to the closed list
        c->state = STATE_CLOSED
        
        '' Add and compute neighbours
        ASTAR_CheckNeighbours( c, c->x, c->y )

    Loop


End Sub

'' ------------------------------------------------------------------
'' MAIN
'' ------------------------------------------------------------------

Dim page As Integer = 0
Dim As Integer mx,my,mz,mb,oldmb,xx,yy


CellClearAll()

'' Initialize the starting conditions
CellSetSolid( 5, 2, TRUE )
CellSetSolid( 5, 3, TRUE )
CellSetSolid( 5, 4, TRUE )

CellSetStart( 3, 3 )
CellSetEnd( 7, 3 )

Screenres SCREEN_W, SCREEN_H, 32, 2

Screenset page, 1-page
page = 1-page

fnt = CreateNumberFont()

Do

    '' Check for input
    oldmb = mb
    Getmouse mx,my,mz,mb

    If( mx >= 0 And my >= 0 ) Then
        xx = mx \ CELL_SIZE_W
        yy = my \ CELL_SIZE_H
        xx = Iif( xx < 0, 0, Iif( xx >= CELL_W, CELL_W - 1, xx ))
        yy = Iif( yy < 0, 0, Iif( yy >= CELL_H, CELL_H - 1, yy ))

        If( ( mb And 1 ) <> 0 And ( oldmb And 1 ) = 0 ) Then
            CellSetStart( xx, yy )
        End If

        If( ( mb And 2 ) <> 0 And ( oldmb And 2 ) = 0 ) Then
            CellSetEnd( xx, yy )
        End If

        If( ( mb And 4 ) <> 0 And ( oldmb And 4 ) = 0 ) Then
            CellToggleSolid( xx, yy )
        End If

    End If

    If( Multikey( fb.sc_escape ) ) Then
        Exit Do
    End If

    '' Do the A* computation
    ASTAR_Compute()

    '' Render the output
    Cls

    DrawMap()
    DrawPath()

    Screenset page, 1-page
    page = 1-page

    Sleep 50,1

Loop

Imagedestroy( fnt )