public CPathAStar()
{
// Make all the nodes that we will use, and put them on the unused list
for (uint i = 0; i < MAX_NODES; i++)
{
CAStarNode pNewNode = new CAStarNode();
pNewNode.SetpNext(m_UnusedNodes.GetpNext());
m_UnusedNodes.SetpNext(pNewNode);
}
}
public void Reset()
{
m_EstimatedCostToDestH = 0;
m_TotalCostForThisNodeF = 0;
m_pCurWayPoint = null;
m_CurWayPointIndex = 0;
m_pParent = null;
m_pNext = null;
m_AccumulatedCostFromStartG = UNSET_NODE_VALUE;
}
CAStarNode FindInClosedList(uint WayPointIndex)
{
CAStarNode pNodeToCheck = m_ClosedNodes.GetpNext();
while (pNodeToCheck != null)
{
if (pNodeToCheck.GetCurWayPointIndex() == WayPointIndex)
{
return(pNodeToCheck);
}
pNodeToCheck = pNodeToCheck.GetpNext();
}
return(null);
}
CAStarNode FindInOpenList(uint WayPointIndex)
{
CAStarNode pNodeToCheck = m_OpenNodes.GetpNext();
while (pNodeToCheck != nul)
{
l
{
if (pNodeToCheck.GetCurWayPointIndex() == WayPointIndex)
{
return(pNodeToCheck);
}
pNodeToCheck = pNodeToCheck.GetpNext();
}
}
return(null);
}
void RemoveFromOpenList(CAStarNode pNodeToRemove)
{
CAStarNode pParentNode = m_OpenNodes;
CAStarNode pNodeToCheck = m_OpenNodes.GetpNext();
while (pNodeToCheck != null)
{
if (pNodeToRemove == pNodeToCheck)
{
pParentNode.SetpNext(pNodeToCheck.GetpNext());
pNodeToCheck.SetpNext(null);
return;
}
pParentNode = pNodeToCheck;
pNodeToCheck = pNodeToCheck.GetpNext();
}
//assert(0); // why are trying to remove an item that is not in the list
}
public void Initialize(CAStarNode pParentNode, CPathLineSegment pConnectingSegment,
CPathWayPoint pCurWayPoint, uint CurWayPointIndex,
CPathWayPoint pFinalDestWayPoint, double AccumulatedCostFromStartG,
ESTIMATE_ADDITIONAL_COST_FUNC_PTR AdditionalCostFunc, double CollisionCostRatio, ESTIMATE_COST_TO_DEST_FUNC_PTR EstimateCostToDestFunc)
{
// you shouldn't change this node unless the change is better
//assert(m_AccumulatedCostFromStartG >= AccumulatedCostFromStartG);
m_AccumulatedCostFromStartG = AccumulatedCostFromStartG;
if (pParentNode == null && pConnectingSegment == null)
{
m_AccumulatedCostFromStartG += AdditionalCostFunc(pParentNode, pConnectingSegment, CollisionCostRatio);
}
m_EstimatedCostToDestH = EstimateCostToDestFunc(pCurWayPoint, pFinalDestWayPoint);
m_TotalCostForThisNodeF = m_AccumulatedCostFromStartG + m_EstimatedCostToDestH;
//assert(pCurWayPoint.GetPosition().x < 1000000 && pCurWayPoint.GetPosition().x > -1000000);
//assert(pCurWayPoint.GetPosition().y < 1000000 && pCurWayPoint.GetPosition().y > -1000000);
m_pCurWayPoint = pCurWayPoint;
m_CurWayPointIndex = CurWayPointIndex;
m_pParent = pParentNode;
}
void AddToOpenList(CAStarNode pNewNode)
{
CAStarNode pParentNode = m_OpenNodes;
CAStarNode pNodeToCheck = m_OpenNodes.GetpNext();
while (pNodeToCheck != null)
{
if (pNewNode.GetTotalCostF() < pNodeToCheck.GetTotalCostF())
{
pParentNode.SetpNext(pNewNode);
pNewNode.SetpNext(pNodeToCheck);
return;
}
pParentNode = pNodeToCheck;
pNodeToCheck = pNodeToCheck.GetpNext();
}
// it is the worst one in the list put it at the end
//assert(pParentNode.GetpNext() == null);
pParentNode.SetpNext(pNewNode);
//assert(pNewNode.GetpNext() == null);
}
public void SetpNext(CAStarNode pNewNext) { m_pNext = pNewNext; }
void ReturnAllNodesToUnusedList(CAStarNode pParentOfUnusedNode)
{
//AssertNoLostNodes();
CAStarNode pFirstNodeToReturn = pParentOfUnusedNode.GetpNext();
if (pFirstNodeToReturn != null)
{
CAStarNode pLastNodeToReturn = pFirstNodeToReturn;
// if the unused list has stuff in it already
if (m_UnusedNodes.GetpNext())
{
// find the last node we want to return
while (pLastNodeToReturn.GetpNext())
{
pLastNodeToReturn = pLastNodeToReturn.GetpNext();
}
// set the last node to return next to the first node of the unused list
pLastNodeToReturn.SetpNext(m_UnusedNodes.GetpNext());
// set the first node of the unused list to the first node to return
m_UnusedNodes.SetpNext(pFirstNodeToReturn);
// make sure the list we took them from doesn't still have them
pParentOfUnusedNode.SetpNext(null);
//AssertNoLostNodes;
}
else
{
// just put it on the unused list
m_UnusedNodes.SetpNext(pFirstNodeToReturn);
pParentOfUnusedNode.SetpNext(null);
//AssertNoLostNodes;
}
}
//AssertNoLostNodes;
}
bool FindPath(CLayerPart pActor, CWayPointMap pWayPointMap, uint StartWayPointIndex, uint EndWayPointIndex,
CPointList pFinalList, CCollideProcessingState pCollideProcessingState)
{
// here's the way it works
// first we set the open and closed lists to nothing
ReturnAllNodesToUnusedList(m_OpenNodes);
ReturnAllNodesToUnusedList(m_ClosedNodes);
//AssertNoLostNodes;
// we add the StartWayPointIndex to the Open list
CPathWayPoint pStartWayPoint = pWayPointMap.GetpWayPoint(StartWayPointIndex);
CPathWayPoint pFinalDestWayPoint = pWayPointMap.GetpWayPoint(EndWayPointIndex);
CAStarNode pNewNode = GetUnusedNode();
pNewNode.Initialize(null, null, pStartWayPoint, StartWayPointIndex, pFinalDestWayPoint, 0, SegmentLengthFunc, CCollideOutputs::MIN_COST_RATIO_TO_TREAT_AS_IMPASSABLE, UclidianDistFunc);
// put our first point in the open list
m_OpenNodes.SetpNext(pNewNode);
//AssertNoLostNodes;
// we processe the open list until we fill up the open list, the closed list, or we find the end
// if we found the end we return true, else we return false
double RadiusOfDestPosSqr = pCollideProcessingState.GetRadiusOfDestPos();
RadiusOfDestPosSqr = RadiusOfDestPosSqr * RadiusOfDestPosSqr;
double OneOverYToXAspectRatioOfDestPosRadius = 1.f / pCollideProcessingState.GetYToXAspectRatioOfDestPosRadius();
CAStarNode pBestNode = null;
bool Done = false;
while (!Done)
{
// we look in the open list and find the node that has the lowest m_TotalCostForThisNodeF
pBestNode = FindBestNode();
if (pBestNode != null)
{
//AssertNoLostNodes;
CPathWayPoint pCurBestWayPoint = pBestNode.GetpCurWayPoint();
bool CloseEnough = false;
if (RadiusOfDestPosSqr > 0.f)
{
CFPoint FFinalDestPos, FCurBestPos;
FFinalDestPos.Set(pFinalDestWayPoint.GetPosition());
FCurBestPos.Set(pCurBestWayPoint.GetPosition());
double DistSqrd = GetDistSqrd(FFinalDestPos, FCurBestPos, OneOverYToXAspectRatioOfDestPosRadius);
if (DistSqrd < RadiusOfDestPosSqr)
{
CloseEnough = true;
}
}
// now that we have the best node lets see if it's the end
if (pCurBestWayPoint == pFinalDestWayPoint || CloseEnough)
{
Done = true;
}
else
{
// Process all the nodes that we can get to from this node (the best one).
// We will look at all the sub-waypoints of this one and check each
// against the open list and the closed list.
// If we find the node in the open list we check its m_TotalCostForThisNodeF if this one
// is lower than this path is better and we hook that node back to this one if not we move on.
// If we find it in the closed list we will not do anything with it.
// If we don't find it in either then we will add it to the open list and move on to the next one.
//AssertNoLostNodes;
if (!AddSubWayPointsToOpenList(pActor, pWayPointMap, pBestNode, pFinalDestWayPoint, pCollideProcessingState))
{
// put the node we just tried on the closed list
AddNodeToClosedList(pBestNode);
// We ran out of space on the open list. We are done, there is no more space in the open list
//Show something on screen to indicate we failed
// to find a path due to running out of points (search went on to long)
// Commented out by JCS 6-30-01 because it shows up all the time even in non-bad cases
//pTheGame.ShowWarningMessage("CPathAStar::FindPath() failed to find a path (Ran out of points)... This is probably what is slowing things down...");
return(false);
}
// We have arrived at this way point through a minimum path and
// added all its possible paths to the open list, it is done, add it to the closed list.
//AssertNoLostNodes;
AddNodeToClosedList(pBestNode);
//AssertNoLostNodes;
}
}
else
{
// we could not find a best node because there are no more open points
// the path has no solution
return(false);
}
}
// So we are done and found a path to the dest.
// Add all the positions to the pFinalList and return true
// the list goes from the end to the start, so we need to add the list in the reverse order.
NodeList.Clear();
// store each pointer
do
{
NodeList.Add(pBestNode);
pBestNode = pBestNode.GetpParentNode();
} while (pBestNode != null);
// and add them from start to end (they were backwards)
uint NumNodes = NodeList.GetNumItems();
for (int i = NumNodes - 1; i >= 0; i--)
{
pFinalList.AddByVal(NodeList.GetItem(i).GetpCurWayPoint().GetPosition());
}
return(true);
}
public CPathAStar()
{
// Make all the nodes that we will use, and put them on the unused list
for (uint i = 0; i < MAX_NODES; i++)
{
CAStarNode pNewNode = new CAStarNode();
pNewNode.SetpNext(m_UnusedNodes.GetpNext());
m_UnusedNodes.SetpNext(pNewNode);
}
}
public void SetpNext(CAStarNode pNewNext)
{
m_pNext = pNewNext;
}
void AddToOpenList(CAStarNode pNewNode)
{
CAStarNode pParentNode = m_OpenNodes;
CAStarNode pNodeToCheck = m_OpenNodes.GetpNext();
while (pNodeToCheck != null)
{
if (pNewNode.GetTotalCostF() < pNodeToCheck.GetTotalCostF())
{
pParentNode.SetpNext(pNewNode);
pNewNode.SetpNext(pNodeToCheck);
return;
}
pParentNode = pNodeToCheck;
pNodeToCheck = pNodeToCheck.GetpNext();
}
// it is the worst one in the list put it at the end
//assert(pParentNode.GetpNext() == null);
pParentNode.SetpNext(pNewNode);
//assert(pNewNode.GetpNext() == null);
}
void RemoveFromOpenList(CAStarNode pNodeToRemove)
{
CAStarNode pParentNode = m_OpenNodes;
CAStarNode pNodeToCheck = m_OpenNodes.GetpNext();
while (pNodeToCheck != null)
{
if (pNodeToRemove == pNodeToCheck)
{
pParentNode.SetpNext(pNodeToCheck.GetpNext());
pNodeToCheck.SetpNext(null);
return;
}
pParentNode = pNodeToCheck;
pNodeToCheck = pNodeToCheck.GetpNext();
}
//assert(0); // why are trying to remove an item that is not in the list
}
public void Initialize(CAStarNode pParentNode, CPathLineSegment pConnectingSegment,
CPathWayPoint pCurWayPoint, uint CurWayPointIndex,
CPathWayPoint pFinalDestWayPoint, double AccumulatedCostFromStartG,
ESTIMATE_ADDITIONAL_COST_FUNC_PTR AdditionalCostFunc, double CollisionCostRatio, ESTIMATE_COST_TO_DEST_FUNC_PTR EstimateCostToDestFunc)
{
// you shouldn't change this node unless the change is better
//assert(m_AccumulatedCostFromStartG >= AccumulatedCostFromStartG);
m_AccumulatedCostFromStartG = AccumulatedCostFromStartG;
if (pParentNode == null && pConnectingSegment == null)
{
m_AccumulatedCostFromStartG += AdditionalCostFunc(pParentNode, pConnectingSegment, CollisionCostRatio);
}
m_EstimatedCostToDestH = EstimateCostToDestFunc(pCurWayPoint, pFinalDestWayPoint);
m_TotalCostForThisNodeF = m_AccumulatedCostFromStartG + m_EstimatedCostToDestH;
//assert(pCurWayPoint.GetPosition().x < 1000000 && pCurWayPoint.GetPosition().x > -1000000);
//assert(pCurWayPoint.GetPosition().y < 1000000 && pCurWayPoint.GetPosition().y > -1000000);
m_pCurWayPoint = pCurWayPoint;
m_CurWayPointIndex = CurWayPointIndex;
m_pParent = pParentNode;
}
static double SegmentLengthFunc(CAStarNode /*pParentNode*/ , CPathLineSegment pConnectingSegment, double CollisionCostRatio)
{
return pConnectingSegment.GetLength() * CollisionCostRatio;
}
void AddNodeToClosedList(CAStarNode pNodeToClose)
{
RemoveFromOpenList(pNodeToClose);
//assert(pNodeToClose.GetpNext() == null); // it should be in no list when you add it to the clossed
pNodeToClose.SetpNext(m_ClosedNodes.GetpNext());
m_ClosedNodes.SetpNext(pNodeToClose);
//AssertNoLostNodes;
}
bool AddSubWayPointsToOpenList(CLayerPart pActor, CWayPointMap pWayPointMap, CAStarNode pParentNode,
CPathWayPoint pFinalDestWayPoint, CCollideProcessingState pCollideProcessingState)
{
throw new NotImplementedException();
}
static double SegmentLengthFunc(CAStarNode /*pParentNode*/, CPathLineSegment pConnectingSegment, double CollisionCostRatio)
public void Reset()
{
m_EstimatedCostToDestH = 0;
m_TotalCostForThisNodeF = 0;
m_pCurWayPoint = null;
m_CurWayPointIndex = 0;
m_pParent = null;
m_pNext = null;
m_AccumulatedCostFromStartG = UNSET_NODE_VALUE;
}
