public void AddNeighbor(NodeDijkstra node, NodeDijkstra neighbor)
{
if (node.Neighbors == null)
{
node.Neighbors = new List <NeighborDijkstra>();
}
if (neighbor.Neighbors == null)
{
neighbor.Neighbors = new List <NeighborDijkstra>();
}
float nodePosX = 0, nodePosZ = 0, neighborPosX = 0, neighborPosZ = 0;
nodePosX = node.Longitude;
nodePosZ = node.Latitude;
neighborPosX = neighbor.Longitude;
neighborPosZ = neighbor.Latitude;
float distance = UIUtils.getDirectDistance(nodePosX, nodePosZ, neighborPosX, neighborPosZ);
node.Neighbors.Add(new NeighborDijkstra()
{
Node = neighbor, Distance = distance
});
neighbor.Neighbors.Add(new NeighborDijkstra()
{
Node = node, Distance = distance
});
}
public List <PathDataDijkstra> Dijkstra(String startName, String destinationName)
{
bool startNodeFound = false, endNodeFound = false;
List <NodeDijkstra> NodesDijkstra = new List <NodeDijkstra>();
NodeDijkstra start = new NodeDijkstra(), destination = new NodeDijkstra();
foreach (NodeDijkstra nodeInList in this.Nodes)
{
if (nodeInList.Active)
{
NodeDijkstra node = new NodeDijkstra(nodeInList);
if (!startNodeFound && node.Name.Equals(startName))
{
node.DijkstraDistance = 0;
start = node;
startNodeFound = !startNodeFound;
}
else
{
node.DijkstraDistance = Int32.MaxValue;
}
if (!endNodeFound && node.Name.Equals(destinationName))
{
destination = node;
endNodeFound = !endNodeFound;
}
NodesDijkstra.Add(node);
}
}
return(Dijkstra(ref NodesDijkstra, start, destination));
}
private void Relaxation(NodeDijkstra currentNode, NodeDijkstra n)
{
if (currentNode.Shortest + 1 < n.Shortest)
{
n.Shortest = currentNode.Shortest + 1;
n.Previous = currentNode;
}
}
private List <PathDataDijkstra> Dijkstra(ref List <NodeDijkstra> NodesDijkstra, NodeDijkstra start, NodeDijkstra destination)
{
foreach (NodeDijkstra node in NodesDijkstra)
{
if (node.Neighbors != null)
{
foreach (NeighborDijkstra neighbor in node.Neighbors)
{
foreach (NodeDijkstra nodeToFind in NodesDijkstra)
{
if (neighbor.Node.Name == nodeToFind.Name)
{
neighbor.Node = nodeToFind;
break;
}
}
}
}
}
float distancePathed;
start.DijkstraPath = new PathDijkstra(start.Name);
while (NodesDijkstra.Count > 0)
{
NodeDijkstra minNode = (from node in NodesDijkstra orderby node.DijkstraDistance select node).First();
NodesDijkstra.Remove(minNode);
minNode.Visited = true;
//if (minNode.Name == "Node 27")
//{
// Debug.Log(minNode.Name);
//}
if (minNode.Neighbors != null)
{
foreach (NeighborDijkstra neighbor in minNode.Neighbors)
{
if (!neighbor.Node.Visited && neighbor.Node.Active)
{
distancePathed = minNode.DijkstraDistance + neighbor.Distance;
if (distancePathed < neighbor.Node.DijkstraDistance)
{
neighbor.Node.DijkstraDistance = distancePathed;
neighbor.Node.DijkstraPath = new PathDijkstra(
minNode.DijkstraPath.Nodes + "|" + neighbor.Distance + "|" + distancePathed + "," + neighbor.Node.Name
);
}
}
}
}
}
return(PathToNodeList(destination.DijkstraPath));
}
public void AddNeighbor(int idNode, int idNodeneighbor)
{
bool nodeFound = false, neighborFound = false;
NodeDijkstra node = new NodeDijkstra(), neighbor = new NodeDijkstra();
foreach (NodeDijkstra nodeInList in Nodes)
{
if (!nodeFound && nodeInList.IdNode.Equals(idNode))
{
node = nodeInList;
nodeFound = !nodeFound;
}
if (!neighborFound && nodeInList.IdNode.Equals(idNodeneighbor))
{
neighbor = nodeInList;
neighborFound = !neighborFound;
}
if (nodeFound && neighborFound)
{
break;
}
}
if (node.Neighbors == null)
{
node.Neighbors = new List <NeighborDijkstra>();
}
if (neighbor.Neighbors == null)
{
neighbor.Neighbors = new List <NeighborDijkstra>();
}
float nodePosX = 0, nodePosZ = 0, neighborPosX = 0, neighborPosZ = 0;
nodePosX = node.Longitude;
nodePosZ = node.Latitude;
neighborPosX = neighbor.Longitude;
neighborPosZ = neighbor.Latitude;
float distance = UIUtils.getDirectDistance(nodePosX, nodePosZ, neighborPosX, neighborPosZ);
node.Neighbors.Add(new NeighborDijkstra()
{
Node = neighbor, Distance = distance
});
neighbor.Neighbors.Add(new NeighborDijkstra()
{
Node = node, Distance = distance
});
}
private PathDataDijkstra GetUsuarioPosition(NodeDijkstra startNavigationNode)
{
PathDataDijkstra userPos = new PathDataDijkstra();
var userLatitude = UIUtils.getZDistance(UIGPS.Latitude);
var userLongitude = UIUtils.getXDistance(UIGPS.Longitude);
userPos.StartNode = new NodeDijkstra()
{
Latitude = userLatitude, Longitude = userLongitude, Name = "User Position"
};
userPos.EndNode = startNavigationNode;
float adjacent = userLongitude - startNavigationNode.Longitude; //x1 - x2
float opposite = userLatitude - startNavigationNode.Latitude; //y1 - y2
float distance = Mathf.Sqrt(Mathf.Pow(adjacent, 2) + Mathf.Pow(opposite, 2));
userPos.DistanceToNeighbor = distance;
userPos.DistancePathed = distance;
return(userPos);
}
public List <Node> Search(int[,] tileMap, Point src, Point dest, int gridCols, int gridRows)
{
List <Node> path = new List <Node>();
List <NodeDijkstra> closeList = new List <NodeDijkstra>();
List <NodeDijkstra> openList = new List <NodeDijkstra>();
List <NodeDijkstra> collectionQ = new List <NodeDijkstra>();
NodeDijkstra currentNode = null;
Point srcSrc = new Point(src.X, src.Y);
NodeDijkstra nodeSrc = new NodeDijkstra(null, srcSrc, 0);
openList.Add(nodeSrc);
while (openList.Count > 0)
{
double value = openList.Min(x => x.Shortest);
currentNode = openList.Where(i => i.Shortest == value).FirstOrDefault();
if (currentNode.X == dest.X && currentNode.Y == dest.Y)
{
// targed reached
break;
}
closeList.Add(currentNode);
openList.RemoveAt(openList.IndexOf(currentNode));
Point nstart = new Point();
nstart.X = currentNode.X - 1 >= 0 ? currentNode.X - 1 : 0;
nstart.Y = currentNode.Y - 1 >= 0 ? currentNode.Y - 1 : 0;
Point nstop = new Point();
nstop.X = currentNode.X + 1 <= gridCols ? currentNode.X + 1 : gridCols;
nstop.Y = currentNode.Y + 1 <= gridRows ? currentNode.Y + 1 : gridRows;
// check eight neighbours
// sprawdz wszystkich sasiadow
for (int col = nstart.X; col <= nstop.X; col++)
{
for (int row = nstart.Y; row <= nstop.Y; row++)
{
if (tileMap[col, row] == 1)
{
continue;
}
var dd = closeList.Where(x => x.X == col && x.Y == row).FirstOrDefault();
if (dd != null)
{
continue;
}
var cc = openList.Where(x => x.X == col && x.Y == row).FirstOrDefault();
if (cc != null)
{
continue;
}
// Not present in any lists, keep going.
var n = new NodeDijkstra(currentNode, new Point(col, row), int.MaxValue);
Relaxation(currentNode, n);
openList.Add(n); // Q collection
}
}
}
while (currentNode.Previous != null)
{
Node pathNode = new Node(null, new Point(currentNode.X, currentNode.Y));
path.Add(pathNode);
currentNode = currentNode.Previous;
}
return(path);
}
