public bool init(IAStarGraph graph)
{
if (graph == null)
{
return(false);
}
mGraph = graph;
mFValues = new float[mGraph.getCellCount()];
mGValues = new float[mGraph.getCellCount()];
mOpenTable = new uint[mGraph.getCellCount()];
mCloseTable = new uint[mGraph.getCellCount()];
mShortestPathTree = new uint[mGraph.getCellCount()];
for (int i = 0; i < mGraph.getCellCount(); i++)
{
mFValues[i] = 0.0f;
mGValues[i] = 0.0f;
mOpenTable[i] = 0;
mCloseTable[i] = 0;
mShortestPathTree[i] = 0;
}
mPriorityQueue.Init(mFValues);
mSessionId = 0;
return(true);
}
public bool findPath(Vector2f startPosition,
Vector2f targetPosition,
out List <Vector2f> path)
{
path = new List <Vector2f>();
if (mGraph == null)
{
return(false);
}
if (mSessionId == 0xFFFFFFFF)
{
reset();
}
mSessionId++;
mPriorityQueue.Reset();
uint startCellIndex = mGraph.getCellIndex(targetPosition);
uint endCellIndex = mGraph.getCellIndex(startPosition);
if (startCellIndex == (uint)PAHT_FINDER_ENUM.INVAILD_INDEX || endCellIndex == (uint)PAHT_FINDER_ENUM.INVAILD_INDEX)
{
return(false);
}
if (startCellIndex == endCellIndex)
{
path.Add(startPosition);
path.Add(targetPosition);
return(true);
}
if (!mGraph.sameArea(startCellIndex, endCellIndex))
{
return(false);
}
mShortestPathTree[(int)startCellIndex] = (uint)PAHT_FINDER_ENUM.INVAILD_INDEX;
mGValues[(int)startCellIndex] = 0;
mFValues[(int)startCellIndex] = 0;
mOpenTable[(int)startCellIndex] = mSessionId;
mPriorityQueue.insert((int)startCellIndex);
while (!mPriorityQueue.empty())
{
uint currentIndex = (uint)mPriorityQueue.Pop();
mCloseTable[(int)currentIndex] = mSessionId;
if (currentIndex == endCellIndex)
{
List <uint> cells = new List <uint>();
uint cellIndex = (uint)currentIndex;
uint maxCycleTime = mGraph.getCellCount();
for (uint i = 0; i < maxCycleTime; i++)
{
cells.Add(cellIndex);
if (mShortestPathTree[(int)cellIndex] == (uint)PAHT_FINDER_ENUM.INVAILD_INDEX)
{
return(mGraph.smoothPath(startPosition, targetPosition, cells, ref path));
}
else
{
cellIndex = mShortestPathTree[(int)cellIndex];
}
}
return(false);
}
else
{
List <uint> adjanceCells = mGraph.getAdjanceCells(currentIndex);
for (int i = 0; adjanceCells != null && i < adjanceCells.Count; i++)
{
uint adjanceIndex = adjanceCells[i];
if (mShortestPathTree[(int)currentIndex] == adjanceIndex)
{
continue;
}
float hValue = mGraph.getHValue(adjanceIndex, startPosition);
float gValue = mGValues[(int)currentIndex] + mGraph.getGValue(mShortestPathTree[(int)currentIndex], currentIndex, adjanceIndex);
if (mOpenTable[(int)adjanceIndex] != mSessionId)
{
mGValues[(int)adjanceIndex] = gValue;
mFValues[(int)adjanceIndex] = gValue + hValue;
mShortestPathTree[(int)adjanceIndex] = currentIndex;
mOpenTable[(int)adjanceIndex] = mSessionId;
mPriorityQueue.insert((int)adjanceIndex);
}
else if (mCloseTable[(int)adjanceIndex] != mSessionId && gValue < mGValues[(int)adjanceIndex])
{
mGValues[(int)adjanceIndex] = gValue;
mFValues[(int)adjanceIndex] = gValue + hValue;
mShortestPathTree[(int)adjanceIndex] = currentIndex;
mPriorityQueue.ChangePriority((int)adjanceIndex);
}
}
}
}
return(false);
}
