public Dijstra(GridWorld G, int s)
{
world = G;
this.s = s;
int V = G.VertexCount;
marked = new bool[V];
edgeTo = new DirectedWeightedEdge[V];
cost = new double[V];
for (var i = 0; i < V; ++i)
{
cost[i] = double.MaxValue;
}
cost[s] = 0;
pq = new IndexMinPQ <double>(V);
pq.Insert(s, 0);
while (!pq.IsEmpty)
{
var v = pq.DelMin();
marked[v] = true;
foreach (var e in G.Adj(v))
{
Relax(G, e);
}
}
}
private void Relax(GridWorld G, DirectedWeightedEdge e)
{
int v = e.From();
int w = e.To();
if (cost[w] > cost[v] + e.Weight)
{
cost[w] = cost[v] + e.Weight;
edgeTo[w] = e;
if (!pq.Contains(w))
{
pq.Insert(w, cost[w]);
}
else
{
pq.DecreaseKey(w, cost[w]);
}
}
}
static void Main(string[] args)
{
Random random = new Random();
QuadTree space = new QuadTree(new Rectangle(0, 0, 2000, 2000));
GridWorld pathFinder = new GridWorld(space);
FVec2 target = new FVec2(1000, 1000);
List <IAgent> population = new List <IAgent>();
for (int i = 0; i < 10; ++i)
{
IAgent agent = new SimpleAgent(space);
float y = 0; // vertical position
float x = random.Next(2000);
float z = random.Next(2000);
agent.Position = new FVec3(x, y, z);
agent.AgentID = i + 1;
population.Add(agent);
}
for (int steps = 0; steps < 20; ++steps)
{
Dijstra paths = pathFinder.dijstra(target);
foreach (IAgent agent in population)
{
float y = 0; // vertical position
float x = random.Next(2000);
float z = random.Next(2000);
agent.Position = new FVec3(x, y, z);
List <FVec3> path = paths.GetPath(agent);
Console.WriteLine("Path for agent #{0} in step {1}: {2} navigation points",
agent.AgentID, steps + 1, path.Count);
}
target = new FVec2(random.Next(2000), random.Next(2000));
}
}