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()))); }