public List<GraphEdge> AStar(Vector3 start, Vector3 end)
{
List<float> gCosts = new List<float>();
List<float> fCosts = new List<float>();
List<GraphEdge> spt = new List<GraphEdge>(); // shortest path tree
List<GraphEdge> searchFrontier = new List<GraphEdge>();
for (int i = 0; i < Nodes.Count; ++i)
{
gCosts.Add(9999);
fCosts.Add(9999);
searchFrontier.Add(new GraphEdge(-1, -1));
}
BinaryHeap pq = new BinaryHeap();
int index = 0;
while (pq.Count() == 0 && index < Nodes.Count)
{
// check if we've found the node we want to start at
#if USE_XZ
if (Nodes[index].GetPosition().x == start.x && Nodes[index].GetPosition().z == start.z)
#else // USE_ZY
if (Nodes[index].GetPosition().x == start.x && Nodes[index].GetPosition().y == start.y)
#endif //USE_XZ
{
pq.Add(0, index); // add it to our pq
gCosts[index] = 0; // set its g cost
fCosts[index] = 0; // set its f cost
searchFrontier[index] = new GraphEdge(index, index); // add it to frontier
}
++index;
}
Vector2 endPosV2;
#if USE_XZ
endPosV2 = new Vector2(end.x, end.z);
#else //USE_XY
endPosV2 = new Vector2(end.x, end.y);
#endif
while (pq.Count() > 0)
{
int closestNode = pq.Values(0);
pq.RemoveAt(0);
if (!spt.Contains(searchFrontier[closestNode]))
spt.Add(searchFrontier[closestNode]); // if I use insert here, then later I can index it instead of searching for it
Vector2 curPositionV2 = new Vector2(Nodes[closestNode].GetPosition().x,
#if USE_XZ
Nodes[closestNode].GetPosition().z);
#else
Nodes[closestNode].GetPosition().y);
#endif
if (curPositionV2 == endPosV2)
{
return spt;
}
GraphNode curNode = Nodes[closestNode];
for (int i = 0; i < curNode.mEdges.GetSize(); ++i)
{
// index of the node that this edge is pointing to
int edgeToIndex = curNode.mEdges.Loc(i).GetToIndex();
float g = gCosts[curNode.GetIndex()] + (curNode.GetPosition() - Nodes[edgeToIndex].GetPosition()).sqrMagnitude;
float h = (end - Nodes[edgeToIndex].GetPosition()).sqrMagnitude;
float f = g + h;
// this is where I would index for the edge
//if (searchFrontier[edgeToIndex].GetToIndex() == -1 &&
// searchFrontier[edgeToIndex].GetFromIndex() == -1)
bool onFrontier = false;
for (int j = 0; j < searchFrontier.Count; +++j)
{
if (searchFrontier[j].GetToIndex() == edgeToIndex)
{
onFrontier = true;
break;
}
}
if (!onFrontier)
{
searchFrontier[edgeToIndex] = curNode.mEdges.Loc(i);
gCosts[edgeToIndex] = g;
fCosts[edgeToIndex] = f;
if (pq.ContainsValue(edgeToIndex))
{
int pqIndex = pq.IndexOfValue(edgeToIndex);
float oldFCost = pq.Keys(pqIndex);
if (f < oldFCost) // if path is shorter
{
pq.RemoveAt(pqIndex);
pq.Add(f, edgeToIndex);
}
}
else
{
pq.Add(f, edgeToIndex);
}
}
else
{
int indexOfEdgeToIndex = pq.IndexOfValue(edgeToIndex);
if (indexOfEdgeToIndex >= 0 && f < pq.Keys(indexOfEdgeToIndex))
{
pq.RemoveAt(indexOfEdgeToIndex);
pq.Add(f, edgeToIndex);
}
}
}
}
//Debug.LogWarning("Path to destination not found");
spt = new List<GraphEdge>(); // clear the list to represent no path found
return spt;
}
