/// <summary>
/// Default constructor (0, 0, 0) to (1, 1, 1)
/// </summary>
public Arc()
{
StartNode = new Node();
EndNode = new Node(1, 1, 1);
Weight = 0;
LengthUpdated = false;
Passable = true;
}
public Track(Track PreviousTrack, Arc Transition)
{
if (_Target==null) throw new InvalidOperationException("You must specify a target Node for the Track class.");
Queue = PreviousTrack;
_Cost = Queue.Cost + Transition.Cost;
_NbArcsVisited = Queue._NbArcsVisited + 1;
EndNode = Transition.EndNode;
}
public Track(Node GraphNode)
{
if ( _Target==null ) throw new InvalidOperationException("You must specify a target Node for the Track class.");
_Cost = 0;
_NbArcsVisited = 0;
Queue = null;
EndNode = GraphNode;
}
/// <summary>
/// Arc constructor.
/// </summary>
/// <exception cref="ArgumentNullException">Extremity nodes cannot be null.</exception>
/// <exception cref="ArgumentException">StartNode and EndNode must be different.</exception>
/// <param name="Start">The node from which the arc starts.</param>
/// <param name="End">The node to which the arc ends.</param>
public Arc(Node Start, Node End)
{
StartNode = Start;
EndNode = End;
Weight = 1;
LengthUpdated = false;
Passable = true;
}
/// <summary>
/// Returns the manhattan distance between two nodes : |Dx|+|Dy|+|Dz|
/// </summary>
/// <exception cref="ArgumentNullException">Argument nodes must not be null.</exception>
/// <param name="N1">First node.</param>
/// <param name="N2">Second node.</param>
/// <returns>Distance value.</returns>
public static double ManhattanDistance(Node N1, Node N2)
{
if ( N1==null || N2==null ) throw new ArgumentNullException();
double DX = N1.Position.X - N2.Position.X;
double DY = N1.Position.Y - N2.Position.Y;
double DZ = N1.Position.Z - N2.Position.Z;
return Math.Abs(DX)+Math.Abs(DY)+Math.Abs(DZ);
}
/// <summary>
/// Returns the euclidian distance between two nodes : Sqrt(Dx²+Dy²+Dz²)
/// </summary>
/// <param name="N1">First node.</param>
/// <param name="N2">Second node.</param>
/// <returns>Distance value.</returns>
public static double EuclidianDistance(Node N1, Node N2)
{
return Math.Sqrt( SquareEuclidianDistance(N1, N2) );
}
private Node[] GoBackUpNodes(Track T)
{
int Nb = T.NbArcsVisited;
Node[] Path = new Node[Nb+1];
for (int i=Nb; i>=0; i--, T=T.Queue)
Path[i] = T.EndNode;
return Path;
}
/// <summary>
/// Searches for the best path to reach the specified EndNode from the specified StartNode.
/// </summary>
/// <exception cref="ArgumentNullException">StartNode and EndNode cannot be null.</exception>
/// <param name="StartNode">The node from which the path must start.</param>
/// <param name="EndNode">The node to which the path must end.</param>
/// <returns>'true' if succeeded / 'false' if failed.</returns>
public bool SearchPath(Node StartNode, Node EndNode)
{
lock (_Graph)
{
Initialize(StartNode, EndNode);
while ( NextStep() ) {}
return PathFound;
}
}
/// <summary>
/// Returns a copy of this node.
/// </summary>
/// <returns>The reference of the new object.</returns>
public object Clone()
{
Node N = new Node(X, Y, Z);
N._Passable = _Passable;
return N;
}
/// <summary>
/// Returns the arc that arc that comes to this from the specified node if it exists.
/// </summary>
/// <exception cref="ArgumentNullException">Argument node must not be null.</exception>
/// <param name="N">A node that could reach this one.</param>
/// <returns>The arc coming to this from N / null if there is no solution.</returns>
public Arc ArcComingFrom(Node N)
{
if ( N==null ) throw new ArgumentNullException();
foreach (Arc A in _IncomingArcs)
if (A.StartNode == N) return A;
return null;
}
/// <summary>
/// Retourne vrai si le noeud peut se lier avec au moins un autre noeud du graph
/// </summary>
/// <param name="node"></param>
/// <param name="obstacles"></param>
/// <param name="distanceSecurite"></param>
/// <param name="distanceMax"></param>
/// <returns></returns>
public bool Raccordable(Node node, List<IForme> obstacles, double distanceSecurite, double distanceMax)
{
foreach (Node no in Nodes)
{
if (node != no)
{
double distance = Math.Sqrt((node.Position.X - no.Position.X) * (node.Position.X - no.Position.X) + (node.Position.Y - no.Position.Y) * (node.Position.Y - no.Position.Y));
if (distance < distanceMax)
{
Arc arc = new Arc(no, node);
arc.Weight = Math.Sqrt(distance);
Arc arc2 = new Arc(node, no);
arc2.Weight = Math.Sqrt(distance);
foreach (IForme obstacle in obstacles)
{
if (obstacle.Distance(new Segment(new PointReel(no.X, no.Y), new PointReel(node.X, node.Y))) < distanceSecurite)
{
arc.Passable = false;
arc2.Passable = false;
break;
}
}
if (arc.Passable)
{
return true;
}
}
}
}
return false;
}
/// <summary>
/// Ajoute un noeud au graph en reliant tous les points à une distance maximale et en prenant en compte les obstacles à éviter
/// Si permanent == false, le point sera supprimé au prochain appel de @CleanNodesArcsAdd
/// </summary>
/// <param name="node">Noeud à ajouter</param>
/// <param name="obstacles">Obstacles à éviter</param>
/// <param name="distanceSecurite">Distance (mm) de sécurité auour des obstacles</param>
/// <param name="distanceMax">Distance (mm) max de liaison avec les autres noeuds</param>
/// <param name="permnant">True si le point est ajouté de façon permanente et donc ne sera pas supprimé au prochain appel de @CleanNodesArcsAdd</param>
/// <returns>Nombre de points reliés au point ajouté</returns>
public int AddNode(Node node, List<IForme> obstacles, double distanceSecurite, double distanceMax, bool permanent = false)
{
double distanceNode;
// Si un noeud est deja présent à cet endroit on ne l'ajoute pas
ClosestNode(node.X, node.Y, node.Z, out distanceNode, true);
if (distanceNode == 0)
return 0;
// Teste si le noeud est franchissable avec la liste des obstacles
foreach (IForme obstacle in Plateau.ObstaclesPlateau)
{
if (obstacle.Distance(new PointReel(node.X, node.Y)) < distanceSecurite)
{
node.Passable = false;
return 0;
}
}
Nodes.Add(node);
if(!permanent)
nodesAdd.Add(node);
int nbLiaisons = 0;
// Liaisons avec les autres noeuds du graph
foreach (Node no in Nodes)
{
if (node != no)
{
double distance = Math.Sqrt((node.Position.X - no.Position.X) * (node.Position.X - no.Position.X) + (node.Position.Y - no.Position.Y) * (node.Position.Y - no.Position.Y));
if (distance < distanceMax)
{
Arc arc = new Arc(no, node);
arc.Weight = Math.Sqrt(distance);
Arc arc2 = new Arc(node, no);
arc2.Weight = Math.Sqrt(distance);
foreach (IForme obstacle in obstacles)
{
if (obstacle.Distance(new Segment(new PointReel(no.X, no.Y), new PointReel(node.X, node.Y))) < distanceSecurite)
{
arc.Passable = false;
arc2.Passable = false;
break;
}
}
if (arc.Passable)
{
AddArc(arc);
AddArc(arc2);
if (!permanent)
{
arcsAdd.Add(arc);
arcsAdd.Add(arc2);
}
nbLiaisons++;
}
}
}
}
return nbLiaisons;
}
/// <summary>
/// Creates a node, adds to the graph and returns its reference.
/// </summary>
/// <param name="x">X coordinate.</param>
/// <param name="y">Y coordinate.</param>
/// <param name="z">Z coordinate.</param>
/// <returns>The reference of the new node / null if the node is already in the graph.</returns>
public Node AddNode(float x, float y, float z)
{
Node NewNode = new Node(x, y, z);
return AddNode(NewNode) ? NewNode : null;
}
/// <summary>
/// Directly Adds a node to the graph.
/// </summary>
/// <param name="NewNode">The node to add.</param>
/// <returns>'true' if it has actually been added / 'false' if the node is null or if it is already in the graph.</returns>
public bool AddNode(Node NewNode)
{
//if ( NewNode==null || LN.Contains(NewNode) ) return false;
LN.Add(NewNode);
return true;
}
/// <summary>
/// Creates an arc between two nodes that are already registered in the graph, adds it to the graph and returns its reference.
/// </summary>
/// <exception cref="ArgumentException">Cannot add an arc if one of its extremity nodes does not belong to the graph.</exception>
/// <param name="StartNode">Start node for the arc.</param>
/// <param name="EndNode">End node for the arc.</param>
/// <param name="Weight">Weight for the arc.</param>
/// <returns>The reference of the new arc / null if the arc is already in the graph.</returns>
public Arc AddArc(Node StartNode, Node EndNode, float Weight)
{
Arc NewArc = new Arc(StartNode, EndNode);
NewArc.Weight = Weight;
return AddArc(NewArc) ? NewArc : null;
}
/// <summary>
/// Returns the maximum distance between two nodes : Max(|Dx|, |Dy|, |Dz|)
/// </summary>
/// <exception cref="ArgumentNullException">Argument nodes must not be null.</exception>
/// <param name="N1">First node.</param>
/// <param name="N2">Second node.</param>
/// <returns>Distance value.</returns>
public static double MaxDistanceAlongAxis(Node N1, Node N2)
{
if ( N1==null || N2==null ) throw new ArgumentNullException();
double DX = Math.Abs(N1.Position.X - N2.Position.X);
double DY = Math.Abs(N1.Position.Y - N2.Position.Y);
double DZ = Math.Abs(N1.Position.Z - N2.Position.Z);
return Math.Max(DX, Math.Max(DY, DZ));
}
/// <summary>
/// Returns the square euclidian distance between two nodes : Dx²+Dy²+Dz²
/// </summary>
/// <exception cref="ArgumentNullException">Argument nodes must not be null.</exception>
/// <param name="N1">First node.</param>
/// <param name="N2">Second node.</param>
/// <returns>Distance value.</returns>
public static double SquareEuclidianDistance(Node N1, Node N2)
{
if ( N1==null || N2==null ) throw new ArgumentNullException();
double DX = N1.Position.X - N2.Position.X;
double DY = N1.Position.Y - N2.Position.Y;
double DZ = N1.Position.Z - N2.Position.Z;
return DX*DX+DY*DY+DZ*DZ;
}
/// <summary>
/// Removes a node from the graph as well as the linked arcs.
/// </summary>
/// <param name="NodeToRemove">The node to remove.</param>
/// <returns>'true' if succeeded / 'false' otherwise.</returns>
public bool RemoveNode(Node NodeToRemove)
{
if (NodeToRemove == null) return false;
try
{
foreach (Arc A in NodeToRemove.IncomingArcs)
{
A.StartNode.OutgoingArcs.Remove(A);
LA.Remove(A);
}
foreach (Arc A in NodeToRemove.OutgoingArcs)
{
A.EndNode.IncomingArcs.Remove(A);
LA.Remove(A);
}
LN.Remove(NodeToRemove);
}
catch { return false; }
return true;
}
/// <summary>
/// Returns the arc that leads to the specified node if it exists.
/// </summary>
/// <exception cref="ArgumentNullException">Argument node must not be null.</exception>
/// <param name="N">A node that could be reached from this one.</param>
/// <returns>The arc leading to N from this / null if there is no solution.</returns>
public Arc ArcGoingTo(Node N)
{
if ( N==null ) throw new ArgumentNullException();
foreach (Arc A in _OutgoingArcs)
if (A.EndNode == N) return A;
return null;
}
/// <summary>
/// Adds the two opposite arcs between both specified nodes to the graph.
/// </summary>
/// <exception cref="ArgumentException">Cannot add an arc if one of its extremity nodes does not belong to the graph.</exception>
/// <param name="Node1"></param>
/// <param name="Node2"></param>
/// <param name="Weight"></param>
public void Add2Arcs(Node Node1, Node Node2, float Weight)
{
AddArc(Node1, Node2, Weight);
AddArc(Node2, Node1, Weight);
}
public static Trajectoire ChercheTrajectoire(Graph graph, List<IForme> obstacles, Position positionActuell, Position destination, double rayonSecurite, double distanceSecuriteCote)
{
DateTime debut = DateTime.Now;
double distanceRaccordable = 150;
double distance;
bool cheminTrouve = false;
bool raccordable = false;
Trajectoire trajectoire = new Trajectoire();
trajectoire.AngleDepart = new Angle(positionActuell.Angle);
trajectoire.AngleFinal = new Angle(destination.Angle);
PointsTrouves = new List<PointReel>();
PointsTrouves.Add(new PointReel(positionActuell.Coordonnees));
trajectoire.AjouterPoint(new PointReel(positionActuell.Coordonnees));
Synchronizer.Lock(graph);
int nbPointsDepart = 0;
int nbPointsArrivee = 0;
Node debutNode;
Node nodeProche = graph.ClosestNode(positionActuell.Coordonnees.X, positionActuell.Coordonnees.Y, 0, out distance, false);
if (distance != 0)
{
debutNode = new Node(positionActuell.Coordonnees.X, positionActuell.Coordonnees.Y, 0);
nbPointsDepart = graph.AddNode(debutNode, obstacles, rayonSecurite, distanceRaccordable);
}
else
debutNode = nodeProche;
if (nbPointsDepart == 0)
{
// On ne peut pas partir de là où on est
Position positionTestee = new Position(positionActuell);
bool franchissable = true;
// Boucle jusqu'à trouver un point qui se connecte au graph jusqu'à 1m devant
int i;
for (i = 0; i < 100 && !raccordable; i += 1)
{
positionTestee.Avancer(10);
debutNode = new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0);
raccordable = graph.Raccordable(debutNode, obstacles, rayonSecurite, distanceRaccordable);
}
// Le point à i*10 mm devant nous est reliable au graph, on cherche à l'atteindre
if (raccordable)
{
Segment segmentTest = new Segment(new PointReel(positionTestee.Coordonnees), new PointReel(positionActuell.Coordonnees));
// Test des obstacles
Synchronizer.Lock(Plateau.ObstaclesBalise);
foreach (IForme obstacle in obstacles)
{
if (obstacle.Distance(segmentTest) < distanceSecuriteCote)
{
franchissable = false;
// Si l'obstacle génant est un adversaire, on diminue petit à petit son rayon pour pouvoir s'échapper au bout d'un moment
if (Plateau.ObstaclesBalise.Contains(obstacle) && Plateau.RayonAdversaire > 50)
{
Robots.GrosRobot.Historique.Log("Adversaire au contact, impossible de s'enfuir, réduction du périmètre adverse", TypeLog.PathFinding);
Plateau.RayonAdversaire -= 10;
}
}
}
Synchronizer.Unlock(Plateau.ObstaclesBalise);
// Si le semgent entre notre position et le graph relié au graph est parcourable on y va !
if (franchissable)
{
PointsTrouves.Add(new PointReel(positionTestee.Coordonnees));
trajectoire.AjouterPoint(new PointReel(positionTestee.Coordonnees));
debutNode = new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0);
nbPointsDepart = graph.AddNode(debutNode, obstacles, rayonSecurite, distanceRaccordable);
}
}
else
franchissable = false;
// Si toujours pas, on teste en marche arrière
if (!franchissable)
{
franchissable = true;
nbPointsDepart = 0;
positionTestee = new Position(positionActuell);
for (i = 0; i > -100 && !raccordable; i--)
{
positionTestee.Avancer(-10);
debutNode = new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0);
raccordable = graph.Raccordable(debutNode, obstacles, rayonSecurite, distanceRaccordable);
}
// Le point à i*10 mm derrière nous est reliable au graph, on cherche à l'atteindre
if (raccordable)
{
Segment segmentTest = new Segment(new PointReel(positionTestee.Coordonnees), new PointReel(destination.Coordonnees));
Synchronizer.Lock(Plateau.ObstaclesBalise);
// Test des obstacles
foreach (IForme obstacle in obstacles)
{
if (obstacle.Distance(segmentTest) < distanceSecuriteCote)
{
franchissable = false;
// Si l'obstacle génant est un adversaire, on diminue petit à petit son rayon pour pouvoir s'échapper au bout d'un moment
if (Plateau.ObstaclesBalise.Contains(obstacle) && Plateau.RayonAdversaire > 50)
{
Robots.GrosRobot.Historique.Log("Adversaire au contact, impossible de s'enfuir, réduction du périmètre adverse", TypeLog.PathFinding);
Plateau.RayonAdversaire -= 10;
}
}
}
Synchronizer.Unlock(Plateau.ObstaclesBalise);
// Si le semgent entre notre position et le graph relié au graph est parcourable on y va !
if (franchissable)
{
PointsTrouves.Add(new PointReel(positionTestee.Coordonnees));
trajectoire.AjouterPoint(new PointReel(positionTestee.Coordonnees));
debutNode = new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0);
nbPointsDepart = graph.AddNode(debutNode, obstacles, rayonSecurite, distanceRaccordable);
}
}
}
}
Node finNode = graph.ClosestNode(destination.Coordonnees.X, destination.Coordonnees.Y, 0, out distance, false);
if (distance != 0)
{
finNode = new Node(destination.Coordonnees.X, destination.Coordonnees.Y, 0);
nbPointsArrivee = graph.AddNode(finNode, obstacles, rayonSecurite, distanceRaccordable);
}
if (nbPointsArrivee == 0)
{
Console.WriteLine("Blocage arrivée : " + (DateTime.Now - debut).TotalMilliseconds + "ms");
// On ne peut pas arriver là où on souhaite aller
// On teste si on peut faire une approche en ligne
// teta ne doit pas être nul sinon c'est qu'on ne maitrise pas l'angle d'arrivée et on ne connait pas l'angle d'approche
Position positionTestee = new Position(destination);
bool franchissable = true;
// Boucle jusqu'à trouver un point qui se connecte au graph jusqu'à 1m devant
int i;
for (i = 0; i < 100 && !raccordable; i++)
{
positionTestee.Avancer(10);
raccordable = graph.Raccordable(new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0), obstacles, rayonSecurite, distanceRaccordable);
}
// Le point à i*10 mm devant nous est reliable au graph, on cherche à l'atteindre
if (raccordable)
{
Segment segmentTest = new Segment(new PointReel(positionTestee.Coordonnees), new PointReel(positionActuell.Coordonnees));
// Test des obstacles
foreach (IForme obstacle in obstacles)
{
if (obstacle.Distance(segmentTest) < distanceSecuriteCote)
franchissable = false;
}
}
else
franchissable = false;
// Si toujours pas, on teste en marche arrière
if (!franchissable)
{
positionTestee = new Position(destination);
nbPointsArrivee = 0;
for (i = 0; i > -100 && !raccordable; i--)
{
positionTestee.Avancer(-10);
raccordable = graph.Raccordable(new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0), obstacles, rayonSecurite, distanceRaccordable);
}
if (raccordable)
{
franchissable = true;
Segment segmentTest = new Segment(new PointReel(positionTestee.Coordonnees), new PointReel(destination.Coordonnees));
// Test des obstacles
foreach (IForme obstacle in obstacles)
{
if (obstacle.Distance(segmentTest) < distanceSecuriteCote)
franchissable = false;
}
}
}
// Si le semgent entre notre position et le node relié au graph est parcourable on y va !
if (franchissable)
{
finNode = new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0);
nbPointsArrivee = graph.AddNode(finNode, obstacles, rayonSecurite, distanceRaccordable);
}
}
Synchronizer.Unlock(graph);
// Teste s'il est possible d'aller directement à la fin sans passer par le graph
bool toutDroit = true;
Segment segment = new Segment(new PointReel(debutNode.X, debutNode.Y), new PointReel(finNode.X, finNode.Y));
foreach (IForme forme in obstacles)
{
if (segment.Distance(forme) < rayonSecurite)
{
toutDroit = false;
break;
}
}
if (toutDroit)
{
Robots.GrosRobot.Historique.Log("Chemin trouvé : ligne droite", TypeLog.PathFinding);
cheminTrouve = true;
PointsTrouves.Add(new PointReel(finNode.X, finNode.Y));
trajectoire.AjouterPoint(new PointReel(finNode.X, finNode.Y));
if (destination.Coordonnees.Distance(new PointReel(finNode.X, finNode.Y)) > 1)
{
PointsTrouves.Add(new PointReel(destination.Coordonnees));
trajectoire.AjouterPoint(new PointReel(destination.Coordonnees));
}
}
// Sinon on passe par le graph
else
{
Synchronizer.Lock(graph);
AStar aStar = new AStar(graph);
aStar.DijkstraHeuristicBalance = 1;
//Console.WriteLine("Avant pathFinding : " + (DateTime.Now - debut).TotalMilliseconds + "ms");
if (aStar.SearchPath(debutNode, finNode))
{
Synchronizer.Unlock(graph);
//Console.WriteLine("PathFinding trouvé : " + (DateTime.Now - debut).TotalMilliseconds + "ms");
List<Node> nodes = aStar.PathByNodes.ToList<Node>();
List<Arc> arcs = aStar.PathByArcs.ToList<Arc>();
Robots.GrosRobot.Historique.Log("Chemin trouvé : " + (nodes.Count - 2) + " noeud(s) intermédiaire(s)", TypeLog.PathFinding);
CheminEnCoursNoeuds = new List<Node>();
CheminEnCoursArcs = new List<Arc>();
CheminTrouve = new List<Arc>(arcs);
NodeTrouve = new List<Node>(nodes);
//Console.WriteLine("Début simplification : " + (DateTime.Now - debut).TotalMilliseconds + "ms");
// Simplification du chemin
// On part du début et on essaie d'aller au point du plus éloigné au moins éloigné en testant si le passage est possible
// Si c'est possible on zappe tous les points entre les deux
for (int iNodeDepart = 0; iNodeDepart < nodes.Count - 1; iNodeDepart++)
{
if (iNodeDepart != 0)
{
PointsTrouves.Add(new PointReel(nodes[iNodeDepart].X, nodes[iNodeDepart].Y));
trajectoire.AjouterPoint(new PointReel(nodes[iNodeDepart].X, nodes[iNodeDepart].Y));
}
CheminEnCoursNoeuds.Add(nodes[iNodeDepart]);
bool raccourciPossible = true;
for (int iNodeArrivee = nodes.Count - 1; iNodeArrivee > iNodeDepart; iNodeArrivee--)
{
raccourciPossible = true;
Segment racourci = new Segment(new PointReel(nodes[iNodeDepart].X, nodes[iNodeDepart].Y), new PointReel(nodes[iNodeArrivee].X, nodes[iNodeArrivee].Y));
//Arc arcRacourci = new Arc(nodes[iNodeDepart], nodes[iNodeArrivee]);
//CheminTest = arcRacourci;
//arcRacourci.Passable = false;
for (int i = obstacles.Count - 1; i >= 4; i--) // > 4 pour ne pas tester les bordures
{
IForme forme = obstacles[i];
ObstacleTeste = forme;
ObstacleProbleme = null;
if (racourci.Distance(forme) < rayonSecurite)
{
ObstacleProbleme = forme;
// Tempo pour l'affichage détaillé de la recherche de trajectoire (option)
if (Config.CurrentConfig.AfficheDetailTraj > 0)
Thread.Sleep(Config.CurrentConfig.AfficheDetailTraj);
raccourciPossible = false;
break;
}
//else if(Config.CurrentConfig.AfficheDetailTraj > 0)
// Thread.Sleep(Config.CurrentConfig.AfficheDetailTraj);
}
if (Config.CurrentConfig.AfficheDetailTraj > 0)
Thread.Sleep(Config.CurrentConfig.AfficheDetailTraj);
ObstacleTeste = null;
if (raccourciPossible)
{
//CheminEnCoursArcs.Add(arcRacourci);
iNodeDepart = iNodeArrivee - 1;
break;
}
}
CheminTest = null;
if (!raccourciPossible)
{
//Arc arc = new Arc(nodes[iNodeDepart], nodes[iNodeDepart + 1]);
//CheminEnCoursArcs.Add(arc);
}
}
CheminEnCoursNoeuds.Add(nodes[nodes.Count - 1]);
PointsTrouves.Add(new PointReel(nodes[nodes.Count - 1].X, nodes[nodes.Count - 1].Y));
trajectoire.AjouterPoint(new PointReel(nodes[nodes.Count - 1].X, nodes[nodes.Count - 1].Y));
Robots.GrosRobot.Historique.Log("Chemin optimisé : " + (CheminEnCoursNoeuds.Count - 2) + " noeud(s) intermédiaire(s)", TypeLog.PathFinding);
cheminTrouve = true;
if (destination.Coordonnees.Distance(new PointReel(finNode.X, finNode.Y)) > 1)
{
PointsTrouves.Add(new PointReel(destination.Coordonnees));
trajectoire.AjouterPoint(new PointReel(destination.Coordonnees));
}
}
else
{
Synchronizer.Unlock(graph);
CheminEnCoursNoeuds = null;
CheminEnCoursArcs = null;
cheminTrouve = false;
}
}
Synchronizer.Lock(graph);
graph.CleanNodesArcsAdd();
Synchronizer.Unlock(graph);
PointsTrouves = null;
ObstacleProbleme = null;
ObstacleTeste = null;
NodeTrouve = new List<Node>();
CheminTrouve = new List<Arc>();
//Console.WriteLine("PathFinding en " + (DateTime.Now - debut).TotalMilliseconds + " ms");
if (!cheminTrouve)
{
Robots.GrosRobot.Historique.Log("Chemin non trouvé", TypeLog.PathFinding);
return null;
}
else
{
if (Plateau.RayonAdversaire < Plateau.RayonAdversaireInitial)
{
threadRAZRayonAdverse = new Thread(ThreadRAZRayonAdverse);
threadRAZRayonAdverse.Start();
}
return trajectoire;
}
}
/// <summary>
/// Use for a 'step by step' search only. This method is alternate to SearchPath.
/// Initializes AStar before performing search steps manually with NextStep.
/// </summary>
/// <exception cref="ArgumentNullException">StartNode and EndNode cannot be null.</exception>
/// <param name="StartNode">The node from which the path must start.</param>
/// <param name="EndNode">The node to which the path must end.</param>
public void Initialize(Node StartNode, Node EndNode)
{
if ( StartNode==null || EndNode==null ) throw new ArgumentNullException();
_Closed.Clear();
_Open.Clear();
Track.Target = EndNode;
_Open.Add( new Track(StartNode) );
_NbIterations = 0;
_LeafToGoBackUp = null;
}
