Пример #1
0
 private MyPath <V> ReturnPath(MyPathfindingData vertexData, MyPathfindingData successor, int remainingVertices)
 {
     if (vertexData.Predecessor == null)
     {
         MyPath <V> retval = new MyPath <V>(remainingVertices + 1);
         retval.Add(vertexData.Parent as V, successor != null ? (successor.Parent as V) : null);
         return(retval);
     }
     else
     {
         MyPath <V> retval = ReturnPath(vertexData.Predecessor, vertexData, remainingVertices + 1);
         retval.Add(vertexData.Parent as V, successor != null ? (successor.Parent as V) : null);
         return(retval);
     }
 }
Пример #2
0
 private bool Visited(MyPathfindingData vertexData)
 {
     return(vertexData.Timestamp == m_timestamp);
 }
Пример #3
0
 private void Visit(MyPathfindingData vertexData)
 {
     vertexData.Timestamp = m_timestamp;
 }
Пример #4
0
        // Note: Termination criterion tells the pathfinding system, how much we want to terminate the pathfinding in the given vertex.
        //
        // Values can range from 0.0 to positive infinity. Infinity means that the vertex will never be returned as a result. Zero, on the
        // other hand, is equivalent to classical pathfinding, which means that the pathfinding will terminate when a path has been found
        // to a vertex with a criterion value of zero and that path's length could not be improved by looking further.
        //
        // Non-zero values on vertices tell the pathfinding that these vertices are acceptable as final vertices, but only if no better
        // vertex is found (i.e. one that would have lower length + heuristic value).
        //
        // The termination criterion is not used as weighting for the pathfinding priority queue though, which means that you can separate
        // termination in a vertex from the searching heuristic
        public MyPath <V> FindPath(V start, Func <V, float> heuristic, Func <V, float> terminationCriterion, Predicate <V> vertexTraversable = null, bool returnClosest = true)
        {
            Debug.Assert(heuristic != null);
            Debug.Assert(terminationCriterion != null);

            CalculateNextTimestamp();

            MyPathfindingData startData = start.PathfindingData;

            Visit(startData);
            startData.Predecessor = null;
            startData.PathLength  = 0.0f;

            IMyPathVertex <V> retVal = null;
            float             lowestAcceptableWeight = float.PositiveInfinity;

            IMyPathVertex <V> closest       = null;
            float             closestWeight = float.PositiveInfinity;

            float terminationValue = terminationCriterion(start);

            if (terminationValue != float.PositiveInfinity)
            {
                retVal = start;
                lowestAcceptableWeight = heuristic(start) + terminationValue;
            }

            m_openVertices.Insert(start.PathfindingData, heuristic(start));
            while (m_openVertices.Count > 0)
            {
                MyPathfindingData currentData = m_openVertices.RemoveMin();
                V     current           = currentData.Parent as V;
                float currentPathLength = currentData.PathLength;

                if (retVal != null && currentPathLength + heuristic(current) >= lowestAcceptableWeight)
                {
                    break;
                }

                for (int i = 0; i < current.GetNeighborCount(); ++i)
                {
                    IMyPathEdge <V> edge = current.GetEdge(i);
                    if (edge == null)
                    {
                        continue;
                    }

                    V neighbor = edge.GetOtherVertex(current);
                    if (neighbor == null || (vertexTraversable != null && !vertexTraversable(neighbor)))
                    {
                        continue;
                    }

                    float             newPathLength  = currentData.PathLength + edge.GetWeight();
                    MyPathfindingData neighborData   = neighbor.PathfindingData;
                    float             neighborWeight = newPathLength + heuristic(neighbor);

                    if (neighborWeight < closestWeight)
                    {
                        closest       = neighbor;
                        closestWeight = neighborWeight;
                    }

                    terminationValue = terminationCriterion(neighbor);
                    if (neighborWeight + terminationValue < lowestAcceptableWeight)
                    {
                        retVal = neighbor;
                        lowestAcceptableWeight = neighborWeight + terminationValue;
                    }

                    if (Visited(neighborData))
                    {
                        if (newPathLength < neighborData.PathLength)
                        {
                            neighborData.PathLength  = newPathLength;
                            neighborData.Predecessor = currentData;
                            m_openVertices.ModifyUp(neighborData, neighborWeight);
                        }
                    }
                    else
                    {
                        Visit(neighborData);
                        neighborData.PathLength  = newPathLength;
                        neighborData.Predecessor = currentData;
                        m_openVertices.Insert(neighborData, neighborWeight);
                    }
                }
            }

            m_openVertices.Clear();

            if (retVal == null)
            {
                if (returnClosest == false || closest == null)
                {
                    return(null);
                }
                else
                {
                    return(ReturnPath(closest.PathfindingData, null, 0));
                }
            }
            else
            {
                return(ReturnPath(retVal.PathfindingData, null, 0));
            }
        }
Пример #5
0
        public MyPath <V> FindPath(V start, V end, Predicate <V> vertexTraversable = null, Predicate <IMyPathEdge <V> > edgeTraversable = null)
        {
            // CH: TODO: Make multiple private copies of this method and call the right one
            // according to what were the arguments to the public interface method

            CalculateNextTimestamp();

            MyPathfindingData startData = start.PathfindingData;

            Visit(startData);
            startData.Predecessor = null;
            startData.PathLength  = 0.0f;

            IMyPathVertex <V> retVal             = null;
            float             shortestPathLength = float.PositiveInfinity;

            m_openVertices.Insert(start.PathfindingData, start.EstimateDistanceTo(end));
            while (m_openVertices.Count > 0)
            {
                MyPathfindingData currentData = m_openVertices.RemoveMin();
                V     current           = currentData.Parent as V;
                float currentPathLength = currentData.PathLength;

                if (retVal != null && currentPathLength >= shortestPathLength)
                {
                    break;
                }

                for (int i = 0; i < current.GetNeighborCount(); ++i)
                {
                    /*IMyPathVertex<V> neighbor = current.GetNeighbor(i);
                     * if (neighbor == null) continue;*/

                    IMyPathEdge <V> edge = current.GetEdge(i);
                    if (edge == null || (edgeTraversable != null && !edgeTraversable(edge)))
                    {
                        continue;
                    }

                    V neighbor = edge.GetOtherVertex(current);
                    if (neighbor == null || (vertexTraversable != null && !vertexTraversable(neighbor)))
                    {
                        continue;
                    }

                    float             newPathLength = currentData.PathLength + edge.GetWeight();
                    MyPathfindingData neighborData  = neighbor.PathfindingData;

                    if (neighbor == end && newPathLength < shortestPathLength)
                    {
                        retVal             = neighbor;
                        shortestPathLength = newPathLength;
                    }

                    if (Visited(neighborData))
                    {
                        if (newPathLength < neighborData.PathLength)
                        {
                            neighborData.PathLength  = newPathLength;
                            neighborData.Predecessor = currentData;
                            m_openVertices.ModifyUp(neighborData, newPathLength + neighbor.EstimateDistanceTo(end));
                        }
                    }
                    else
                    {
                        Visit(neighborData);
                        neighborData.PathLength  = newPathLength;
                        neighborData.Predecessor = currentData;
                        m_openVertices.Insert(neighborData, newPathLength + neighbor.EstimateDistanceTo(end));
                    }
                }
            }

            m_openVertices.Clear();

            if (retVal == null)
            {
                return(null);
            }
            else
            {
                return(ReturnPath(retVal.PathfindingData, null, 0));
            }
        }