SLikeNet 0.1.3

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
DS_WeightedGraph.h
Go to the documentation of this file.
1 /*
2 * Original work: Copyright (c) 2014, Oculus VR, Inc.
3 * All rights reserved.
4 *
5 * This source code is licensed under the BSD-style license found in the
6 * RakNet License.txt file in the licenses directory of this source tree. An additional grant
7 * of patent rights can be found in the RakNet Patents.txt file in the same directory.
8 *
9 *
10 * Modified work: Copyright (c) 2016-2017, SLikeSoft UG (haftungsbeschränkt)
11 *
12 * This source code was modified by SLikeSoft. Modifications are licensed under the MIT-style
13 * license found in the license.txt file in the root directory of this source tree.
14 */
15
21
22
23 #ifndef __WEIGHTED_GRAPH_H
24 #define __WEIGHTED_GRAPH_H
25
26 #include "DS_OrderedList.h"
27 #include "DS_Map.h"
28 #include "DS_Heap.h"
29 #include "DS_Queue.h"
30 #include "DS_Tree.h"
31 #include "assert.h"
32 #include "memoryoverride.h"
33 #ifdef _DEBUG
34 #include <stdio.h>
35 #endif
36
37 #ifdef _MSC_VER
38 #pragma warning( push )
39 #endif
40
43 namespace DataStructures
44 {
45 template <class node_type, class weight_type, bool allow_unlinkedNodes>
46 class RAK_DLL_EXPORT WeightedGraph
47 {
48 public:
49 static void IMPLEMENT_DEFAULT_COMPARISON(void) {DataStructures::defaultMapKeyComparison<node_type>(node_type(),node_type());}
50
51 WeightedGraph();
52 ~WeightedGraph();
53 WeightedGraph( const WeightedGraph& original_copy );
54 WeightedGraph& operator= ( const WeightedGraph& original_copy );
55 void AddNode(const node_type &node);
56 void RemoveNode(const node_type &node);
57 void AddConnection(const node_type &node1, const node_type &node2, weight_type weight);
58 void RemoveConnection(const node_type &node1, const node_type &node2);
59 bool HasConnection(const node_type &node1, const node_type &node2);
60 void Print(void);
61 void Clear(void);
62 bool GetShortestPath(DataStructures::List<node_type> &path, node_type startNode, node_type endNode, weight_type INFINITE_WEIGHT);
63 bool GetSpanningTree(DataStructures::Tree<node_type> &outTree, DataStructures::List<node_type> *inputNodes, node_type startNode, weight_type INFINITE_WEIGHT );
64 unsigned GetNodeCount(void) const;
65 unsigned GetConnectionCount(unsigned nodeIndex) const;
66 void GetConnectionAtIndex(unsigned nodeIndex, unsigned connectionIndex, node_type &outNode, weight_type &outWeight) const;
67 node_type GetNodeAtIndex(unsigned nodeIndex) const;
68
69 protected:
70 void ClearDijkstra(void);
71 void GenerateDisjktraMatrix(node_type startNode, weight_type INFINITE_WEIGHT);
72
73 DataStructures::Map<node_type, DataStructures::Map<node_type, weight_type> *> adjacencyLists;
74
75 // All these variables are for path finding with Dijkstra
76 // 08/23/06 Won't compile as a DLL inside this struct
77 // struct
78 // {
79 bool isValidPath;
80 node_type rootNode;
81 DataStructures::OrderedList<node_type, node_type> costMatrixIndices;
82 weight_type *costMatrix;
83 node_type *leastNodeArray;
84 // } dijkstra;
85
86 struct NodeAndParent
87 {
88 DataStructures::Tree<node_type>*node;
89 DataStructures::Tree<node_type>*parent;
90 };
91 };
92
93 template <class node_type, class weight_type, bool allow_unlinkedNodes>
94 WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::WeightedGraph()
95 {
96 isValidPath=false;
97 costMatrix=0;
98 }
99
100 template <class node_type, class weight_type, bool allow_unlinkedNodes>
101 WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::~WeightedGraph()
102 {
103 Clear();
104 }
105
106 template <class node_type, class weight_type, bool allow_unlinkedNodes>
107 WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::WeightedGraph( const WeightedGraph& original_copy )
108 {
109 adjacencyLists=original_copy.adjacencyLists;
110
111 isValidPath=original_copy.isValidPath;
112 if (isValidPath)
113 {
114 rootNode=original_copy.rootNode;
115 costMatrixIndices=original_copy.costMatrixIndices;
116 costMatrix = SLNet::OP_NEW_ARRAY<weight_type>(costMatrixIndices.Size() * costMatrixIndices.Size(), _FILE_AND_LINE_ );
117 leastNodeArray = SLNet::OP_NEW_ARRAY<node_type>(costMatrixIndices.Size(), _FILE_AND_LINE_ );
118 memcpy(costMatrix, original_copy.costMatrix, costMatrixIndices.Size() * costMatrixIndices.Size() * sizeof(weight_type));
119 memcpy(leastNodeArray, original_copy.leastNodeArray, costMatrixIndices.Size() * sizeof(weight_type));
120 }
121 }
122
123 template <class node_type, class weight_type, bool allow_unlinkedNodes>
124 WeightedGraph<node_type, weight_type, allow_unlinkedNodes>& WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::operator=( const WeightedGraph& original_copy )
125 {
126 adjacencyLists=original_copy.adjacencyLists;
127
128 isValidPath=original_copy.isValidPath;
129 if (isValidPath)
130 {
131 rootNode=original_copy.rootNode;
132 costMatrixIndices=original_copy.costMatrixIndices;
133 costMatrix = SLNet::OP_NEW_ARRAY<weight_type>(costMatrixIndices.Size() * costMatrixIndices.Size(), _FILE_AND_LINE_ );
134 leastNodeArray = SLNet::OP_NEW_ARRAY<node_type>(costMatrixIndices.Size(), _FILE_AND_LINE_ );
135 memcpy(costMatrix, original_copy.costMatrix, costMatrixIndices.Size() * costMatrixIndices.Size() * sizeof(weight_type));
136 memcpy(leastNodeArray, original_copy.leastNodeArray, costMatrixIndices.Size() * sizeof(weight_type));
137 }
138
139 return *this;
140 }
141
142 template <class node_type, class weight_type, bool allow_unlinkedNodes>
143 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::AddNode(const node_type &node)
144 {
145 adjacencyLists.SetNew(node, SLNet::OP_NEW<DataStructures::Map<node_type, weight_type> >( _FILE_AND_LINE_) );
146 }
147
148 template <class node_type, class weight_type, bool allow_unlinkedNodes>
149 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::RemoveNode(const node_type &node)
150 {
151 unsigned i;
152 DataStructures::Queue<node_type> removeNodeQueue;
153
154 removeNodeQueue.Push(node, _FILE_AND_LINE_ );
155 while (removeNodeQueue.Size())
156 {
157 SLNet::OP_DELETE(adjacencyLists.Pop(removeNodeQueue.Pop()), _FILE_AND_LINE_);
158
159 // Remove this node from all of the other lists as well
160 for (i=0; i < adjacencyLists.Size(); i++)
161 {
162 adjacencyLists[i]->Delete(node);
163
164 #ifdef _MSC_VER
165 #pragma warning( disable : 4127 ) // warning C4127: conditional expression is constant
166 #endif
167 if (allow_unlinkedNodes==false && adjacencyLists[i]->Size()==0)
168 removeNodeQueue.Push(adjacencyLists.GetKeyAtIndex(i), _FILE_AND_LINE_ );
169 }
170 }
171
172 ClearDijkstra();
173 }
174
175 template <class node_type, class weight_type, bool allow_unlinkedNodes>
176 bool WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::HasConnection(const node_type &node1, const node_type &node2)
177 {
178 if (node1==node2)
179 return false;
180 if (adjacencyLists.Has(node1)==false)
181 return false;
182 return adjacencyLists.Get(node1)->Has(node2);
183 }
184
185 template <class node_type, class weight_type, bool allow_unlinkedNodes>
186 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::AddConnection(const node_type &node1, const node_type &node2, weight_type weight)
187 {
188 if (node1==node2)
189 return;
190
191 if (adjacencyLists.Has(node1)==false)
192 AddNode(node1);
193 adjacencyLists.Get(node1)->Set(node2, weight);
194 if (adjacencyLists.Has(node2)==false)
195 AddNode(node2);
196 adjacencyLists.Get(node2)->Set(node1, weight);
197 }
198
199 template <class node_type, class weight_type, bool allow_unlinkedNodes>
200 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::RemoveConnection(const node_type &node1, const node_type &node2)
201 {
202 adjacencyLists.Get(node2)->Delete(node1);
203 adjacencyLists.Get(node1)->Delete(node2);
204
205 #ifdef _MSC_VER
206 #pragma warning( disable : 4127 ) // warning C4127: conditional expression is constant
207 #endif
208 if (allow_unlinkedNodes==false) // If we do not allow _unlinked nodes, then if there are no connections, remove the node
209 {
210 if (adjacencyLists.Get(node1)->Size()==0)
211 RemoveNode(node1); // Will also remove node1 from the adjacency list of node2
212 if (adjacencyLists.Has(node2) && adjacencyLists.Get(node2)->Size()==0)
213 RemoveNode(node2);
214 }
215
216 ClearDijkstra();
217 }
218
219 template <class node_type, class weight_type, bool allow_unlinkedNodes>
220 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::Clear(void)
221 {
222 unsigned i;
223 for (i=0; i < adjacencyLists.Size(); i++)
224 SLNet::OP_DELETE(adjacencyLists[i], _FILE_AND_LINE_);
225 adjacencyLists.Clear();
226
227 ClearDijkstra();
228 }
229
230 template <class node_type, class weight_type, bool allow_unlinkedNodes>
231 bool WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetShortestPath(DataStructures::List<node_type> &path, node_type startNode, node_type endNode, weight_type INFINITE_WEIGHT)
232 {
233 path.Clear(false, _FILE_AND_LINE_);
234 if (startNode==endNode)
235 {
236 path.Insert(startNode, _FILE_AND_LINE_);
237 path.Insert(endNode, _FILE_AND_LINE_);
238 return true;
239 }
240
241 if (isValidPath==false || rootNode!=startNode)
242 {
243 ClearDijkstra();
244 GenerateDisjktraMatrix(startNode, INFINITE_WEIGHT);
245 }
246
247 // return the results
248 bool objectExists;
249 unsigned col,row;
250 weight_type currentWeight;
251 DataStructures::Queue<node_type> outputQueue;
252 col=costMatrixIndices.GetIndexFromKey(endNode, &objectExists);
253 if (costMatrixIndices.Size()<2)
254 {
255 return false;
256 }
257 if (objectExists==false)
258 {
259 return false;
260 }
261 node_type vertex;
262 row=costMatrixIndices.Size()-2;
263 if (row==0)
264 {
265 path.Insert(startNode, _FILE_AND_LINE_);
266 path.Insert(endNode, _FILE_AND_LINE_);
267 return true;
268 }
269 currentWeight=costMatrix[row*adjacencyLists.Size() + col];
270 if (currentWeight==INFINITE_WEIGHT)
271 {
272 // No path
273 return true;
274 }
275 vertex=endNode;
276 outputQueue.PushAtHead(vertex, 0, _FILE_AND_LINE_);
277 row--;
278 for(;;)
279 {
280 while (costMatrix[row*adjacencyLists.Size() + col] == currentWeight)
281 {
282 if (row==0)
283 {
284 path.Insert(startNode, _FILE_AND_LINE_);
285 while(!outputQueue.IsEmpty())
286 path.Insert(outputQueue.Pop(), _FILE_AND_LINE_);
287 return true;
288 }
289 --row;
290 }
291
292 vertex=leastNodeArray[row];
293 outputQueue.PushAtHead(vertex, 0, _FILE_AND_LINE_);
294 if (row==0)
295 break;
296 col=costMatrixIndices.GetIndexFromKey(vertex, &objectExists);
297 currentWeight=costMatrix[row*adjacencyLists.Size() + col];
298 }
299
300 path.Insert(startNode, _FILE_AND_LINE_);
301 while(!outputQueue.IsEmpty())
302 path.Insert(outputQueue.Pop(), _FILE_AND_LINE_);
303 return true;
304 }
305
306 template <class node_type, class weight_type, bool allow_unlinkedNodes>
307 node_type WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetNodeAtIndex(unsigned nodeIndex) const
308 {
309 return adjacencyLists.GetKeyAtIndex(nodeIndex);
310 }
311
312 template <class node_type, class weight_type, bool allow_unlinkedNodes>
313 unsigned WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetNodeCount(void) const
314 {
315 return adjacencyLists.Size();
316 }
317
318 template <class node_type, class weight_type, bool allow_unlinkedNodes>
319 unsigned WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetConnectionCount(unsigned nodeIndex) const
320 {
321 return adjacencyLists[nodeIndex]->Size();
322 }
323
324 template <class node_type, class weight_type, bool allow_unlinkedNodes>
325 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetConnectionAtIndex(unsigned nodeIndex, unsigned connectionIndex, node_type &outNode, weight_type &outWeight) const
326 {
327 outWeight=adjacencyLists[nodeIndex]->operator[](connectionIndex);
328 outNode=adjacencyLists[nodeIndex]->GetKeyAtIndex(connectionIndex);
329 }
330
331 template <class node_type, class weight_type, bool allow_unlinkedNodes>
332 bool WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetSpanningTree(DataStructures::Tree<node_type> &outTree, DataStructures::List<node_type> *inputNodes, node_type startNode, weight_type INFINITE_WEIGHT )
333 {
334 // Find the shortest path from the start node to each of the input nodes. Add this path to a new WeightedGraph if the result is reachable
335 DataStructures::List<node_type> path;
336 DataStructures::WeightedGraph<node_type, weight_type, allow_unlinkedNodes> outGraph;
337 bool res;
338 unsigned i,j;
339 for (i=0; i < inputNodes->Size(); i++)
340 {
341 res=GetShortestPath(path, startNode, (*inputNodes)[i], INFINITE_WEIGHT);
342 if (res && path.Size()>0)
343 {
344 for (j=0; j < path.Size()-1; j++)
345 {
346 // Don't bother looking up the weight
347 outGraph.AddConnection(path[j], path[j+1], INFINITE_WEIGHT);
348 }
349 }
350 }
351
352 // Copy the graph to a tree.
353 DataStructures::Queue<NodeAndParent> nodesToProcess;
354 DataStructures::Tree<node_type> *current;
355 DataStructures::Map<node_type, weight_type> *adjacencyList;
356 node_type key;
357 NodeAndParent nap, nap2;
358 outTree.DeleteDecendants();
359 outTree.data=startNode;
360 current=&outTree;
361 if (outGraph.adjacencyLists.Has(startNode)==false)
362 return false;
363 adjacencyList = outGraph.adjacencyLists.Get(startNode);
364
365 for (i=0; i < adjacencyList->Size(); i++)
366 {
367 nap2.node= SLNet::OP_NEW<DataStructures::Tree<node_type> >( _FILE_AND_LINE_ );
368 nap2.node->data=adjacencyList->GetKeyAtIndex(i);
369 nap2.parent=current;
370 nodesToProcess.Push(nap2, _FILE_AND_LINE_ );
371 current->children.Insert(nap2.node, _FILE_AND_LINE_);
372 }
373
374 while (nodesToProcess.Size())
375 {
376 nap=nodesToProcess.Pop();
377 current=nap.node;
378 adjacencyList = outGraph.adjacencyLists.Get(nap.node->data);
379
380 for (i=0; i < adjacencyList->Size(); i++)
381 {
382 key=adjacencyList->GetKeyAtIndex(i);
383 if (key!=nap.parent->data)
384 {
385 nap2.node= SLNet::OP_NEW<DataStructures::Tree<node_type> >( _FILE_AND_LINE_ );
386 nap2.node->data=key;
387 nap2.parent=current;
388 nodesToProcess.Push(nap2, _FILE_AND_LINE_ );
389 current->children.Insert(nap2.node, _FILE_AND_LINE_);
390 }
391 }
392 }
393
394 return true;
395 }
396
397 template <class node_type, class weight_type, bool allow_unlinkedNodes>
398 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GenerateDisjktraMatrix(node_type startNode, weight_type INFINITE_WEIGHT)
399 {
400 if (adjacencyLists.Size()==0)
401 return;
402
403 costMatrix = SLNet::OP_NEW_ARRAY<weight_type>(adjacencyLists.Size() * adjacencyLists.Size(), _FILE_AND_LINE_ );
404 leastNodeArray = SLNet::OP_NEW_ARRAY<node_type>(adjacencyLists.Size(), _FILE_AND_LINE_ );
405
406 node_type currentNode;
407 unsigned col, row, row2, openSetIndex;
408 node_type adjacentKey;
409 unsigned adjacentIndex;
410 weight_type edgeWeight, currentNodeWeight, adjacentNodeWeight;
411 DataStructures::Map<node_type, weight_type> *adjacencyList;
412 DataStructures::Heap<weight_type, node_type, false> minHeap;
413 DataStructures::Map<node_type, weight_type> openSet;
414
415 for (col=0; col < adjacencyLists.Size(); col++)
416 {
417 // This should be already sorted, so it's a bit inefficient to do an insertion sort, but what the heck
418 costMatrixIndices.Insert(adjacencyLists.GetKeyAtIndex(col),adjacencyLists.GetKeyAtIndex(col), true, _FILE_AND_LINE_);
419 }
420 for (col=0; col < adjacencyLists.Size() * adjacencyLists.Size(); col++)
421 costMatrix[col]=INFINITE_WEIGHT;
422 currentNode=startNode;
423 row=0;
424 currentNodeWeight=0;
425 rootNode=startNode;
426
427 // Clear the starting node column
428 if (adjacencyLists.Size())
429 {
430 adjacentIndex=adjacencyLists.GetIndexAtKey(startNode);
431 for (row2=0; row2 < adjacencyLists.Size(); row2++)
432 costMatrix[row2*adjacencyLists.Size() + adjacentIndex]=0;
433 }
434
435 while (row < adjacencyLists.Size()-1)
436 {
437 adjacencyList = adjacencyLists.Get(currentNode);
438 // Go through all connections from the current node. If the new weight is less than the current weight, then update that weight.
439 for (col=0; col < adjacencyList->Size(); col++)
440 {
441 edgeWeight=(*adjacencyList)[col];
442 adjacentKey=adjacencyList->GetKeyAtIndex(col);
443 adjacentIndex=adjacencyLists.GetIndexAtKey(adjacentKey);
444 adjacentNodeWeight=costMatrix[row*adjacencyLists.Size() + adjacentIndex];
445
446 if (currentNodeWeight + edgeWeight < adjacentNodeWeight)
447 {
448 // Update the weight for the adjacent node
449 for (row2=row; row2 < adjacencyLists.Size(); row2++)
450 costMatrix[row2*adjacencyLists.Size() + adjacentIndex]=currentNodeWeight + edgeWeight;
451 openSet.Set(adjacentKey, currentNodeWeight + edgeWeight);
452 }
453 }
454
455 // Find the lowest in the open set
456 minHeap.Clear(true,_FILE_AND_LINE_);
457 for (openSetIndex=0; openSetIndex < openSet.Size(); openSetIndex++)
458 minHeap.Push(openSet[openSetIndex], openSet.GetKeyAtIndex(openSetIndex),_FILE_AND_LINE_);
459
460 /*
461 unsigned i,j;
462 for (i=0; i < adjacencyLists.Size()-1; i++)
463 {
464 for (j=0; j < adjacencyLists.Size(); j++)
465 {
466 RAKNET_DEBUG_PRINTF("%2i ", costMatrix[i*adjacencyLists.Size() + j]);
467 }
468 RAKNET_DEBUG_PRINTF("Node=%i", leastNodeArray[i]);
469 RAKNET_DEBUG_PRINTF("\n");
470 }
471 */
472
473 if (minHeap.Size()==0)
474 {
475 // Unreachable nodes
476 isValidPath=true;
477 return;
478 }
479
480 currentNodeWeight=minHeap.PeekWeight(0);
481 leastNodeArray[row]=minHeap.Pop(0);
482 currentNode=leastNodeArray[row];
483 openSet.Delete(currentNode);
484 row++;
485 }
486
487 /*
488 #ifdef _DEBUG
489 unsigned i,j;
490 for (i=0; i < adjacencyLists.Size()-1; i++)
491 {
492 for (j=0; j < adjacencyLists.Size(); j++)
493 {
494 RAKNET_DEBUG_PRINTF("%2i ", costMatrix[i*adjacencyLists.Size() + j]);
495 }
496 RAKNET_DEBUG_PRINTF("Node=%i", leastNodeArray[i]);
497 RAKNET_DEBUG_PRINTF("\n");
498 }
499 #endif
500 */
501
502 isValidPath=true;
503 }
504
505 template <class node_type, class weight_type, bool allow_unlinkedNodes>
506 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::ClearDijkstra(void)
507 {
508 if (isValidPath)
509 {
510 isValidPath=false;
511 SLNet::OP_DELETE_ARRAY(costMatrix, _FILE_AND_LINE_);
512 SLNet::OP_DELETE_ARRAY(leastNodeArray, _FILE_AND_LINE_);
513 costMatrixIndices.Clear(false, _FILE_AND_LINE_);
514 }
515 }
516
517 template <class node_type, class weight_type, bool allow_unlinkedNodes>
518 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::Print(void)
519 {
520 #ifdef _DEBUG
521 unsigned i,j;
522 for (i=0; i < adjacencyLists.Size(); i++)
523 {
524 //RAKNET_DEBUG_PRINTF("%i connected to ", i);
525 RAKNET_DEBUG_PRINTF("%s connected to ", adjacencyLists.GetKeyAtIndex(i).systemAddress.ToString());
526
527 if (adjacencyLists[i]->Size()==0)
528 RAKNET_DEBUG_PRINTF("<Empty>");
529 else
530 {
531 for (j=0; j < adjacencyLists[i]->Size(); j++)
532 // RAKNET_DEBUG_PRINTF("%i (%.2f) ", adjacencyLists.GetIndexAtKey(adjacencyLists[i]->GetKeyAtIndex(j)), (float) adjacencyLists[i]->operator[](j) );
533 RAKNET_DEBUG_PRINTF("%s (%.2f) ", adjacencyLists[i]->GetKeyAtIndex(j).systemAddress.ToString(), (float) adjacencyLists[i]->operator[](j) );
534 }
535
536 RAKNET_DEBUG_PRINTF("\n");
537 }
538 #endif
539 }
540 }
541
542 #ifdef _MSC_VER
543 #pragma warning( pop )
544 #endif
545
546 #endif