#Region Activity Attributes
#FullScreen: true
#IncludeTitle: True
#End Region
' Pathfinder theory 'A' Star
#Region
' Summary of the A* Method
' Okay, now that you have gone through the explanation, let’s lay out the Step-by-Step method all In one place:
' 1) Add the starting square (OR node) To the open List.
' 2) Repeat the following:
' a) Look For the lowest F cost square on the open List. We refer To this As the current square.
' b) Switch it To the closed List.
' c) For Each of the 8 squares adjacent To this current square …
' (i)If it Is Not walkable OR If it Is on the closed List, ignore it. Otherwise Do the following.
' (ii)If it isn’t on the open List, add it To the open List. Make the current square the parent of this square. Record the F, G, AND H costs of the square.
' (iii)If it Is on the open List already, check To see If this Path To that square Is better, using G cost As the measure.
' A lower G cost means that this Is a better Path. If so, change the parent of the square To the current square,
' AND recalculate the G AND F scores of the square. If you are keeping your open List sorted by F score,
' you may need To resort the List To account For the change.
' d) Stop when you:
' Add the target square To the closed List, In which Case the Path has been found (see note below), OR
' Fail To find the target square, AND the open List Is empty. In this Case, there Is no Path.
' 3) Save the Path. Working backwards from the target square, go from Each square To its parent square Until you reach the starting square. That Is your Path.
#End Region
Sub Process_Globals
'These global variables will be declared once when the application starts.
'These variables can be accessed from all modules.
End Sub
Sub Globals
'These global variables will be redeclared each time the activity is created.
'These variables can only be accessed from this module.
Dim x1,x2,y1,y2 As Int 'Screen coords of start and finish markers
'Define new type to hold info about square
Type PathType(X As Int, Y As Int, F As Int, G As Int, H As Int, ParentX As Int, ParentY As Int)
Dim AdjacentSquare, CurrentSquare As PathType 'Store coords + f,g,h, parentx, parenty values
Dim OpenList, ClosedList As List '2 lists for pathfinding, sort with OpenList.SortType("F", true)
Dim Intv As Int=Main.ZoomFactor*2 'Distance between two test point on path (high odd numbers work well 21+)
'Timing values of Intv for emulator in release mode
'21 works well but can jump too far. 11 secs, 45 iterations, 45 points
'11 way too slow. 293 secs, 299 iterations and points.
'15 aömost perfect. 40 secs, 214 points. Path is good.
'17 is good. 54 secs, 105 points
'16 is not good. Path is wrong. 88 secs, 212 iterations
Dim GValue As Int 'the value to move to this square
Dim cvs As Canvas
Dim ShowCalculation As Boolean=True 'Show the path during calcs - slower (use to debug)
Dim PathFound As Boolean=False
'Variables for image processing
Dim b As Bitmap
Dim iv As ImageView
Dim PixelColorR, PixelColorG, PixelColorB As Int
Dim T2B, L2R As Double 'Difference in GPS coordinates N E S W
Dim MyIcon As BitmapDrawable
End Sub
Sub Activity_Create(FirstTime As Boolean)
'Do not forget to load the layout file created with the visual designer. For example:
'Activity.LoadLayout("Layout1")
'Load screenshot into imageview
iv.Initialize("")
Activity.AddView(iv,0,0,100%x,100%Y)
b.Initialize(File.DirRootExternal,Main.dt & ".png")
iv.Bitmap=b
MyIcon.Initialize(LoadBitmap(File.DirAssets, "pathicon.png"))
cvs.Initialize(iv) 'Add canvas to imageview, here we will draw the route
'cvs.Initialize(Activity)
OpenList.Initialize
ClosedList.Initialize
GetCoordinatesOfMarkers 'Find start and end markers
cvs.DrawCircle(x1,y1,4,Colors.red,True,2) 'Draw start point
cvs.DrawCircle(x2,y2,4,Colors.red,True,2) 'Draw finish point
Activity.Invalidate
Activity.Title="Pathfinder: Calculating route ..."
BeginPathfinder
Activity.Title="Pathfinder: Navigation complete."
'plot route
If PathFound=True Then
Dim i, px, py As Int
Dim TestPoint, OldPoint As PathType 'reset variable
OldPoint=ClosedList.Get(ClosedList.Size-1) 'Get first point
cvs.DrawCircle(OldPoint.ParentX,OldPoint.ParentY,4,Colors.red,False,2)
Do While px<>x1 AND py<>y1
px=OldPoint.ParentX
py=OldPoint.ParentY
For i=ClosedList.Size-1 To 1 Step -1
Dim TestPoint As PathType 'reset variable
TestPoint=ClosedList.Get(i)
If TestPoint.X=px AND TestPoint.Y=py Then 'Search for child
'Generate the markers
ConvertPathToMarkers(TestPoint)
'Draw marker on map
cvs.DrawCircle(TestPoint.ParentX,TestPoint.ParentY,4,Colors.red,False,2)
' Log("X: "& TestPoint.X & ",Y: " & TestPoint.Y & ",F: " & TestPoint.F & ",G: " & TestPoint.G & ",H: " & TestPoint.H & _
' ",Parent X: " & TestPoint.ParentX & ",Parent Y: " & TestPoint.ParentY)
Exit
End If
Next
OldPoint=TestPoint
Loop
End If
'WriteClosedList 'Remove after debugging
Activity.Invalidate
Msgbox("Navigation complete","")
Activity.Finish
End Sub
'Output closed list to file - used for debugging
Sub WriteClosedList
Dim Writer As TextWriter
Writer.Initialize(File.OpenOutput(File.DirRootExternal, "ClosedList.txt", False))
Dim i As Int
For i=ClosedList.Size-1 To 0 Step -1
Dim TestPoint As PathType 'reset variable
TestPoint=ClosedList.Get(i)
Writer.Write("X,"& TestPoint.X & ",Y," & TestPoint.Y & ",F," & TestPoint.F & ",G," & TestPoint.G & ",H," & TestPoint.H & _
",Parent X," & TestPoint.ParentX & ",Parent Y," & TestPoint.ParentY & CRLF)
Next
Writer.Close
End Sub
Sub ConvertPathToMarkers(TestP As PathType)
Dim Geopoint1 As GeoPoint
Dim MyMarker As Marker
Dim MyPoint As PathType 'reset variable
Dim mLat, mLon As Double
MyPoint=TestP
'Log("*************************")
'Log("Generating marker geopoints:")
mLon=(MyPoint.X*L2R/Main.ActWidth) + Main.NordValues(3)
mLat=Main.NordValues(0)-(MyPoint.Y * T2B/Main.ActHeight)
Geopoint1.Initialize(mLat,mLon)
'Log("Point X: " & MyPoint.X)
'Log("Point Y: " & MyPoint.Y)
'Log("Calc Long: " & Geopoint1.Longitude)
'Log("Calc Lat.: " & Geopoint1.Latitude)
'Log("*************************")
MyMarker.Initialize3("My Route", "Route Point", Geopoint1, MyIcon)
Main.Markers.Add(MyMarker)
End Sub
Sub BeginPathfinder
'Here we start pathfinding
'Define starting point and add start to open list
Public StartTime, EndTime, MilliSeconds As Long
Dim count As Int=1
StartTime = DateTime.Now
'This is our starting point
CurrentSquare.X=x1
CurrentSquare.Y=y1
CurrentSquare.F=0
CurrentSquare.G=0
CurrentSquare.H=0
CurrentSquare.ParentX=x1
CurrentSquare.ParentY=y1
AddToOpenList(CurrentSquare) 'No need to check this square
Do While OpenList.Size>0
CurrentSquare=OpenList.Get(0) 'Get lowest F coordinate from OpenList
AddToClosedList(CurrentSquare)
'Are we near the finish?
If Abs(CurrentSquare.X-x2)<Intv AND Abs(CurrentSquare.Y-y2)<Intv Then
PathFound=True
Exit
End If
'Add the 8 neighbours
AddReachable(CurrentSquare.X,CurrentSquare.Y)
count=count+1
Loop
If PathFound=True Then
Log("We are at destination")
Log("Path from:" & x1 & "," & y1 & " to " & x2 & "," & y2)
Log("Using an interval of " & Intv)
EndTime = DateTime.Now
MilliSeconds = EndTime - StartTime
Log("Total Time: " & (MilliSeconds/1000) & " secs")
Log("iterations: " & count)
Log("Closed List has " & ClosedList.Size & " points")
Log("Open List has " & OpenList.Size & " points")
Else
Log("Path not found")
End If
End Sub
'Add surrounding squares in a square form
Sub AddReachable(x As Int, y As Int)
'Add all 8 surrounding squares to current square (x,y)
Dim i As Int
'G is path to point from start. Horizontal and vertical border points are 10, diagonals are 14
'H is the Manhattan distance to the finish point (x2,y2)
'F=G+H
'For i=-Intv To Intv Step Intv 'for 8 squares
'For i=-Intv To Intv Step Intv/2 '16 boundary squares
For i=-Intv To Intv Step Intv/3 '24 boundary squares
'Top
Dim AdjacentSquare As PathType 'Need to redim to clear the old values!
AdjacentSquare.x=x+i
AdjacentSquare.y=y-Intv
AdjacentSquare.H=CalcHValue(x,y)
GValue=CalcGValue(AdjacentSquare,x,y)
AdjacentSquare.G=CurrentSquare.G+GValue
AdjacentSquare.F=AdjacentSquare.G+AdjacentSquare.H
AdjacentSquare.ParentX=CurrentSquare.ParentX
AdjacentSquare.ParentY=CurrentSquare.ParentY
TestSquare(AdjacentSquare.x,AdjacentSquare.y)
'Bottom
Dim AdjacentSquare As PathType 'Need to redim to clear the old values!
AdjacentSquare.x=x+i
AdjacentSquare.y=y+Intv
AdjacentSquare.H=CalcHValue(x,y)
GValue=CalcGValue(AdjacentSquare,x,y)
AdjacentSquare.G=CurrentSquare.G+GValue
AdjacentSquare.F=AdjacentSquare.G+AdjacentSquare.H
AdjacentSquare.ParentX=CurrentSquare.ParentX
AdjacentSquare.ParentY=CurrentSquare.ParentY
TestSquare(AdjacentSquare.x,AdjacentSquare.y)
'Left
Dim AdjacentSquare As PathType 'Need to redim to clear the old values!
AdjacentSquare.x=x-Intv
AdjacentSquare.y=y+i
AdjacentSquare.H=CalcHValue(x,y)
GValue=CalcGValue(AdjacentSquare,x,y)
AdjacentSquare.G=CurrentSquare.G+GValue
AdjacentSquare.F=AdjacentSquare.G+AdjacentSquare.H
AdjacentSquare.ParentX=CurrentSquare.ParentX
AdjacentSquare.ParentY=CurrentSquare.ParentY
TestSquare(AdjacentSquare.x,AdjacentSquare.y)
'Right
Dim AdjacentSquare As PathType 'Need to redim to clear the old values!
AdjacentSquare.x=x+Intv
AdjacentSquare.y=y-i
AdjacentSquare.H=CalcHValue(x,y)
GValue=CalcGValue(AdjacentSquare,x,y)
AdjacentSquare.G=CurrentSquare.G+GValue
AdjacentSquare.F=AdjacentSquare.G+AdjacentSquare.H
AdjacentSquare.ParentX=CurrentSquare.ParentX
AdjacentSquare.ParentY=CurrentSquare.ParentY
TestSquare(AdjacentSquare.x,AdjacentSquare.y)
Next
End Sub
Sub CalcHValue(xx As Int, yy As Int) As Double
' Return Sqrt(Power(x2-xx,2)+Power(y2-yy,2))*10 'Euclidean distance Careful, can give a bad path!!
Return (Abs(x2-xx)+Abs(y2-yy))*10 'the Manhattan distance
End Sub
Sub CalcGValue(cSquare As PathType, xx As Int, yy As Int) As Double
' If cSquare.X=xx OR cSquare.Y=yy Then 'If x or y coordinate is same as parent then set value 10
' Return 10 'horizontal or vertical path
' Else
' Return 14 'diagonal path
' End If
Return Sqrt(Power(cSquare.X-xx,2)+Power(cSquare.Y-yy,2))
End Sub
'Check if surrounding square can be added!
Sub TestSquare(x As Int, y As Int)
'x & y refer to the adjacent square coordinates!
Dim C As Int
' Check to see if we are out of bounds
If x<0 OR y<0 Then Return 'Stay in screen
If x>b.Width-1 OR y>b.Height-1 Then Return 'Stay in bitmap
' Get the pixel colors
GetRedPixel(x,y)
' **************************************************************************
'Start testing
' **************************************************************************
' (i) If it is not walkable, ignore it.
' **************************************************************************
#Region
'Square is not a river, do nothing
'Test for river borders
'If PixelColorR=239 Then
If PixelColorR>200 Then
Return
End If
If PixelColorR=181 AND (PixelColorG>200 OR PixelColorG>200) Then 'Rivers in Mapnik have this red color
'do nothing 'Samsung Tab has 181,208,208 for rivers in Mapnik
Else
Return
End If
#End Region
' **************************************************************************
' (i) If it Is on the closed List, ignore it. ***Done
#Region
For C=0 To ClosedList.Size-1
Dim TestPoint As PathType 'reset
TestPoint=ClosedList.Get(C)
' If AdjacentSquare.x=TestPoint.x AND AdjacentSquare.y=TestPoint.y Then
If x=TestPoint.x AND y=TestPoint.y Then
Return 'Already on the ClosedList
End If
Next
#End Region
' **************************************************************************
' (iii) If it is on the open List, .
#Region
For C=0 To OpenList.Size-1
Dim TestPoint As PathType 'reset
TestPoint=OpenList.Get(C)
If x=TestPoint.x AND y=TestPoint.y Then 'Already on the OpenList
If AdjacentSquare.G<TestPoint.G Then 'Is new G value less than old value?
TestPoint.ParentX=CurrentSquare.ParentX 'If yes, then change parents
TestPoint.ParentY=CurrentSquare.ParentY
TestPoint.G=AdjacentSquare.G
TestPoint.H=CalcHValue(CurrentSquare.ParentX,CurrentSquare.ParentY)
TestPoint.F=TestPoint.G+TestPoint.H
OpenList.Set(C,TestPoint) 'Save changes
OpenList.SortType("F",True) 'Sort list according to F, smallest first.
End If
Return
End If
Next
#End Region
' **************************************************************************
' (ii)its not on the open list then add it To the open List ***Done
#Region
'Calculate values and add to OpenList
AdjacentSquare.ParentX=CurrentSquare.x
AdjacentSquare.ParentY=CurrentSquare.y
If ShowCalculation=True Then
cvs.DrawCircle(x,y,2,Colors.blue,True,2) 'Speed up by commenting out these three lines
Activity.Invalidate
DoEvents
'Log("Added: " & x & "," & y)
End If
AddToOpenList(AdjacentSquare)
#End Region
End Sub
Sub AddToOpenList(Node As PathType)
OpenList.Add(Node)
OpenList.SortType("F",True) 'Sort list according to F, smallest first.
End Sub
Sub AddToClosedList(Node As PathType)
ClosedList.Add(Node)
OpenList.RemoveAt(0) 'Remove it from open list
End Sub
Sub GetCoordinatesOfMarkers
T2B=Main.NordValues(0)-Main.NordValues(2) 'N - S
L2R=Main.NordValues(1)-Main.NordValues(3) 'E - W
'Get screen coordinres of first marker (Lat are horizontal)
x1=(Main.FirstPointLon-Main.NordValues(3))*Main.ActWidth/L2R
y1=Main.ActHeight-((Main.FirstPointLat-Main.NordValues(2))*Main.ActHeight/T2B)
x2=(Main.SecondPointLon-Main.NordValues(3))*Main.ActWidth/L2R
y2=Main.ActHeight-((Main.SecondPointLat-Main.NordValues(2))*Main.ActHeight/T2B)
Log("Point 1:" & x1 & "," & y1)
Log("Point 2:" & x2 & "," & y2)
End Sub
'Get rgb components of pixel color
Sub GetRedPixel(x As Int, y As Int) 'As Int
'Dim res(4) As Int
Dim PixelColor As Int
PixelColor=b.GetPixel(x,y) 'Returns a long integer value
PixelColorR = Bit.UnsignedShiftRight(Bit.AND(PixelColor, 0xff0000), 16)
PixelColorG = Bit.UnsignedShiftRight(Bit.AND(PixelColor, 0xff00), 8)
PixelColorB = Bit.AND(PixelColor, 0xff)
'Log("Pixelcolor RGB: " & PixelColorR & ", " & PixelColorG & ", " & PixelColorB)
End Sub
Sub Activity_Resume
End Sub
Sub Activity_Pause (UserClosed As Boolean)
End Sub
' subs not used
'Add surrounding squares in a circular form
Sub AddReachable2(x As Int, y As Int)
Dim Angle, NumberOfPoints As Int
NumberOfPoints=12
For Angle = -180 To 180 Step 360/NumberOfPoints
Dim AdjacentSquare As PathType 'Need to redim to clear the old values!
AdjacentSquare.x=x + Cos(Angle) * Intv
AdjacentSquare.y=y + Sin(Angle) * Intv
AdjacentSquare.H=CalcHValue(x,y)
GValue=CalcGValue(CurrentSquare,x,y)
AdjacentSquare.G=CurrentSquare.G+GValue
AdjacentSquare.F=AdjacentSquare.G+AdjacentSquare.H
AdjacentSquare.ParentX=CurrentSquare.ParentX
AdjacentSquare.ParentY=CurrentSquare.ParentY
TestSquare(AdjacentSquare.x,AdjacentSquare.y)
Next
End Sub
