Алгоритм дейкстры для большого числа объектов

class Точка
    {
        public PointF точка; //сама точка
        public double метка; // метка этой точки, по умолчанию - бесконечность
        public Точка предок; //предок точки для вывода кратчайшего пути
        public Точка(PointF точка, double метка = Double.PositiveInfinity)
        {
            this.точка = точка;
            this.метка = метка;
        }
        public Точка() { }
    }
    class Ребро
    {
        public Точка начальная_вершина;
        public Точка конечная_вершина;
        public double вес; 
        public Ребро(Точка начальная_вершина, Точка конечная_вершина, double вес = Double.PositiveInfinity)
        {
            this.начальная_вершина = начальная_вершина;
            this.конечная_вершина = конечная_вершина;
            this.вес = вес;
        }
        public Ребро() { }
    }
    class Алгоритм
    {
        Точка[] точки; //массив ребер и массив точек
        Ребро[] ребра;
        Точка минимальная_точка; //точка с минимальной меткой
        Ребро[] найденные_ребра; //массив ребер, смежных с минимальной точкой
        public ArrayList путь = new ArrayList(); //кратчайший маршрут
        public Алгоритм(Точка[] точки, Ребро[] ребра)
        {
            this.точки = точки;
            this.ребра = ребра;
        }
        public void Начать_Алгоритм(Точка начальная_точка_алгоритма, Точка конечная_точка_алгоритма)
        {
            ArrayList S = new ArrayList();//множество S, содержащее помеченные метки            
            while (минимальная_вершина(S))
            {                
                if (поиск_ребра(минимальная_точка))
                {
                    for (int i = 0; i < найденные_ребра.Length; i++)
                    {
                        double новая_метка = минимальная_точка.метка + найденные_ребра[i].вес;
                        if (найденные_ребра[i].начальная_вершина.точка != минимальная_точка.точка)
                        {
                            if (новая_метка < найденные_ребра[i].начальная_вершина.метка)
                            {
                                найденные_ребра[i].начальная_вершина.предок = минимальная_точка;
                                найденные_ребра[i].начальная_вершина.метка = новая_метка;
                            }
                        }
                        if (найденные_ребра[i].конечная_вершина.точка != минимальная_точка.точка)
                        {
                            if (новая_метка < найденные_ребра[i].конечная_вершина.метка)
                            {
                                найденные_ребра[i].конечная_вершина.предок = минимальная_точка;
                                найденные_ребра[i].конечная_вершина.метка = новая_метка;
                            }
                        }
                    }
                }
                S.Add(минимальная_точка.точка);//отмечаю точку
                if (минимальная_точка.точка == конечная_точка_алгоритма.точка) //если дошел до конца, строю путь
                {
                    путь.Add(минимальная_точка.точка);
                    Точка точка_предок = минимальная_точка;
                    while (точка_предок.точка != начальная_точка_алгоритма.точка)
                    {
                        путь.Add(точка_предок.предок.точка);
                        точка_предок = точка_предок.предок;
                    }
                    break;
                }
            }
        }
        private bool минимальная_вершина(ArrayList S)
        {
            double метка_минимальная = Double.PositiveInfinity;
            bool есть_точки = false;
            for (int i = 0; i < точки.Length; i++)
            {
                if (точки[i].метка < метка_минимальная & S.IndexOf(точки[i].точка) == -1)
                {
                    метка_минимальная = точки[i].метка;
                    минимальная_точка = точки[i];
                    есть_точки = true;
                }
            }
            if (есть_точки)
            {
                return true;
            }
            else
            {
                return false; //ну, тут должно быть что-то вроде сообщения: "Невозможно добраться до точки"
            }
        }        
        private bool поиск_ребра(Точка точка1)
        {
            //получаю смежные ребра с данной точкой
            IEnumerable<Ребро> коллекция = from ребро in ребра where (ребро.начальная_вершина.точка == точка1.точка || ребро.конечная_вершина.точка == точка1.точка) select ребро;
            if (коллекция.Count() > 0)
            {
                найденные_ребра = коллекция.ToArray();
                return true;
            }
            else
            {
                return false;
            }
        }
    }

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
 
namespace algorithmDijkstra
{
    public class Peak:IComparable<Peak>   // вершина
    {
        public int Number { get; set; }
        public bool Mark { get; set; }
        public int Weight { get; set; }
        
        public Peak(int n, bool m = false)
        {
            Number = n;
            Mark = m;
            Weight = int.MaxValue;
        }
 
        int IComparable<Peak>.CompareTo(Peak obj)
        {
            if (this.Number > obj.Number)
                return 1;
            if (this.Number < obj.Number)
                return -1;
            return 0;
        }
    }
 
    public class Edge   // ребро
    {
        public Peak StartPeak { get; set; }
        public Peak EndPeak { get; set; }
        public int Weight { get; set; }
 
        public Edge() { }
 
        public Edge(Peak start, Peak end, int w)
        {
            this.StartPeak = start;
            this.EndPeak = end;
            this.Weight = w;
        }
    }
 
    public class Algorithm
    {
        List<Peak> lPeak { get; set; }
        List<Edge> lEdge { get; set; }
        Queue<Peak> queue = new Queue<Peak>();
 
