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;
}
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>
/// 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;
}
private void LoadFromDb(string inputFile)
{
//_core.Log("test1=" + inputFile);
Thread.CurrentThread.CurrentCulture = System.Globalization.CultureInfo.InvariantCulture;
Dictionary<int, Node> npoints = new Dictionary<int, Node> { };
using (var cnn = new SQLiteConnection(String.Format("Data Source={0}; Version=3;Read Only=True;", inputFile))) {
try {
cnn.Open();
var sql = "SELECT id, x, y, z, name, radius FROM point"; // WHERE temp_point = 0";
SQLiteCommand cmd = new SQLiteCommand(sql, cnn);
SQLiteDataReader reader = cmd.ExecuteReader();
while (reader.Read()) {
int id = 0; if (!Int32.TryParse(reader["id"].ToString(), out id)) continue;
if (npoints.ContainsKey(id)) continue;
double x = 0; if (!Double.TryParse(reader["x"].ToString(), out x)) continue;
double y = 0; if (!Double.TryParse(reader["y"].ToString(), out y)) continue;
double z = 0; if (!Double.TryParse(reader["z"].ToString(), out z)) continue;
string name = reader["name"].ToString();
double r = 0; if (!Double.TryParse(reader["radius"].ToString(), out r)) continue;
double dist = 0.02;
Node nd = G.ClosestNode(x, y, z, out dist, true);
if (dist > 0.02 || dist == -1) { nd = new Node(x, y, z); G.AddNode(nd); }
npoints.Add(id, nd);
if ((nd.name == null || nd.name == "") && name != null && name != "") nd.name = name;
if (nd.Position.radius < r) nd.Position.radius = r;
}
sql = "SELECT start_point_id, end_point_id, one_way FROM link"; // WHERE temp_point=0";
cmd = new SQLiteCommand(sql, cnn);
reader = cmd.ExecuteReader();
while (reader.Read()) {
int s_id = 0; if (!Int32.TryParse(reader["start_point_id"].ToString(), out s_id)) continue;
int e_id = 0; if (!Int32.TryParse(reader["end_point_id"].ToString(), out e_id)) continue;
if (e_id == s_id) continue; // ага, и такое бывает.
int ow = 0; if (!Int32.TryParse(reader["one_way"].ToString(), out ow)) continue;
if (!npoints.ContainsKey(s_id) || !npoints.ContainsKey(e_id)) continue;
if (npoints[s_id] == npoints[e_id]) continue;
try {
if (ow == 0) G.Add2Arcs(npoints[s_id], npoints[e_id], 1d);
else G.AddArc(npoints[s_id], npoints[e_id], 1d);
} catch (Exception add) { Err("add " + add.Message + "\n" + npoints[s_id]); }
}
} catch (Exception e) { Err("dbload for \"" + inputFile + "\" error:\n" + e); }
}
}
/// <summary>
///
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static double distZ(Node a, Node b)
{
return distZ(a.Position, b.Position);
}
/// <summary>
///
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static double distZ(Node a, Point3D b)
{
return distZ(a.Position, b);
}
/// <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>
/// 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>
/// 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 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>
/// 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>
/// Creates a node, adds to the graph and returns its reference. Overload for double
/// </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(double x, double y, double 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.
/// overload for double
/// </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, double Weight)
{
Arc NewArc = new Arc(StartNode, EndNode);
NewArc.Weight = Weight;
return AddArc(NewArc) ? NewArc : null;
}
/// <summary>
/// Adds the two opposite arcs between both specified nodes to the graph.
/// overload for double
/// </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, double Weight)
{
AddArc(Node1, Node2, Weight);
AddArc(Node2, Node1, Weight);
}
/// <summary>
/// Direct load path
/// </summary>
/// <param name="data">list of points</param>
public void LoadPath(List<Point3D> data)
{
Node n = null;
Node p = null;
for (var ip = 0; ip < data.Count; ip++) {
if (n != null) p = n;
n = new Node(data[ip]);
G.AddNode(n);
if (p != null) G.AddArc(new Arc(p, n));
}
}
/// <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 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) );
}
/// <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 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));
}
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>
/// 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>
/// Distance from point to node
/// </summary>
/// <param name="a">Point</param>
/// <param name="b">Node</param>
/// <returns></returns>
public static double distZ(Point3D a, Node b)
{
if (a == null || b == null) return 999999999; return distZ(a, b.Position);
}
/// <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>
/// 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;
}