public AStarGraph(int[,] map)
        {
            mapLength = map.GetUpperBound(0) + 1;
            mapWidth = map.GetUpperBound(1) + 1;

            vertices = new Vertex[mapLength,mapWidth,4];

            // init vertices for the all free positions
            for (var i = 0; i < mapLength; i++)
            {
                for (var j = 0; j < mapWidth; j++)
                {
                    // if free
                    if (map[i, j] == 0)
                    {
                        // create vertices for direction 0 +z, 1 +x, 2 -z, 3 -x
                        var _0 = new Vertex { position = new MatrixPosition { x = i, z = j, k = 0 } };
                        var _1 = new Vertex { position = new MatrixPosition {x = i, z = j, k = 1} };
                        var _2 = new Vertex { position = new MatrixPosition {x = i, z = j, k = 2} };
                        var _3 = new Vertex { position = new MatrixPosition {x = i, z = j, k = 3} };
                        // add them to list
                        vertices[i, j, 0] = _0;
                        vertices[i, j, 1] = _1;
                        vertices[i, j, 2] = _2;
                        vertices[i, j, 3] = _3;
                        // connect them with the each other
                        var edge01 = new Edge { From = _0, To = _1, Weight = rotateWeight };
                        var edge03 = new Edge { From = _0, To = _3, Weight = rotateWeight };
                        _0.Output.Add(edge01);
                        _0.Output.Add(edge03);
                        var edge12 = new Edge { From = _1, To = _2, Weight = rotateWeight };
                        var edge10 = new Edge { From = _1, To = _0, Weight = rotateWeight };
                        _1.Output.Add(edge12);
                        _1.Output.Add(edge10);
                        var edge21 = new Edge { From = _2, To = _1, Weight = rotateWeight };
                        var edge23 = new Edge { From = _2, To = _3, Weight = rotateWeight };
                        _2.Output.Add(edge21);
                        _2.Output.Add(edge23);
                        var edge30 = new Edge { From = _3, To = _0, Weight = rotateWeight };
                        var edge32 = new Edge { From = _3, To = _2, Weight = rotateWeight };
                        _3.Output.Add(edge30);
                        _3.Output.Add(edge32);
                    }
                }
            }
            // connect rotations with the neighbours
            for (var i = 0; i < mapLength; i++)
            {
                for (var j = 0; j < mapWidth; j++)
                {
                    // check is there any vertice exists
                    if (vertices[i, j, 0] != null)
                    {
                        if (i != mapLength - 1 && vertices[i + 1, j, 1] != null)
                        {
                            var edge = new Edge
                            {
                                From = vertices[i, j, 1],
                                To = vertices[i + 1, j, 1],
                                Weight = moveWeight
                            };
                            vertices[i, j, 1].Output.Add(edge);
                        }
                        if (i != 0 && vertices[i - 1, j, 3] != null)
                        {
                            var edge = new Edge
                            {
                                From = vertices[i, j, 3],
                                To = vertices[i - 1, j, 3],
                                Weight = moveWeight
                            };
                            vertices[i, j, 3].Output.Add(edge);
                        }
                        if (j != mapWidth - 1 && vertices[i, j + 1, 0] != null)
                        {
                            var edge = new Edge
                            {
                                From = vertices[i, j, 0],
                                To = vertices[i, j+1, 0],
                                Weight = moveWeight
                            };
                            vertices[i, j, 0].Output.Add(edge);
                        }
                        if (j != 0 && vertices[i, j - 1, 2] != null)
                        {
                            var edge = new Edge
                            {
                                From = vertices[i, j, 2],
                                To = vertices[i, j - 1, 2],
                                Weight = moveWeight
                            };
                            vertices[i, j, 2].Output.Add(edge);
                        }
                    }
                }
            }
        }
 private bool CheckVertexExplored(Vertex vertex, List<Path> pathes)
 {
     foreach (var p in pathes)
     {
         if (p.frontier == vertex)
         {
             return true;
         }
         foreach (var a in p.actions)
         {
             if (a.From == vertex)
             {
                 return true;
             }
         }
     }
     return false;
 }