/**
* Initialize the pathfinder and the navigation graph
*
**/
public PathFinder(World world)
{
nodes = new List<Node>();
List<Node> remainingNodes = new List<Node>();// = ALL
List<Node> toProcess = new List<Node>();
List<Node> toProcessLater = new List<Node>();
List<Node> toRemove = new List<Node>();
// , les mettres dans toProcessLater et ajouter n à l.outNodes
Vector2 size = world.Size;
objective = new Node(world.Objective);
for(int x = 0; x < size.X; x += (int)size.X/20){
for(int y = 0; y < size.Y; y += (int)size.Y/20){
remainingNodes.Add(new Node(new Vector2(x, y)));
}
}
toProcess.Add(objective); // la premiere node a traiter est la destination finale
//Tant qu'il reste des nodes à traiter
while(toProcess.Count != 0){
//pour chaque nodes n de toProcess,
//on doit chercher les nodes accessible l dans remainingNode
foreach( Node outNode in toProcess ){
Console.Out.WriteLine("toProcess : " + toProcess.Count);
foreach( Node inNode in remainingNodes ){
Console.Out.WriteLine("remaining : "+remainingNodes.Count);
if(!world.isCollidingWithObstacle(outNode.Position, inNode.Position)){
//Chaque node accessible est mise de coté pour le prochain traitement
if(toProcessLater.IndexOf(inNode) < 0){
toProcessLater.Add(inNode);
toRemove.Add(inNode);
}
//Chaque node accessible se voit ajouté outNode a sa liste de nodes sortantes
inNode.OutNodes.Add(outNode);
//On se prépare a retirer la node trouvée de remainingNode
}
}
nodes.Add(outNode);
}
//Retrait des nodes trouvées de remainingNodes
foreach( Node nod in toRemove ){
remainingNodes.Remove(nod);
}
toProcess.Clear();
toProcess.AddRange(toProcessLater);
toProcessLater.Clear();
Console.Out.WriteLine("cout : " + toProcess.Count);
}
}
/**
* Create a new entity with the given parameters
* The default radius and maxiSpeed may change
**/
public Entity(Node dest, Vector2 position, int rad = 20, float maxiSpeed = 0.07f)
{
this.position = position;
radius = rad;
this.destination = dest;
maxSpeed = maxiSpeed;
lastMoves = new List<Vector2>(nbLastMoves);
}
/**
* Create a new Entity with the given parameters
* The default radius and maxiSpeed may change
**/
public Entity(Node dest, Vector2 position, int zoneWi, int zoneHei, int rad = 20, float maxiSpeed = 0.07f)
{
this.position = position;
radius = rad;
this.destination = dest;
maxSpeed = maxiSpeed;
lastUpdateTime = 0;
lastMoves = new List<Vector2>(nbLastMoves);
toRemove = false;
zoneHeight = zoneHei;
zoneWidth = zoneWi;
}
/**
* Find the closest reachable node from the position
**/
public Node findClosestSubGoal(Vector2 pos, World world, Node lastGoal=null)
{
Node closest = objective;
foreach( Node nod in nodes){
if(nod != lastGoal && !world.isCollidingWithObstacle(pos, nod.Position)
&& (nod.Position - pos).Length() < (closest.Position - pos).Length()){
closest = nod;
}
}
return closest;
}
/**
* Find the next destination after reaching the current node
**/
public Node findNextNode(Node current)
{
List<Node> outNodes = current.OutNodes;
//That shouldn't happen...really
if(outNodes.Count == 0){
Console.Error.WriteLine("Error in during findNextNode, destination is already reached");
return null;
}
Node closestToEnd = outNodes[0];
//Look for the most direct way
foreach(Node nod in outNodes){
if((nod.Position - objective.Position).Length() < (closestToEnd.Position - objective.Position).Length()){
closestToEnd = nod;
}
}
return closestToEnd;
}
public World(int xObjective, int yObjective, int width = 800, int height = 800,
int nbEntities = 30, int nbZonesPath = 20)
{
nbZonesPerSide = nbZonesPath;
lockZones = new List<List<Mutex>>();
for(int i = 0; i < nbZonesPerSide; ++i){
lockZones.Add(new List<Mutex>());
for(int j = 0; j < nbZonesPerSide; ++j){
lockZones[i].Add(new Mutex());
}
}
zoneWidth = width/nbZonesPerSide;
zoneHeight = height/nbZonesPerSide;
entities = new List<Entity>();
obstacles = new List<Obstacle>();
objective = new Vector2(xObjective, yObjective);
size = new Vector2(width,height);
Node destination = new Node(new Vector2(xObjective, yObjective));
Random r = new Random();
//Add some obstacles
Wall wall;
Vector2 startWall, endWall;
/*for(int i = 0; i < 4; ++i){
startWall = new Vector2(r.Next((int)size.X), r.Next((int)size.Y));
endWall = new Vector2(r.Next((int)size.X), r.Next((int)size.Y));
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
}*/
//surrounding walls
startWall = new Vector2(10,10);
endWall = new Vector2(10, height - 10);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
startWall = new Vector2(width - 10, height - 10);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
endWall = new Vector2(width - 10, 10);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
startWall = new Vector2(10,10);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
//objective
int tempW = 100;
objective.X = width/2;
objective.Y = tempW/2;
//in walls
startWall = new Vector2(width/2 - tempW/2, height - tempW);
endWall = new Vector2(tempW, height - tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
startWall = new Vector2(tempW, tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
endWall = new Vector2(width - tempW, tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
startWall = new Vector2(width - tempW, height - tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
endWall = new Vector2(width/2 + tempW/2, height - tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
//Stall
/*startWall = new Vector2(width/2 - tempW/4, height - tempW - tempW/8);
endWall = new Vector2(width/2 + tempW/4, height - tempW + tempW/8);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);*/
//Create the pathFinder here
pathFinder = new PathFinder(this, nbZonesPerSide);
//Add some entities to the world
Entity ent;
Vector2 pos;
for(int i = 0; i < nbEntities; ++i){
pos = new Vector2(r.Next((int)size.X-30)+15, r.Next((int)size.Y-30)+15);
ent = new Entity(destination, pos, zoneWidth, zoneHeight);
while(isCollidingWithObstacle(ent.Position, ent.Position)
|| isCollidingWithEntities(ent, ent.Position)){
pos = new Vector2(r.Next((int)size.X-30)+15, r.Next((int)size.Y-30)+15);
ent = new Entity(destination, pos, zoneWidth, zoneHeight);
}
entities.Add(ent);
//find the first goal of each entities
ent.Destination = pathFinder.findClosestSubGoal(ent.Position, this);
}
threads = new List<Thread>();
foreach(Entity enti in entities){
Thread t = new Thread(enti.autonomousUpdate);
threads.Add(t);
}
}
public World(int xObjective, int yObjective, int width = 800, int height = 800, int nbEntities = 30)
{
entities = new List<Entity>();
obstacles = new List<Obstacle>();
objective = new Vector2(xObjective, yObjective);
size = new Vector2(width,height);
Node destination = new Node(new Vector2(xObjective, yObjective));
Random r = new Random();
//Add some obstacles
Wall wall;
Vector2 startWall, endWall;
/*for(int i = 0; i < 4; ++i){
startWall = new Vector2(r.Next((int)size.X), r.Next((int)size.Y));
endWall = new Vector2(r.Next((int)size.X), r.Next((int)size.Y));
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
}*/
startWall = new Vector2(10,10);
endWall = new Vector2(10, height - 10);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
startWall = new Vector2(width - 10, height - 10);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
endWall = new Vector2(width - 10, 10);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
startWall = new Vector2(10,10);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
//objective
int tempW = 100;
objective.X = width/2;
objective.Y = tempW/2;
//in walls
startWall = new Vector2(width/2 - tempW/2, height - tempW);
endWall = new Vector2(tempW, height - tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
startWall = new Vector2(tempW, tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
endWall = new Vector2(width - tempW, tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
startWall = new Vector2(width - tempW, height - tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
endWall = new Vector2(width/2 + tempW/2, height - tempW);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
//Stall
startWall = new Vector2(width/2 - tempW/4, height - tempW - tempW/8);
endWall = new Vector2(width/2 + tempW/4, height - tempW + tempW/8);
wall = new Wall(startWall, endWall);
obstacles.Add(wall);
//Create the pathFinder here
pathFinder = new PathFinder(this);
//Add some entities to the world
Entity ent;
Vector2 pos;
for(int i = 0; i < nbEntities; ++i){
pos = new Vector2(r.Next((int)size.X -30)+15, r.Next((int)size.Y-30)+15);
ent = new Entity(destination, pos);
while(isCollidingWithObstacle(ent.Position, ent.Position)
|| isCollidingWithEntities(ent, ent.Position)){
pos = new Vector2(r.Next((int)size.X -30)+15, r.Next((int)size.Y-30)+15);
ent = new Entity(destination, pos);
}
entities.Add(ent);
//find the first goal of each entities
ent.Destination = pathFinder.findClosestSubGoal(ent.Position, this);
}
}
