#By jeremydeath at 2009-07-23

@name A* Routefinder
@inputs RefindPath Targ:vector
@outputs Path:array

@persist StartPoint:vector2 EndPoint:vector2
@persist MainEnt:entity

@persist NodeDist PathFound Sqrt2 Height HoloScale:vector Radius CurHeight LastHeight

#The Nodes Master tables
@persist Nodes:array InvertNodes:table
@persist NodesParents:array NodesFCost:array NodesGCost:array NodesHCost:array

#End Node
@persist StartNode:vector2 StartNodeIndex

#Start Node
@persist EndTargetNode:vector2 EndTargetNodeIndex

#Current Node
@persist CurrentNode:vector2 CurrentNodeIndex

#Array deminsions
@persist NodeCountX NodeCountY

#The open list
@persist OpenListNodes:array OpenListParent:array OpenListFCost:array

#The closed list
@persist ClosedListNodes:array ClosedListParent:array ClosedListFCost:array

#looping vars and counters
@persist LoopMain LoopMainCounter

#The finished path
@persist Path:array

@trigger all

interval(100)

if(first()| (RefindPath & ~RefindPath))
{

#Reset everything
PathFound = 0
Nodes = array()
InvertNodes = table()
NodesParents = array()
NodesFCost = array()
NodesGCost = array()
NodesHCost = array()
StartNode = vec2(0,0)
StartNodeIndex = 0
EndTargetNode = vec2(0,0)
EndTargetNodeIndex = 0
CurrentNode = vec2(0,0)
CurrentNodeIndex = 0
NodeCountX = 0
NodeCountY = 0
OpenListNodes = array()
OpenListParent = array()
OpenListFCost = array()
ClosedListNodes = array()
ClosedListParent = array()
ClosedListFCost = array()
LoopMain = 0
LoopMainCounter = 0
Path = array()



MainEnt = entity()

#define the square root of 2
Sqrt2 = sqrt(2)

#define start/end points
StartPoint = vec2(MainEnt:pos())
EndPoint = vec2(0,0)

NodeDist = 100
Radius = 15
HoloScale = vec(NodeDist/30,NodeDist/30,NodeDist/30)

#Switch the end and starting points so that every thing comes out the correct direction
Temp = EndPoint
EndPoint = StartPoint
StartPoint = Temp

#Define the height
Height = MainEnt:pos():z()+5
CurHeight = Height
LastHeight = CurHeight

#fill the node array

#Round the nodes so that they fit with the node table
RoundStart = vec2(round(StartPoint:x()/NodeDist)*NodeDist,round(StartPoint:y()/NodeDist)*NodeDist)
RoundEnd = vec2(round(EndPoint:x()/NodeDist)*NodeDist,round(EndPoint:y()/NodeDist)*NodeDist)
#calculate the difference/NodeDist so that the node table can be filled
RoundDiff = vec2(round((RoundStart:x()-RoundEnd:x())/NodeDist),round((RoundStart:y()-RoundEnd:y())/NodeDist))

#=========== Build the node table ================

#Set the margin
Margin = 6


#Set some varibles to reduce ops
RoundEndX = RoundEnd:x()
RoundEndY = RoundEnd:y()

MinPoint = vec2(min(RoundStart:x(),RoundEnd:x()), min(RoundStart:y(),RoundEnd:y()))-vec2(Margin*NodeDist,Margin*NodeDist)
MaxPoint = vec2(max(RoundStart:x(),RoundEnd:x()), max(RoundStart:y(),RoundEnd:y()))+vec2(Margin*NodeDist,Margin*NodeDist)

print(toString(MinPoint))

NodeCountX = 2*Margin+abs(RoundDiff:x())
NodeCountY = 2*Margin+abs(RoundDiff:y())

if(NodeCountX*NodeCountY > 1048576)
{
print("Error! The search area is too big!")
print("Raise the NodeDist, or lower the margin")
exit()
}

MinPointX = MinPoint:x()
MinPointY = MinPoint:y()

RoundDiffMarg = positive(RoundDiff)+vec2(Margin*2,Margin*2)
RoundDiffXMarg = RoundDiffMarg:x()

X = 0
Y = 0
RoundDifXMarg = RoundDiff:x()+Margin
while( Y <= RoundDiffMarg:y() ){
while( X <= RoundDiffXMarg ){
Nodes:pushVector2( vec2(MinPointX+X*NodeDist,MinPointY+Y*NodeDist) )
X++
}
X = 0
Y++
}





#============ Invert the node table ==============
InvertNodes = invert(Nodes)

#assign start node
StartNode = RoundStart
StartNodeIndex = InvertNodes[toString(StartNode),number]


#assign end node
EndTargetNode = RoundEnd
EndTargetNodeIndex = InvertNodes[toString(EndTargetNode),number]


#Calculation of H is done with the manhattan method
#H = abs(currentX-targetX) + abs(currentY-targetY)
#H = Current:distance(Target)


#Set the start node infromation on the master tables
NodesParents[StartNodeIndex,number] = StartNodeIndex
NodesGCost[StartNodeIndex,number] = 0 #Distance from start along optimal path.
NodesHCost[StartNodeIndex,number] = abs(StartNode:x()-EndTargetNode:x()) + abs(StartNode:y()-EndTargetNode:y())
NodesFCost[StartNodeIndex,number] = NodesHCost[StartNodeIndex,number] #Estimated total distance from start to goal through y.

#push the start node into the open list
OpenListNodes:pushNumber(StartNodeIndex)
OpenListParent:pushNumber(StartNodeIndex) #push yourself as the parent
OpenListFCost:pushNumber(NodesFCost[StartNodeIndex,number])

#Tell it to start the main loop
LoopMain = 1

#=================== Debug =======================
#print(toString(ops()))
#Corners for debug

Corner1 = Nodes[1,vector2]
holoCreate(1,vec(Corner1:x(),Corner1:y(),Height))
holoScale(1,HoloScale)

Corner2 = Nodes[Nodes:count(),vector2]
holoCreate(2,vec(Corner2:x(),Corner2:y(),Height))
holoScale(2,HoloScale)

Corner3 = vec2(Corner1:x(),Corner2:y())
holoCreate(3,vec(Corner3:x(),Corner3:y(),Height))
holoScale(3,HoloScale)

Corner4 = vec2(Corner2:x(),Corner1:y())
holoCreate(4,vec(Corner4:x(),Corner4:y(),Height))
holoScale(4,HoloScale)


holoCreate(5,vec(StartNode:x(),StartNode:y(),Height))
holoColor(5,vec(0,255,0))
holoScale(5,HoloScale)


holoCreate(6,vec(EndTargetNode:x(),EndTargetNode:y(),Height))
holoColor(6,vec(255,0,0))
holoScale(6,HoloScale)

holoCreate(752752,vec(0,0,0))
holoColor(752752,vec(255,255,0))
holoScale(752752,HoloScale)

print(toString(NodeCountX))
print(toString(NodeCountY))

#================= Debug ==========================

exit()
}



