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;
}
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;
}
/// <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>
/// 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>
/// 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>
/// 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 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 static void ReadData()
{
fileNodes = new int[squareSize, squareSize];
nodes = new Node[squareSize, squareSize];
string[] lines = File.ReadAllLines(fileName);
// create raw fileNodes
for (int y = 0; y < squareSize; y++)
{
string[] vals = lines[y].Split(',');
for (int x = 0; x < squareSize; x++)
{
int val = Int32.Parse(vals[x]);
fileNodes[x, y] = val;
nodes[x, y] = new Node(x, y, 0);
}
}
}
/// <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;
}
public Node AddNodeWithNoChk(Node NewNode)
{
LN.Add(NewNode);
return NewNode;
}
/// <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;
}
public bool AddArcWithNoChk(Node StartNode, Node EndNode, float Weight)
{
Arc NewArc = new Arc(StartNode, EndNode);
NewArc.Weight = Weight;
LA.Add(NewArc);
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 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;
}
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>
/// 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>
/// 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>
/// 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);
}
/// <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;
}