Create your own navigation system (with a Graph, Node and Connection class)
Lots of people uses naviagtion systems like TomTom these days. As a C# deveoper you might want to know what the basic principles are behind these systems. This post shows you 3 fully functional classes behind the principles of a navigation system.
Creating a map
Imagine that your map is as simple as this:
We create 3 classes using the Dijkstra Algorithm conceived in 1956! (source code later in this blog)
The map can now be created using this code:
private INode<string> _firstNode;
public void CreateMap()
{
_firstNode = new Node<string, int>("A");
var otherNodeKeys = new[] { "B", "C", "D", "E", "F", "G", "H", "I", "J", "K" };
_firstNode.AddNodeRange(otherNodeKeys);
AddConnection("A", "B", 5); // Default two way
AddConnection("A", "D", 7);
AddConnection("B", "D", 6);
AddConnection("B", "E", 5);
AddConnection("C", "A", 13, false); // One way
AddConnection("C", "D", 11, false);
AddConnection("C", "F", 8);
AddConnection("C", "G", 7);
AddConnection("D", "C", 14, false);
AddConnection("D", "E", 3);
AddConnection("D", "F", 4);
AddConnection("D", "H", 6);
AddConnection("D", "I", 6, false);
AddConnection("E", "H", 3);
AddConnection("F", "I", 4, false);
AddConnection("F", "G", 4);
AddConnection("G", "I", 8);
AddConnection("G", "J", 13);
AddConnection("H", "I", 5);
AddConnection("H", "K", 12);
AddConnection("I", "J", 7);
AddConnection("I", "K", 10);
AddConnection("J", "K", 15);
}
private void AddConnection(string keyFrom, string keyTo, decimal weight, bool twoWay = true)
{
var nodeFrom = _firstNode.GetNode(keyFrom);
var nodeTo = _firstNode.GetNode(keyTo);
_firstNode.AddConnection(new Connection<string, string>(nodeFrom, nodeTo, weight), twoWay);
}
Shortest route
Now we want the shortest route from B to C:
var nodeB = _firstNode.GetNode("B");
var nodeC = _firstNode.GetNode("C");
nodeB.SetAsStartNode();
ShowRoute(nodeC);
Using this helper method:
private void ShowRoute(INode<string> endNode)
{
foreach (var node in endNode.FromStartToThis)
{
var line = string.Format("Node {0} \tlength connection {1}\ttotal lenght {2}",
node.Key, node.ConnectionFromStartWeight, node.TotalWeightFromStart);
Console.WriteLine(line);
}
}
Result:
Node B length connection 0 total lenght 0
Node D length connection 6 total lenght 6
Node F length connection 4 total lenght 10
Node C length connection 8 total lenght 18
Now we remove the "street from B to D" and recalculate te route:
nodeB.RemoveConnectionFrom("D");
ShowRoute(nodeC);
Result:
Node B length connection 0 total lenght 0
Node E length connection 5 total lenght 5
Node D length connection 3 total lenght 8
Node F length connection 4 total lenght 12
Node C length connection 8 total lenght 20
And what happens if we remove "D" and all its connections:
var nodeD = _firstNode.GetNode("D");
nodeD.Remove();
ShowRoute(nodeC);
Result:
Node B length connection 0 total lenght 0
Node E length connection 5 total lenght 5
Node H length connection 3 total lenght 8
Node I length connection 5 total lenght 13
Node G length connection 8 total lenght 21
Node C length connection 7 total lenght 28
And an example to show you the (dis)connected nodes:
var connectedNodes = _firstNode.AllNodes.Where(n => n.IsConnectedToStartNode);
// Result A, B, C, E, F, G, H, I, K
var notConnectedNodes = _firstNode.AllNodes.Where(n => !n.IsConnectedToStartNode);
// Result: J (not D because it is removed, not disconnected)
Generic puzzle
The Node and Connection class should have extra generic values so we can store additional info like a streeetname (Connection) or a City (Node).
I wanted to create classes that do not know the type of each others value. So if a Node had a value of type Guid (ID in database) the Graph must not know. Otherwise the Graph need to know the Guid type for adding Nodes. The solution was to add Nodes to a Graph using the interface INode
Source
Last but no least: The source of the 3 classes.
class Graph<TKey> : IGraph<TKey>, IEnumerable<INode<TKey>>
{
private readonly IDictionary<TKey, INode<TKey>> _nodes;
public bool IsValid { get; set; }
internal Graph()
{
_nodes = new Dictionary<TKey, INode<TKey>>();
}
public void Add(INode<TKey> node)
{
if (node == null)
{
throw new ArgumentNullException("node");
}
if (ContainsKey(node.Key))
{
throw new ArgumentException(string.Format("A node with the same key ({0})already exists.", node.Key), "node");
}
_nodes.Add(node.Key, node);
}
public void RemoveNode(TKey key)
{
_nodes.Remove(key);
}
public IEnumerable<INode<TKey>> Nodes
{
get
{
return _nodes.Values;
}
}
private INode<TKey> _startNode;
public INode<TKey> StartNode
{
get
{
return _startNode;
}
set
{
if (value != _startNode) // Really changes
{
_startNode = value;
IsValid = false;
}
}
}
public IEnumerator<INode<TKey>> GetEnumerator()
{
return Nodes.GetEnumerator();
}
IEnumerator IEnumerable.GetEnumerator()
{
return GetEnumerator();
}
public INode<TKey> GetNode(TKey key)
{
if (key.Equals(null))
{
throw new ArgumentNullException("key");
}
INode<TKey> result;
var found = _nodes.TryGetValue(key, out result);
if (!found)
{
throw new ArgumentException("This graph does not contain this key");
}
return result;
}
public bool ContainsKey(TKey key)
{
if (key.Equals(null))
{
throw new ArgumentNullException("key");
}
return _nodes.ContainsKey(key);
}
public bool ContainsNode(INode<TKey> node)
{
if (node.Equals(null))
{
throw new ArgumentNullException("node");
}
INode<TKey> nodeInDictionary;
var found = _nodes.TryGetValue(node.Key, out nodeInDictionary);
return found && ReferenceEquals(node, nodeInDictionary);
}
#region Calculate
private bool _busyCalculating;
public void CalculateWeights()
{
if (_busyCalculating || IsValid || !Nodes.Any()) return; // No need to recalculate
_busyCalculating = true;
InitializeGraph();
ProcessGraph();
IsValid = true;
_busyCalculating = false;
}
private void InitializeGraph()
{
foreach (var node in Nodes)
{
var nodeInternal = (INodeInternal<TKey>) node;
nodeInternal.ConnectionFromStartInternal = null;
nodeInternal.TotalWeightFromStartInternal = decimal.MaxValue;
}
((INodeInternal<TKey>)StartNode).TotalWeightFromStartInternal = 0;
}
private void ProcessGraph()
{
var finished = false;
var nodesNotFinallyCalculated = Nodes.ToDictionary(i => i.Key, i => i);
while (!finished)
{
var lightestNotFinallyCalculatedNode = nodesNotFinallyCalculated.Values
.OrderBy(n => n.TotalWeightFromStart)
.FirstOrDefault(n => n.IsConnectedToStartNode);
if (lightestNotFinallyCalculatedNode == null)
{
finished = true;
}
else
{
ProcessNode(lightestNotFinallyCalculatedNode, nodesNotFinallyCalculated);
nodesNotFinallyCalculated.Remove(lightestNotFinallyCalculatedNode.Key);
}
}
}
private void ProcessNode(INode<TKey> node, IDictionary<TKey, INode<TKey>> queue)
{
var connectionsNotFinallyCalculated = node.ConnectionsFromThis.Where(c => queue.ContainsKey(c.ToNode.Key));
foreach (var connectionNotFinallyCalculated in connectionsNotFinallyCalculated)
{
var weight = node.TotalWeightFromStart + connectionNotFinallyCalculated.Weight;
if (weight < connectionNotFinallyCalculated.ToNode.TotalWeightFromStart)
{
var nodeInternal = (INodeInternal<TKey>) connectionNotFinallyCalculated.ToNode;
nodeInternal.ConnectionFromStartInternal = connectionNotFinallyCalculated;
nodeInternal.TotalWeightFromStartInternal = weight;
}
}
}
#endregion
}
and its interface:
public interface IGraph<TKey> :IEnumerable<INode<TKey>>
{
void CalculateWeights();
bool IsValid { get; set; }
INode<TKey> StartNode { get; set; }
void RemoveNode(TKey key);
INode<TKey> GetNode(TKey key);
void Add(INode<TKey> node);
bool ContainsKey(TKey key);
bool ContainsNode(INode<TKey> node);
}
public class Node<TKey, TValue> : INode<TKey>, INodeInternal<TKey>
{
private readonly IGraph<TKey> _graph;
readonly Dictionary<TKey, IConnection<TKey>> _connectionsFromThis;
readonly Dictionary<TKey, IConnection<TKey>> _connectionsToThis;
private decimal _weightFromStart;
private IConnection<TKey> _connectionFromStart;
public TKey Key { get; private set; }
public TValue Value { get; set; }
public Node(TKey key): this(key, new Graph<TKey>())
{
SetAsStartNode();
}
internal Node(TKey key, IGraph<TKey> graph)
{
_graph = graph;
Key = key;
SetValueIskeyWhenPossible();
_connectionsFromThis = new Dictionary<TKey, IConnection<TKey>>();
_connectionsToThis = new Dictionary<TKey, IConnection<TKey>>();
_graph.Add(this);
_graph.IsValid = false;
}
private void SetValueIskeyWhenPossible()
{
if (typeof(TKey) == typeof(TValue))
{
Value = (TValue)(object)Key;
}
}
public INode<TKey> AddNode(TKey key)
{
var result = new Node<TKey, TValue>(key, _graph);
return result;
}
public void AddNodeRange(IEnumerable<TKey> keys)
{
foreach (var key in keys)
{
AddNode(key);
}
}
public IEnumerable<INode<TKey>> NodesStartToThis
{
get
{
_graph.CalculateWeights();
if (IsStartNode)
{
return new []{this};
}
return ParentFromStart.NodesStartToThis.Concat(new[] {this});
}
}
#region "Internal interface members"
IConnection<TKey> INodeInternal<TKey>.ConnectionFromStartInternal
{
get
{
_graph.CalculateWeights();
return _connectionFromStart;
}
set
{
_connectionFromStart = value;
}
}
decimal INodeInternal<TKey>.TotalWeightFromStartInternal
{
get
{
_graph.CalculateWeights();
return _weightFromStart;
}
set
{
_weightFromStart = value;
}
}
private INodeInternal<TKey> NodeInternal
{
get
{
return this;
}
}
#endregion
public IConnection<TKey> ConnectionFromStart
{
get
{
return NodeInternal.ConnectionFromStartInternal;
}
}
public decimal TotalWeightFromStart
{
get
{
return NodeInternal.TotalWeightFromStartInternal;
}
}
public decimal ConnectionFromStartWeight
{
get
{
return ConnectionFromStart == null ? 0 : ConnectionFromStart.Weight;
}
}
public INode<TKey> ParentFromStart
{
get
{
return ConnectionFromStart.FromNode;
}
}
public IEnumerable<IConnection<TKey>> ConnectionsFromThis
{
get { return _connectionsFromThis.Values; }
}
public IEnumerable<IConnection<TKey>> ConnectionsToThis
{
get { return _connectionsToThis.Values; }
}
public IConnection<TKey> GetConnectionToThis(TKey key)
{
return GetConnection(_connectionsToThis, key);
}
public IConnection<TKey> GetConnectionFromThis(TKey key)
{
return GetConnection(_connectionsFromThis, key);
}
public INode<TKey> GetNode(TKey key)
{
return _graph.GetNode(key);
}
private IConnection<TKey> GetConnection(Dictionary<TKey, IConnection<TKey>> dictionary, TKey key)
{
if (key.Equals(null))
{
throw new ArgumentNullException("key");
}
IConnection<TKey> result;
var found = dictionary.TryGetValue(key, out result);
if (!found)
{
var message = string.Format("Node with key [{0}] does not contain a connection to key [{1}]", Key, key);
throw new ArgumentException(message);
}
return result;
}
public bool ContainsConnectionToThis(TKey key)
{
if (key.Equals(null))
{
throw new ArgumentNullException("key");
}
return _connectionsToThis.ContainsKey(key);
}
public bool ContainsConnectionFromThis(TKey key)
{
if (key.Equals(null))
{
throw new ArgumentNullException("key");
}
return _connectionsFromThis.ContainsKey(key);
}
public bool ContainsConnectionToThis(IConnection<TKey> connection)
{
if (connection == null)
{
throw new ArgumentNullException("connection");
}
IConnection<TKey> foundConnection;
var found = _connectionsToThis.TryGetValue(connection.FromNode.Key, out foundConnection);
return found && ReferenceEquals(foundConnection, connection);
}
public bool ContainsConnectionFromThis(IConnection<TKey> connection)
{
if (connection == null)
{
throw new ArgumentNullException("connection");
}
IConnection<TKey> foundConnection;
var found = _connectionsFromThis.TryGetValue(connection.ToNode.Key, out foundConnection);
return found && ReferenceEquals(foundConnection, connection);
}
public void AddConnection(IConnection<TKey> connection, bool twoWay = true)
{
if (connection == null)
{
throw new ArgumentNullException("connection");
}
if (!_graph.ContainsNode(connection.FromNode))
{
throw new ArgumentException("The from node is not part of the graph");
}
if (!_graph.ContainsNode(connection.FromNode))
{
throw new ArgumentException("The from node is not part of the graph");
}
if (!_graph.ContainsNode(connection.ToNode))
{
throw new ArgumentException("The to node is not part of the graph");
}
// Casting is always possible when it is added to the Graph
var fromNode = (Node<TKey, TValue>)connection.FromNode;
var toNode = (Node<TKey, TValue>)connection.ToNode;
fromNode.AddConnectionImplementation(connection);
if (twoWay)
{
var reverseConnection = connection.ReverseConnection();
toNode.AddConnectionImplementation(reverseConnection);
}
}
private void AddConnectionImplementation(IConnection<TKey> connection)
{
if (connection == null) throw new ArgumentNullException("connection");
if (connection.FromNode == null) throw new ArgumentException("connection.FromNode can not be null");
if (connection.ToNode == null) throw new ArgumentException("connection.ToNode can not be null");
if (ReferenceEquals(connection.FromNode, connection.ToNode) ||
connection.FromNode.Key.Equals(connection.ToNode.Key))
{
throw new InvalidOperationException("The to node and the from node must be different");
}
if (ReferenceEquals(this, connection.FromNode))
{
_connectionsFromThis.Add(connection.ToNode.Key, connection);
// Casting is always possible when it is added to the Graph
var toNode = (Node<TKey, TValue>)connection.ToNode;
toNode.AddConnectionImplementation(connection);
}
else if (ReferenceEquals(this, connection.ToNode))
{
_connectionsToThis.Add(connection.FromNode.Key, connection);
}
else
{
throw new InvalidOperationException("The connection must contains this node.");
}
_graph.IsValid = false;
}
public void RemoveConnectionFromThis(TKey toNodeKey)
{
IConnection<TKey> connection;
var found = _connectionsFromThis.TryGetValue(toNodeKey, out connection);
if (!found)
{
throw new KeyNotFoundException(string.Format("No connection from node with key {0} to node with key {1} has been found.", Key, toNodeKey));
}
RemoveConnectionFromThis(connection);
}
public void RemoveConnectionFromThis(IConnection<TKey> connection)
{
var toNode = (INodeInternal<TKey>)connection.ToNode;
if (ContainsConnectionFromThis(connection))
{
_connectionsFromThis.Remove(toNode.Key);
if (toNode.ContainsConnectionToThis(connection))
{
toNode.RemoveConnectionToThis(connection); // Remove Back link
}
}
else
{
throw new InvalidOperationException("The connection must contains this node in the FromThisConnections.");
}
_graph.IsValid = false;
}
public void RemoveConnectionToThis(IConnection<TKey> connection)
{
var fromNode = (INodeInternal<TKey>) connection.FromNode;
if (ContainsConnectionToThis(connection))
{
_connectionsToThis.Remove(fromNode.Key);
if (fromNode.ContainsConnectionFromThis(connection))
{
fromNode.RemoveConnectionFromThis(connection); // Remove Back link
}
}
else
{
throw new InvalidOperationException("The connection must contains this node in the ToThisConnections.");
}
_graph.IsValid = false;
}
public IEnumerable<INode<TKey>> OtherNodesToThis
{
get
{
return _connectionsToThis.Values.Select(c => c.FromNode);
}
}
public IEnumerable<INode<TKey>> OtherNodesFromThis
{
get
{
return _connectionsFromThis.Values.Select(c => c.ToNode);
}
}
public void RemoveAllConnectionsToThis()
{
var connectionCache = _connectionsToThis.Values.ToList();
foreach (var connection in connectionCache)
{
RemoveConnectionToThis(connection);
}
}
public void RemoveAllConnectionsFromThis()
{
var connectionCache = _connectionsFromThis.Values.ToList();
foreach (var connection in connectionCache)
{
RemoveConnectionFromThis(connection);
}
}
public void RemoveAllConnections()
{
RemoveAllConnectionsFromThis();
RemoveAllConnectionsToThis();
}
public void Remove()
{
RemoveAllConnections();
_graph.RemoveNode(Key);
}
public bool IsConnectedToStartNode
{
get
{
_graph.CalculateWeights();
return TotalWeightFromStart != Decimal.MaxValue;
}
}
public bool IsStartNode
{
get
{
return ReferenceEquals(_graph.StartNode, this);
}
}
public void SetAsStartNode()
{
_graph.StartNode = this;
}
public void SetAsStartNode(TKey key)
{
_graph.StartNode = GetNode(key);
}
public IEnumerable<INode<TKey>> FromStartToThis
{
get
{
if (IsStartNode)
{
return new[] { this };
}
return ParentFromStart.FromStartToThis.Concat(new[] { this });
}
}
public IEnumerable<INode<TKey>> AllNodes
{
get
{
return _graph;
}
}
public override string ToString()
{
return Key.ToString();
}
}
and the 2 interfaces it uses:
internal interface INodeInternal<TKey>
{
TKey Key { get; }
IConnection<TKey> ConnectionFromStartInternal { get; set; }
decimal TotalWeightFromStartInternal { get; set; }
void RemoveConnectionFromThis(IConnection<TKey> connection);
void RemoveConnectionToThis(IConnection<TKey> connection);
bool ContainsConnectionToThis(IConnection<TKey> connection);
bool ContainsConnectionFromThis(IConnection<TKey> connection);
}
public interface INode<TKey>
{
TKey Key { get; }
IEnumerable<INode<TKey>> NodesStartToThis { get; }
IEnumerable<INode<TKey>> FromStartToThis { get; }
IConnection<TKey> ConnectionFromStart { get; }
decimal TotalWeightFromStart { get; }
bool IsConnectedToStartNode { get; }
IEnumerable<IConnection<TKey>> ConnectionsFromThis{ get; }
IEnumerable<IConnection<TKey>> ConnectionsToThis { get; }
decimal ConnectionFromStartWeight { get; }
IEnumerable<INode<TKey>> AllNodes { get; }
void Remove();
void RemoveConnectionFromThis(TKey nodeKey);
void SetAsStartNode();
void SetAsStartNode(TKey key);
void AddNodeRange(IEnumerable<TKey> nodeKeys);
INode<TKey> GetNode(TKey key);
void AddConnection(IConnection<TKey> connection, bool twoWay = true);
void RemoveAllConnectionsToThis();
void RemoveAllConnectionsFromThis();
void RemoveAllConnections();
}
public class Connection<TKey, TValue> : IConnection<TKey>
{
public INode<TKey> FromNode { get; private set; }
public INode<TKey> ToNode { get; private set; }
public decimal Weight { get; set; }
public Connection(INode<TKey> fromNode, INode<TKey> toNode, decimal weight = 1)
{
if (fromNode == null) throw new ArgumentNullException("fromNode");
if (toNode == null) throw new ArgumentNullException("toNode");
if (weight < 0) throw new ArgumentException("Weight must 0 or be positive.");
if (ReferenceEquals(toNode, fromNode) || toNode.Key.Equals(fromNode.Key))
{
throw new InvalidOperationException("The to node and the from node must be different");
}
FromNode = fromNode;
ToNode = toNode;
Weight = weight;
}
public IConnection<TKey> ReverseConnection()
{
return new Connection<TKey, TValue>(ToNode, FromNode, Weight);
}
public override string ToString()
{
return string.Format("From {0} to {1}.", FromNode, ToNode);
}
}
And its interface:
public interface IConnection<TKey>
{
INode<TKey> FromNode { get; }
INode<TKey> ToNode { get; }
decimal Weight { get; set; }
IConnection<TKey> ReverseConnection();
}
- LINQ
- 02-03-2014