00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #ifndef __WEIGHTED_GRAPH_H
00012 #define __WEIGHTED_GRAPH_H
00013
00014 #include "DS_OrderedList.h"
00015 #include "DS_Map.h"
00016 #include "DS_Heap.h"
00017 #include "DS_Queue.h"
00018 #include "DS_Tree.h"
00019 #include "RakAssert.h"
00020 #include "RakMemoryOverride.h"
00021 #ifdef _DEBUG
00022 #include <stdio.h>
00023 #endif
00024
00025 #ifdef _MSC_VER
00026 #pragma warning( push )
00027 #endif
00028
00031 namespace DataStructures
00032 {
00033 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00034 class RAK_DLL_EXPORT WeightedGraph
00035 {
00036 public:
00037 static void IMPLEMENT_DEFAULT_COMPARISON(void) {DataStructures::defaultMapKeyComparison<node_type>(node_type(),node_type());}
00038
00039 WeightedGraph();
00040 ~WeightedGraph();
00041 WeightedGraph( const WeightedGraph& original_copy );
00042 WeightedGraph& operator= ( const WeightedGraph& original_copy );
00043 void AddNode(const node_type &node);
00044 void RemoveNode(const node_type &node);
00045 void AddConnection(const node_type &node1, const node_type &node2, weight_type weight);
00046 void RemoveConnection(const node_type &node1, const node_type &node2);
00047 bool HasConnection(const node_type &node1, const node_type &node2);
00048 void Print(void);
00049 void Clear(void);
00050 bool GetShortestPath(DataStructures::List<node_type> &path, node_type startNode, node_type endNode, weight_type INFINITE_WEIGHT);
00051 bool GetSpanningTree(DataStructures::Tree<node_type> &outTree, DataStructures::List<node_type> *inputNodes, node_type startNode, weight_type INFINITE_WEIGHT );
00052 unsigned GetNodeCount(void) const;
00053 unsigned GetConnectionCount(unsigned nodeIndex) const;
00054 void GetConnectionAtIndex(unsigned nodeIndex, unsigned connectionIndex, node_type &outNode, weight_type &outWeight) const;
00055 node_type GetNodeAtIndex(unsigned nodeIndex) const;
00056
00057 protected:
00058 void ClearDijkstra(void);
00059 void GenerateDisjktraMatrix(node_type startNode, weight_type INFINITE_WEIGHT);
00060
00061 DataStructures::Map<node_type, DataStructures::Map<node_type, weight_type> *> adjacencyLists;
00062
00063
00064
00065
00066
00067 bool isValidPath;
00068 node_type rootNode;
00069 DataStructures::OrderedList<node_type, node_type> costMatrixIndices;
00070 weight_type *costMatrix;
00071 node_type *leastNodeArray;
00072
00073
00074 struct NodeAndParent
00075 {
00076 DataStructures::Tree<node_type>*node;
00077 DataStructures::Tree<node_type>*parent;
00078 };
00079 };
00080
00081 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00082 WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::WeightedGraph()
00083 {
00084 isValidPath=false;
00085 costMatrix=0;
00086 }
00087
00088 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00089 WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::~WeightedGraph()
00090 {
00091 Clear();
00092 }
00093
00094 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00095 WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::WeightedGraph( const WeightedGraph& original_copy )
00096 {
00097 adjacencyLists=original_copy.adjacencyLists;
00098
00099 isValidPath=original_copy.isValidPath;
00100 if (isValidPath)
00101 {
00102 rootNode=original_copy.rootNode;
00103 costMatrixIndices=original_copy.costMatrixIndices;
00104 costMatrix = RakNet::OP_NEW_ARRAY<weight_type>(costMatrixIndices.Size() * costMatrixIndices.Size(), __FILE__, __LINE__ );
00105 leastNodeArray = RakNet::OP_NEW_ARRAY<node_type>(costMatrixIndices.Size(), __FILE__, __LINE__ );
00106 memcpy(costMatrix, original_copy.costMatrix, costMatrixIndices.Size() * costMatrixIndices.Size() * sizeof(weight_type));
00107 memcpy(leastNodeArray, original_copy.leastNodeArray, costMatrixIndices.Size() * sizeof(weight_type));
00108 }
00109 }
00110
00111 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00112 WeightedGraph<node_type, weight_type, allow_unlinkedNodes>& WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::operator=( const WeightedGraph& original_copy )
00113 {
00114 adjacencyLists=original_copy.adjacencyLists;
00115
00116 isValidPath=original_copy.isValidPath;
00117 if (isValidPath)
00118 {
00119 rootNode=original_copy.rootNode;
00120 costMatrixIndices=original_copy.costMatrixIndices;
00121 costMatrix = RakNet::OP_NEW_ARRAY<weight_type>(costMatrixIndices.Size() * costMatrixIndices.Size(), __FILE__, __LINE__ );
00122 leastNodeArray = RakNet::OP_NEW_ARRAY<node_type>(costMatrixIndices.Size(), __FILE__, __LINE__ );
00123 memcpy(costMatrix, original_copy.costMatrix, costMatrixIndices.Size() * costMatrixIndices.Size() * sizeof(weight_type));
00124 memcpy(leastNodeArray, original_copy.leastNodeArray, costMatrixIndices.Size() * sizeof(weight_type));
00125 }
00126
00127 return *this;
00128 }
00129
00130 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00131 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::AddNode(const node_type &node)
00132 {
00133 adjacencyLists.SetNew(node, RakNet::OP_NEW<DataStructures::Map<node_type, weight_type> >( __FILE__, __LINE__) );
00134 }
00135
00136 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00137 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::RemoveNode(const node_type &node)
00138 {
00139 unsigned i;
00140 DataStructures::Queue<node_type> removeNodeQueue;
00141
00142 removeNodeQueue.Push(node, __FILE__, __LINE__ );
00143 while (removeNodeQueue.Size())
00144 {
00145 RakNet::OP_DELETE(adjacencyLists.Pop(removeNodeQueue.Pop()), __FILE__, __LINE__);
00146
00147
00148 for (i=0; i < adjacencyLists.Size(); i++)
00149 {
00150 adjacencyLists[i]->Delete(node);
00151
00152 #ifdef _MSC_VER
00153 #pragma warning( disable : 4127 ) // warning C4127: conditional expression is constant
00154 #endif
00155 if (allow_unlinkedNodes==false && adjacencyLists[i]->Size()==0)
00156 removeNodeQueue.Push(adjacencyLists.GetKeyAtIndex(i), __FILE__, __LINE__ );
00157 }
00158 }
00159
00160 ClearDijkstra();
00161 }
00162
00163 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00164 bool WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::HasConnection(const node_type &node1, const node_type &node2)
00165 {
00166 if (node1==node2)
00167 return false;
00168 if (adjacencyLists.Has(node1)==false)
00169 return false;
00170 return adjacencyLists.Get(node1)->Has(node2);
00171 }
00172
00173 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00174 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::AddConnection(const node_type &node1, const node_type &node2, weight_type weight)
00175 {
00176 if (node1==node2)
00177 return;
00178
00179 if (adjacencyLists.Has(node1)==false)
00180 AddNode(node1);
00181 adjacencyLists.Get(node1)->Set(node2, weight);
00182 if (adjacencyLists.Has(node2)==false)
00183 AddNode(node2);
00184 adjacencyLists.Get(node2)->Set(node1, weight);
00185 }
00186
00187 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00188 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::RemoveConnection(const node_type &node1, const node_type &node2)
00189 {
00190 adjacencyLists.Get(node2)->Delete(node1);
00191 adjacencyLists.Get(node1)->Delete(node2);
00192
00193 #ifdef _MSC_VER
00194 #pragma warning( disable : 4127 ) // warning C4127: conditional expression is constant
00195 #endif
00196 if (allow_unlinkedNodes==false)
00197 {
00198 if (adjacencyLists.Get(node1)->Size()==0)
00199 RemoveNode(node1);
00200 if (adjacencyLists.Has(node2) && adjacencyLists.Get(node2)->Size()==0)
00201 RemoveNode(node2);
00202 }
00203
00204 ClearDijkstra();
00205 }
00206
00207 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00208 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::Clear(void)
00209 {
00210 unsigned i;
00211 for (i=0; i < adjacencyLists.Size(); i++)
00212 RakNet::OP_DELETE(adjacencyLists[i], __FILE__, __LINE__);
00213 adjacencyLists.Clear();
00214
00215 ClearDijkstra();
00216 }
00217
00218 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00219 bool WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetShortestPath(DataStructures::List<node_type> &path, node_type startNode, node_type endNode, weight_type INFINITE_WEIGHT)
00220 {
00221 path.Clear(false, __FILE__, __LINE__);
00222 if (startNode==endNode)
00223 {
00224 path.Insert(startNode, __FILE__, __LINE__);
00225 path.Insert(endNode, __FILE__, __LINE__);
00226 return true;
00227 }
00228
00229 if (isValidPath==false || rootNode!=startNode)
00230 {
00231 ClearDijkstra();
00232 GenerateDisjktraMatrix(startNode, INFINITE_WEIGHT);
00233 }
00234
00235
00236 bool objectExists;
00237 unsigned col,row;
00238 weight_type currentWeight;
00239 DataStructures::Queue<node_type> outputQueue;
00240 col=costMatrixIndices.GetIndexFromKey(endNode, &objectExists);
00241 if (costMatrixIndices.Size()<2)
00242 {
00243 return false;
00244 }
00245 if (objectExists==false)
00246 {
00247 return false;
00248 }
00249 node_type vertex;
00250 row=costMatrixIndices.Size()-2;
00251 if (row==0)
00252 {
00253 path.Insert(startNode, __FILE__, __LINE__);
00254 path.Insert(endNode, __FILE__, __LINE__);
00255 return true;
00256 }
00257 currentWeight=costMatrix[row*adjacencyLists.Size() + col];
00258 if (currentWeight==INFINITE_WEIGHT)
00259 {
00260
00261 return true;
00262 }
00263 vertex=endNode;
00264 outputQueue.PushAtHead(vertex, 0, __FILE__,__LINE__);
00265 row--;
00266 #ifdef _MSC_VER
00267 #pragma warning( disable : 4127 ) // warning C4127: conditional expression is constant
00268 #endif
00269 while (1)
00270 {
00271 while (costMatrix[row*adjacencyLists.Size() + col] == currentWeight)
00272 {
00273 if (row==0)
00274 {
00275 path.Insert(startNode, __FILE__, __LINE__);
00276 for (col=0; outputQueue.Size(); col++)
00277 path.Insert(outputQueue.Pop(), __FILE__, __LINE__);
00278 return true;
00279 }
00280 --row;
00281 }
00282
00283 vertex=leastNodeArray[row];
00284 outputQueue.PushAtHead(vertex, 0, __FILE__,__LINE__);
00285 if (row==0)
00286 break;
00287 col=costMatrixIndices.GetIndexFromKey(vertex, &objectExists);
00288 currentWeight=costMatrix[row*adjacencyLists.Size() + col];
00289 }
00290
00291 path.Insert(startNode, __FILE__, __LINE__);
00292 for (col=0; outputQueue.Size(); col++)
00293 path.Insert(outputQueue.Pop(), __FILE__, __LINE__);
00294 return true;
00295 }
00296
00297 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00298 node_type WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetNodeAtIndex(unsigned nodeIndex) const
00299 {
00300 return adjacencyLists.GetKeyAtIndex(nodeIndex);
00301 }
00302
00303 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00304 unsigned WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetNodeCount(void) const
00305 {
00306 return adjacencyLists.Size();
00307 }
00308
00309 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00310 unsigned WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetConnectionCount(unsigned nodeIndex) const
00311 {
00312 return adjacencyLists[nodeIndex]->Size();
00313 }
00314
00315 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00316 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GetConnectionAtIndex(unsigned nodeIndex, unsigned connectionIndex, node_type &outNode, weight_type &outWeight) const
00317 {
00318 outWeight=adjacencyLists[nodeIndex]->operator[](connectionIndex);
00319 outNode=adjacencyLists[nodeIndex]->GetKeyAtIndex(connectionIndex);
00320 }
00321
00322 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00323 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 )
00324 {
00325
00326 DataStructures::List<node_type> path;
00327 DataStructures::WeightedGraph<node_type, weight_type, allow_unlinkedNodes> outGraph;
00328 bool res;
00329 unsigned i,j;
00330 for (i=0; i < inputNodes->Size(); i++)
00331 {
00332 res=GetShortestPath(path, startNode, (*inputNodes)[i], INFINITE_WEIGHT);
00333 if (res && path.Size()>0)
00334 {
00335 for (j=0; j < path.Size()-1; j++)
00336 {
00337
00338 outGraph.AddConnection(path[j], path[j+1], INFINITE_WEIGHT);
00339 }
00340 }
00341 }
00342
00343
00344 DataStructures::Queue<NodeAndParent> nodesToProcess;
00345 DataStructures::Tree<node_type> *current;
00346 DataStructures::Map<node_type, weight_type> *adjacencyList;
00347 node_type key;
00348 NodeAndParent nap, nap2;
00349 outTree.DeleteDecendants();
00350 outTree.data=startNode;
00351 current=&outTree;
00352 if (outGraph.adjacencyLists.Has(startNode)==false)
00353 return false;
00354 adjacencyList = outGraph.adjacencyLists.Get(startNode);
00355
00356 for (i=0; i < adjacencyList->Size(); i++)
00357 {
00358 nap2.node=RakNet::OP_NEW<DataStructures::Tree<node_type> >( __FILE__, __LINE__ );
00359 nap2.node->data=adjacencyList->GetKeyAtIndex(i);
00360 nap2.parent=current;
00361 nodesToProcess.Push(nap2, __FILE__, __LINE__ );
00362 current->children.Insert(nap2.node, __FILE__, __LINE__);
00363 }
00364
00365 while (nodesToProcess.Size())
00366 {
00367 nap=nodesToProcess.Pop();
00368 current=nap.node;
00369 adjacencyList = outGraph.adjacencyLists.Get(nap.node->data);
00370
00371 for (i=0; i < adjacencyList->Size(); i++)
00372 {
00373 key=adjacencyList->GetKeyAtIndex(i);
00374 if (key!=nap.parent->data)
00375 {
00376 nap2.node=RakNet::OP_NEW<DataStructures::Tree<node_type> >( __FILE__, __LINE__ );
00377 nap2.node->data=key;
00378 nap2.parent=current;
00379 nodesToProcess.Push(nap2, __FILE__, __LINE__ );
00380 current->children.Insert(nap2.node, __FILE__, __LINE__);
00381 }
00382 }
00383 }
00384
00385 return true;
00386 }
00387
00388 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00389 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::GenerateDisjktraMatrix(node_type startNode, weight_type INFINITE_WEIGHT)
00390 {
00391 if (adjacencyLists.Size()==0)
00392 return;
00393
00394 costMatrix = RakNet::OP_NEW_ARRAY<weight_type>(adjacencyLists.Size() * adjacencyLists.Size(), __FILE__, __LINE__ );
00395 leastNodeArray = RakNet::OP_NEW_ARRAY<node_type>(adjacencyLists.Size(), __FILE__, __LINE__ );
00396
00397 node_type currentNode;
00398 unsigned col, row, row2, openSetIndex;
00399 node_type adjacentKey;
00400 unsigned adjacentIndex;
00401 weight_type edgeWeight, currentNodeWeight, adjacentNodeWeight;
00402 DataStructures::Map<node_type, weight_type> *adjacencyList;
00403 DataStructures::Heap<weight_type, node_type, false> minHeap;
00404 DataStructures::Map<node_type, weight_type> openSet;
00405
00406 for (col=0; col < adjacencyLists.Size(); col++)
00407 {
00408
00409 costMatrixIndices.Insert(adjacencyLists.GetKeyAtIndex(col),adjacencyLists.GetKeyAtIndex(col), true, __FILE__,__LINE__);
00410 }
00411 for (col=0; col < adjacencyLists.Size() * adjacencyLists.Size(); col++)
00412 costMatrix[col]=INFINITE_WEIGHT;
00413 currentNode=startNode;
00414 row=0;
00415 currentNodeWeight=0;
00416 rootNode=startNode;
00417
00418
00419 if (adjacencyLists.Size())
00420 {
00421 adjacentIndex=adjacencyLists.GetIndexAtKey(startNode);
00422 for (row2=0; row2 < adjacencyLists.Size(); row2++)
00423 costMatrix[row2*adjacencyLists.Size() + adjacentIndex]=0;
00424 }
00425
00426 while (row < adjacencyLists.Size()-1)
00427 {
00428 adjacencyList = adjacencyLists.Get(currentNode);
00429
00430 for (col=0; col < adjacencyList->Size(); col++)
00431 {
00432 edgeWeight=(*adjacencyList)[col];
00433 adjacentKey=adjacencyList->GetKeyAtIndex(col);
00434 adjacentIndex=adjacencyLists.GetIndexAtKey(adjacentKey);
00435 adjacentNodeWeight=costMatrix[row*adjacencyLists.Size() + adjacentIndex];
00436
00437 if (currentNodeWeight + edgeWeight < adjacentNodeWeight)
00438 {
00439
00440 for (row2=row; row2 < adjacencyLists.Size(); row2++)
00441 costMatrix[row2*adjacencyLists.Size() + adjacentIndex]=currentNodeWeight + edgeWeight;
00442 openSet.Set(adjacentKey, currentNodeWeight + edgeWeight);
00443 }
00444 }
00445
00446
00447 minHeap.Clear(true,__FILE__,__LINE__);
00448 for (openSetIndex=0; openSetIndex < openSet.Size(); openSetIndex++)
00449 minHeap.Push(openSet[openSetIndex], openSet.GetKeyAtIndex(openSetIndex),__FILE__,__LINE__);
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464 if (minHeap.Size()==0)
00465 {
00466
00467 isValidPath=true;
00468 return;
00469 }
00470
00471 currentNodeWeight=minHeap.PeekWeight(0);
00472 leastNodeArray[row]=minHeap.Pop(0);
00473 currentNode=leastNodeArray[row];
00474 openSet.Delete(currentNode);
00475 row++;
00476 }
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493 isValidPath=true;
00494 }
00495
00496 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00497 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::ClearDijkstra(void)
00498 {
00499 if (isValidPath)
00500 {
00501 isValidPath=false;
00502 RakNet::OP_DELETE_ARRAY(costMatrix, __FILE__, __LINE__);
00503 RakNet::OP_DELETE_ARRAY(leastNodeArray, __FILE__, __LINE__);
00504 costMatrixIndices.Clear(false, __FILE__, __LINE__);
00505 }
00506 }
00507
00508 template <class node_type, class weight_type, bool allow_unlinkedNodes>
00509 void WeightedGraph<node_type, weight_type, allow_unlinkedNodes>::Print(void)
00510 {
00511 #ifdef _DEBUG
00512 unsigned i,j;
00513 for (i=0; i < adjacencyLists.Size(); i++)
00514 {
00515
00516 RAKNET_DEBUG_PRINTF("%s connected to ", adjacencyLists.GetKeyAtIndex(i).systemAddress.ToString());
00517
00518 if (adjacencyLists[i]->Size()==0)
00519 RAKNET_DEBUG_PRINTF("<Empty>");
00520 else
00521 {
00522 for (j=0; j < adjacencyLists[i]->Size(); j++)
00523
00524 RAKNET_DEBUG_PRINTF("%s (%.2f) ", adjacencyLists[i]->GetKeyAtIndex(j).systemAddress.ToString(), (float) adjacencyLists[i]->operator[](j) );
00525 }
00526
00527 RAKNET_DEBUG_PRINTF("\n");
00528 }
00529 #endif
00530 }
00531 }
00532
00533 #ifdef _MSC_VER
00534 #pragma warning( pop )
00535 #endif
00536
00537 #endif