        public Algorithm(List<Peak> listPeak, List<Edge> listEdge)
        {
            lPeak = listPeak;
            lEdge = listEdge;
        }
 
        private void CurrentExecute(Peak currPeak)
        {
            currPeak.Mark = true;
            IEnumerable<Peak> processing = from lp in (from le in lEdge
                                                      where (le.StartPeak == currPeak || le.EndPeak == currPeak)
                                                      select le.StartPeak == currPeak ? le.EndPeak : le.StartPeak
                                                     ).ToList<Peak>()
                                           where lp.Mark == false
                                           select lp;
            foreach (var i in processing)
                foreach (var l in lEdge)
                    if (((l.StartPeak == i && l.EndPeak == currPeak) || (l.StartPeak == currPeak && l.EndPeak == i)) && i.Weight > currPeak.Weight + l.Weight)
                    {
                        i.Weight = currPeak.Weight + l.Weight;
                        queue.Enqueue(i);
                    }
        }
 
        public int Execute(Peak start, Peak end)
        {
            start.Weight = 0;            
            queue.Enqueue(start);
            while (queue.Count > 0)
                CurrentExecute(queue.Dequeue());
            return end.Weight;
        }
    }
}

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using algorithmDijkstra;
 
namespace test
{
    class Program
    {
        static void Main(string[] args)
        {
            Peak p1 = new Peak(1);  Peak p2 = new Peak(2);  Peak p3 = new Peak(3);
            Peak p4 = new Peak(4);  Peak p5 = new Peak(5);  Peak p6 = new Peak(6);
 
            List<Peak> p = new List<Peak> { p1, p2, p3, p4, p5, p6 };
 
            List<Edge> e = new List<Edge> { new Edge(p1, p2, 7),  new Edge(p1, p3, 9),  new Edge(p1, p6, 14),
                                            new Edge(p2, p3, 10), new Edge(p2, p4, 15), new Edge(p3, p4, 11),
                                            new Edge(p3, p6, 2),  new Edge(p4, p5, 6),  new Edge(p5, p6, 9),
                                          };
            Algorithm alg = new Algorithm(p, e);
            Console.WriteLine(alg.Execute(p1, p5));
            Console.ReadLine();
        }
    }
}

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
 
namespace algorithmDijkstra
{
    public class Peak:IComparable<Peak>   // вершина
    {
        public int Number { get; set; }
        public bool Mark { get; set; }
        public int Weight { get; set; }
        public List<Peak> shortPath { get; set; }  // кратчайший путь до текущей точки от стартовой
        
        public Peak(int n, bool m = false)
        {
            Number = n;
            Mark = m;
            Weight = int.MaxValue;
            shortPath = new List<Peak>();
        }
 
        int IComparable<Peak>.CompareTo(Peak obj)
        {
            if (this.Number > obj.Number)
                return 1;
            if (this.Number < obj.Number)
                return -1;
            return 0;
        }
    }
 
    public class Edge   // ребро
    {
        public Peak StartPeak { get; set; }
        public Peak EndPeak { get; set; }
        public int Weight { get; set; }
 
        public Edge() { }
 
        public Edge(Peak start, Peak end, int w)
        {
            this.StartPeak = start;
            this.EndPeak = end;
            this.Weight = w;
        }
    }
 
    public class Algorithm
    {
        List<Peak> lPeak { get; set; }
        List<Edge> lEdge { get; set; }
        Queue<Peak> queue = new Queue<Peak>();
 
        public Algorithm(List<Peak> listPeak, List<Edge> listEdge)
        {
            lPeak = listPeak;
            lEdge = listEdge;
        }
 
        private void ProcessingCurrentPeak(Peak currPeak)
        {
            currPeak.Mark = true;
            IEnumerable<Peak> processing = from lp in (from le in lEdge
                                                      where (le.StartPeak == currPeak || le.EndPeak == currPeak)
                                                      select le.StartPeak == currPeak ? le.EndPeak : le.StartPeak
                                                     ).ToList<Peak>()
                                           where lp.Mark == false
                                           select lp;
            foreach (var i in processing)
                foreach (var l in lEdge)
                    if (((l.StartPeak == i && l.EndPeak == currPeak) || (l.StartPeak == currPeak && l.EndPeak == i)) && i.Weight > currPeak.Weight + l.Weight)
                    {
                        i.Weight = currPeak.Weight + l.Weight;
                        i.shortPath.Clear();
                        i.shortPath.AddRange(currPeak.shortPath);
                        i.shortPath.Add(i);
                        queue.Enqueue(i);
                    }
        }
 
        private void Execute(Peak start)
        {
            start.Weight = 0;
            start.shortPath.Add(start);
            queue.Enqueue(start);
            while (queue.Count > 0)
                ProcessingCurrentPeak(queue.Dequeue());
        }
 
        public int ExeWeight(Peak start, Peak end)
        {
            Execute(start);
            return end.Weight;
        }
 
        public List<Peak> ExePath(Peak start, Peak end)
        {
            Execute(start);
            return end.shortPath;
        }
    }
}