void EditorDraw(Graphics2D pDestFrame)
{
Vector2 WayPointPos = m_pCurWayPoint.GetPosition();
if (m_pParent != null)
{
RGBA_Bytes LineColor = new RGBA_Bytes(128, 128, 128);
Vector2 ParentPos = m_pParent.m_pCurWayPoint.GetPosition();
// draw a line back to your parent
pDestFrame.Line(WayPointPos.x, WayPointPos.y, ParentPos.x, ParentPos.y, LineColor);
}
// print out the stats for this point
int LineSpacing = 12;
int LineOffset = -LineSpacing;
string Text = "";
Text.FormatWith("G: {0:0.0}", m_AccumulatedCostFromStartG);
pDestFrame.DrawString(Text, WayPointPos.x, WayPointPos.y + LineOffset, justification: Justification.Center);
LineOffset += LineSpacing;
Text.FormatWith("H: {0:0.0}", m_EstimatedCostToDestH);
pDestFrame.DrawString(Text, WayPointPos.x, WayPointPos.y + LineOffset, justification: Justification.Center);
LineOffset += LineSpacing;
Text.FormatWith("F: {0:0.0}", m_TotalCostForThisNodeF);
pDestFrame.DrawString(Text, WayPointPos.x, WayPointPos.y + LineOffset, justification: Justification.Center);
LineOffset += LineSpacing;
}
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);
}
static double UclidianDistFunc(CPathWayPoint pCurWayPoint, CPathWayPoint pFinalDestWayPoint)
{
return MMSqrt((double)(pCurWayPoint.GetPosition().GetDistSqrd(pFinalDestWayPoint.GetPosition())));
}