#Start the main loop
if(LoopMain == 1 & clk() & OpenListNodes:count() > 0)
{

#Look for the lowest F cost square on the open list. Refer to this as the current node
LowestFIndex = OpenListFCost:minIndex()
CurrentNodeIndex = OpenListNodes[LowestFIndex,number]
CurrentNode = Nodes[CurrentNodeIndex,vector2]

holoPos(752752, vec(CurrentNode:x(),CurrentNode:y(),CurHeight))

#build some data to be used in the future
CurrentNodeGCost = NodesGCost[CurrentNodeIndex,number]
CurrentNodeHCost = NodesHCost[CurrentNodeIndex,number]
CurrentNodeFCost = NodesFCost[CurrentNodeIndex,number]
CurrentNodeParent = OpenListParent:number(LowestFIndex)

#Stop if you Add the target square to the closed list, in which case the optimal path has been found, or
if( CurrentNode == EndTargetNode )
{
PathFound = 1
LoopMain = 0
exit()
}


#Switch it to the closed list.
ClosedListNodes:pushNumber( OpenListNodes[LowestFIndex,number] )
ClosedListParent:pushNumber( OpenListParent[LowestFIndex,number] )
ClosedListFCost:pushNumber( OpenListFCost[LowestFIndex,number] )

#Remove it from the open list
OpenListNodes:removeNumber(LowestFIndex)
OpenListParent:removeNumber(LowestFIndex)
OpenListFCost:removeNumber(LowestFIndex)

#Center Node
#print("Center Node: IDX: "+toString(CurrentNodeIndex)+" Value: "+toString(CurrentNode))

#Find the adjacent squares
AdjacentNodes = array()
#North-West
AdjacentNodes:pushNumber(CurrentNodeIndex-NodeCountX-2)
#North
AdjacentNodes:pushNumber(CurrentNodeIndex-NodeCountX-1)
#North-East
AdjacentNodes:pushNumber(CurrentNodeIndex-NodeCountX)
#East
AdjacentNodes:pushNumber(CurrentNodeIndex+1)
#South-East
AdjacentNodes:pushNumber(CurrentNodeIndex+NodeCountX+2)
#South
AdjacentNodes:pushNumber(CurrentNodeIndex+NodeCountX+1)
#South-West
AdjacentNodes:pushNumber(CurrentNodeIndex+NodeCountX)
#West
AdjacentNodes:pushNumber(CurrentNodeIndex-1)


#For each of the 8 squares adjacent to this current square
AdjStep = 1
while(AdjStep <= AdjacentNodes:count())
{
#If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
CurAdjNodeIndex = AdjacentNodes[AdjStep,number]
CurAdjNode = Nodes[CurAdjNodeIndex,vector2]


#print("Adj Node "+toString(AdjStep)+": IDX: "+toString(CurAdjNodeIndex)+" Value: "+toString(CurAdjNode))

#Check for out-of-bounds nodes
if(AdjStep == 1){
if(CurAdjNode != CurrentNode+vec2(-NodeDist,-NodeDist)){
AdjStep++
continue
}
}
elseif(AdjStep == 2){
if(CurAdjNode != CurrentNode+vec2(0,-NodeDist)){
AdjStep++
continue
}
}
elseif(AdjStep == 3){
if(CurAdjNode != CurrentNode+vec2(NodeDist,-NodeDist)){
AdjStep++
continue}
}
elseif(AdjStep == 4){
if(CurAdjNode != CurrentNode+vec2(NodeDist,0)){
AdjStep++
continue
}
}
elseif(AdjStep == 5){
if(CurAdjNode != CurrentNode+vec2(NodeDist,NodeDist)){
AdjStep++
continue
}
}
elseif(AdjStep == 6){
if(CurAdjNode != CurrentNode+vec2(0,NodeDist)){
AdjStep++
continue
}
}
elseif(AdjStep == 7){
if(CurAdjNode != CurrentNode+vec2(-NodeDist,NodeDist)){
AdjStep++
continue
}
}
elseif(AdjStep == 8){
if(CurAdjNode != CurrentNode+vec2(-NodeDist,0)){
AdjStep++
continue
}
}
#Check Closed List
I = 1
OnClosed = 0
while(I <= ClosedListNodes:count())
{
if(ClosedListNodes[I,number] == CurAdjNodeIndex) {
OnClosed = 1
break
}
I++
}
if(OnClosed == 1)
{
AdjStep++
continue
}

#================== Insert Terain Check's Here =================

#Simple trace ranger check
TempVec = vec(CurrentNode:x(),CurrentNode:y(),CurHeight)

CurHeight = rangerOffset(5000,TempVec+vec(0,0,100),vec(0,0,-1)):position():z()+5

if(abs(LastHeight-CurHeight) > 70) #if there is a big jump, disregard the node
{
AdjStep++
continue
}


CurrentNodePos = vec(CurrentNode:x(),CurrentNode:y(),CurHeight)
CurAdjNodePos = vec(CurAdjNode:x(),CurAdjNode:y(),CurHeight)

RD = rangerOffset(CurrentNodePos,CurAdjNodePos)
if(RD:hit() != 0) #If the ranger hit anything, disregard the node
{
AdjStep++
continue
}

#Check the edges so that the objec will fit


Edge = (((CurAdjNode-CurrentNode)/NodeDist)):normalized()*Radius
Edge = vec2(-Edge:y(),Edge:x())

#print(toString(vec(Edge)+CurrentNodePos)+" "+toString(AdjStep))

RD1Pos = vec(Edge)+CurrentNodePos

RD1 = rangerOffset(vec(Edge)+CurrentNodePos,vec(Edge)+CurAdjNodePos)
RD2 = rangerOffset(vec(-Edge)+CurrentNodePos,vec(-Edge)+CurAdjNodePos)



if(RD1:hit() != 0 | RD2:hit() != 0) #If the rangers hit anything, disregard the node
{
AdjStep++
continue
}

LastHeight = CurHeight

#================== End of Terain Checks =======================

#Update the tentative G score
TentativeGCost = NodesGCost[CurrentNodeIndex,number] + CurrentNode:distance(CurAdjNode)
TentativeIsBetter = 0

#Check the open list
I = 1
OnOpen = 0
while(I <= OpenListNodes:count())
{
if(OpenListNodes[I,number] == CurAdjNodeIndex) {
OnOpen = 1
break
}
I++
}

#If it isnt on the open list...
if(OnOpen != 1) {
#add it to the open list.
OpenListNodes:pushNumber(CurAdjNodeIndex)
OpenListParent:pushNumber(CurrentNodeIndex)


#update the infromation on the master tables
HCost = abs(CurAdjNode:x()-EndTargetNode:x()) + abs(CurAdjNode:y()-EndTargetNode:y())
NodesHCost[CurAdjNodeIndex,number] = HCost
TentativeIsBetter = 1

OpenListFCost:pushNumber(TentativeGCost+HCost)
}
elseif( TentativeGCost < NodesGCost[CurAdjNodeIndex,number] )
{
TentativeIsBetter = 1
}

if( TentativeIsBetter == 1 )
{
NodesParents[CurAdjNodeIndex,number] = CurrentNodeIndex
NodesGCost[CurAdjNodeIndex,number] = TentativeGCost
NodesFCost[CurAdjNodeIndex,number] = NodesHCost[CurAdjNodeIndex,number]+NodesGCost[CurAdjNodeIndex,number]
}

AdjStep++
}


LoopMainCounter++
}
#Stop if you fail to find the target square, and the open list is empty. In this case, there is no path.
elseif(LoopMain == 1 & clk() & OpenListNodes:count() <= 0)
{
PathFound = -1
LoopMain = 0
}
elseif(LoopMain == 0 & clk() & PathFound == 1)
{

#Trace back from the Target square, going through each of its parents
print("Path Found!")

TraceBackCurNodeIndex = EndTargetNodeIndex
TraceBackCurNode = EndTargetNode

while( StartNode != TraceBackCurNode )
{
TraceBackCurNodeIndex = NodesParents[TraceBackCurNodeIndex,number]
TraceBackCurNode = Nodes[TraceBackCurNodeIndex,vector2]

TempVec = vec(TraceBackCurNode:x(),TraceBackCurNode:y(),CurHeight)

CurHeight = rangerOffset(5000,TempVec+vec(0,0,100),vec(0,0,-1)):position():z()+20

Path:pushVector(vec(TraceBackCurNode:x(),TraceBackCurNode:y(),CurHeight))

holoCreate(1000+TraceBackCurNodeIndex,vec(TraceBackCurNode:x(),TraceBackCurNode:y(),CurHeight),HoloScale)
holoColor(1000+TraceBackCurNodeIndex,vec(0,0,255))
}

PathFound = 11

}
elseif(LoopMain == 0 & clk() & PathFound == -1)
{
print("There is no path!!")
}
#[/highlight]


Technical Information:
The H component generation function is actually NOT admissible (meaning that it will not always find the shortest route), but it is so much faster to do it the way that I do it (finding the X distance and then adding in the Y distance) that it outweighs the costs.

The terrain searching code doesn't do well with changes in height, it will try to account for them, but will most likely just ignore any path that slopes up.

You can change the distance of the nodes from one another in the code. This will help if it is exceeding the tick quota.

There is a margin that is generated (indicated by the 4 white holos). You can change how many nodes far out this margin is by changing the Margin varible

By default the target position is set to the point (0,0), but you can change this to whatever you want.

The RefindPath doesn't always work. Mostly this is just because I don't clear the holos when you start over.

If you try to find a path in a grid larger than 1024*1024, it will fail, telling you there is no path. This is because there is a limit on the size of the array functions in E2

There is a part in the Terrain Checks section that tries to account for the radius of the object that you want to navigate through the path by using the rangers on each edge of the imaginary vehicle. You can change this radius in the code.

Have fun with it. :)
