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