All pastes #2047458 Raw Edit

Unnamed

public text v1 · immutable
#2047458 ·published 2011-04-17 16:47 UTC
rendered paste body
        public void GenerateRoute(ref Junction pStart, ref Junction pTarget)
        {
            // The set of nodes already evaluated
            List<Junction> closedSet = new List<Junction>();
            // The set of tentative nodes to be evaluated
            List<Junction> openSet = new List<Junction>();
                           openSet.Add(pStart);
            // The map of navigated nodes
            List<Junction> cameFrom = new List<Junction>();

            // Distance from start along optimal path
            double gScore = 0;
            // heuristic_estimate_of_distance
            double hScore = pStart.Position().Subtract(pTarget.Position()).Length();
            // Estimated total distance from start to goal
            double fScore = hScore;

            // Iterative value
            int depth = 0;
            // While openset is not empty
            while (openSet.Count() != 0)
            {
                // Iterative value
                depth = depth + 1;
                // x := the node in openset having the lowest f_score[] value
                // if x = goal - // return list
                if(openSet[0].Index() == pTarget.Index())
                    break;

                // remove x from openset
                // add x to closedset
                Junction considered = openSet[0];
                openSet.Remove(considered);
                closedSet.Add(considered);

                //foreach y in neightbor_nodes(x)
                double best_g_score = 10000000;
                bool tentative_is_better = false;
                foreach (Junction j in considered._ConnectedJunctions)
                {
                    // if y in closedset
                    if(closedSet.Contains(j))
                        continue;

                    // tentative_g_score := g_score[x] + dist_between(x,y)
                    double tentative_g_score = j.Position().Subtract(pTarget.Position()).Length();
                       
                    // if y not in openset
                    if(!openSet.Contains(j))
                    {
                        openSet.Add(j);
                        tentative_is_better = true;
                    } // else if tentative_g_score < g_score[y]
                    else if (tentative_g_score < best_g_score)
                    {
                        tentative_is_better = true;
                    }
                    else
                        tentative_is_better = false;

                    //if tentative_is_better = true
                    if(tentative_is_better)
                    {
                        cameFrom.Add(j);
                        gScore = tentative_g_score;
                        hScore = j.Position().Subtract(pTarget.Position()).Length();
                        fScore = tentative_g_score + hScore;
                    }
                }
            }

            cameFrom.Reverse();
            _CompleteRoute = cameFrom;
        }