graph.lua

   1 local function Graph_Search(self, first, last)
   2   if first ~= last then
   3     local heuristic = self.h
   4     local open = self.open
   5     while #open > 0 do table.remove(open) end
   6     for _, n in ipairs(self.nodes) do n.s = 0 end
   7
   8     table.insert(open, first)
   9
  10     first.g = 0
  11     first.s = 2
  12
  13     while #open > 0 do
  14       local current = table.remove(open)
  15
  16       if current == last then
  17         first.p = nil
  18         return true
  19       end
  20
  21       current.s = 2
  22       local cd = current.g
  23
  24       for n, d in pairs(current.n) do
  25         if n.s == 0 then
  26           -- Haven't visited this node yet.
  27           n.g = cd+d
  28           n.s = 1
  29           n.p = current
  30           n.h = heuristic(n, last)
  31           local f = n.g+n.h
  32
  33           local mn, mx = 1, #open+1
  34
  35           while mn ~= mx do
  36             local m = math.floor((mn+mx)*0.5)
  37
  38             if open[m].f > f then
  39               mn = m+1
  40             else
  41               mx = m
  42             end
  43           end
  44
  45           table.insert(open, mn, n)
  46           n.f = f
  47         elseif n.s == 1 then
  48           local g = cd+d
  49           local f = g+n.h
  50           if f < n.f then
  51             n.g = g
  52             n.p = current
  53             local of = n.f
  54             local mn, mx = 1, #open
  55
  56             while mn ~= mx do
  57               local m = math.floor((mn+mx)*0.5)
  58               if open[m].f > of then
  59                 mn = m+1
  60               else
  61                 mx = m
  62               end
  63             end
  64
  65             while open[mn] ~= n do
  66               mn = mn + 1
  67             end
  68             mx = #open
  69             table.remove(open, mn)
  70
  71             while mn ~= mx do
  72               local m = math.floor((mn+mx)*0.5)
  73
  74               if open[m].f > f then
  75                 mn = m+1
  76               else
  77                 mx = m
  78               end
  79             end
  80
  81             table.insert(open, mn, n)
  82             n.f = f
  83           end
  84         end
  85       end
  86     end
  87   end
  88   last.p = nil
  89   return first == last
  90 end
  91
  92 local function sanity(list, node)
  93   local last = nil
  94   local contains = false
  95   for i, n in ipairs(list) do
  96     if not (not last or last >= n.f) then
  97       for i, n in ipairs(list) do
  98         QuestHelper:TextOut(i..") "..n.f)
  99       end
 100       QuestHelper:Error("Order "..i.."/"..#list.." ("..n.f..")")
 101     end
 102     assert(not last or last >= n.f)
 103     last = n.f
 104     if n == node then
 105       contains = true
 106     end
 107   end
 108   if node and not contains then QuestHelper:Error("Missing") end
 109 end
 110
 111 local function Graph_Node_Link(self, next_node, distance)
 112   if type(distance) ~= "number" or distance < 0 then QuestHelper:Error("Boom!") end
 113   if self ~= next_node then
 114     self.n[next_node] = distance
 115     table.insert(next_node.r, self)
 116   end
 117 end
 118
 119 local function Graph_CreateNode(self)
 120   local node = QuestHelper:CreateTable()
 121   node.Link = Graph_Node_Link
 122   node.n = QuestHelper:CreateTable()
 123   node.r = QuestHelper:CreateTable()
 124   table.insert(self.nodes, node)
 125   return node
 126 end
 127
 128 local function Graph_DestroyNode(self, node)
 129   for i = 1,#self.nodes do
 130     if self.nodes[i] == node then
 131       table.remove(self.nodes, i)
 132       QuestHelper:ReleaseTable(node.n)
 133       QuestHelper:ReleaseTable(node.r)
 134       QuestHelper:ReleaseTable(node)
 135       break
 136     end
 137   end
 138 end
 139
 140 local function Graph_Reset(self)
 141   while #self.nodes > 0 do
 142     local node = table.remove(self.nodes)
 143     QuestHelper:ReleaseTable(node.n)
 144     QuestHelper:ReleaseTable(node.r)
 145     QuestHelper:ReleaseTable(node)
 146   end
 147 end
 148
 149 -- Tries to find a path from first to last.
 150 -- heuristic is a function that takes two nodes and estimates how long a path between them would be.
 151 -- If a path is found, last.p will be set to the second last node, it's .p will be set to the third
 152 -- last node, and so on and so forth until you get to first, which will have .p set to nil. So, basically
 153 -- you end up with a reversed linked list.
 154
 155 local function Graph_SetHeuristic(self, heuristic)
 156   self.h = heuristic
 157 end
 158
 159 local function Graph_AddRouteStartNode(self, n, g, end_list)
 160   local heuristic = self.h
 161   local open = self.open
 162
 163   n.p = nil
 164
 165   if n.s == 3 then
 166     n.s = 4
 167     n.h = n.e*n.w
 168   elseif n.s == 0 then
 169     n.s = 1
 170
 171     local e = end_list[1]
 172     n.h = (heuristic(n, e)+e.e)*e.w
 173     for i = 2,#end_list do
 174       e = end_list[i]
 175       n.h = math.min(n.h, (heuristic(n, e)+e.e)*e.w)
 176     end
 177   else
 178     local of = n.f
 179     if g+n.h < of then
 180       local mn, mx = 1, #open
 181
 182       while mn ~= mx do
 183         local m = math.floor((mn+mx)*0.5)
 184         if open[m].f > of then
 185           mn = m+1
 186         else
 187           mx = m
 188         end
 189       end
 190
 191       while open[mn] ~= n do
 192         mn = mn + 1
 193       end
 194
 195       table.remove(open, mn)
 196     else
 197       return -- Don't want to insert the node a second time.
 198     end
 199   end
 200
 201   local f = g+n.h
 202
 203   local mn, mx = 1, #open+1
 204
 205   while mn ~= mx do
 206     local m = math.floor((mn+mx)*0.5)
 207
 208     if open[m].f > f then
 209       mn = m+1
 210     else
 211       mx = m
 212     end
 213   end
 214
 215   table.insert(open, mn, n)
 216
 217   n.g = g
 218   n.f = f
 219 end
 220
 221 local function Graph_DoRouteSearch(self, end_list)
 222   local heuristic = self.h
 223   local open = self.open
 224   local end_count = #end_list
 225
 226   while #open > 0 do
 227     local current = table.remove(open)
 228
 229     if current.s == 3 or current.s == 4 then
 230       current.s = 2
 231       return current
 232     end
 233
 234     current.s = 2
 235
 236     local cd = current.g
 237
 238     for n, d in pairs(current.n) do
 239       if n.s == 0 or n.s == 3 then
 240         -- Haven't visited this node yet.
 241         n.p = current
 242         local f
 243
 244         if n.s == 3 then
 245           n.s = 4
 246           n.h = n.e*n.w
 247           n.g = cd+d
 248         else
 249           n.s = 1
 250           n.g = cd+d
 251
 252           local e = end_list[1]
 253           n.h = (heuristic(n, e)+e.e)*e.w
 254
 255           for i = 2,end_count do
 256             e = end_list[i]
 257             n.h = math.min(n.h, (heuristic(n, e)+e.e)*e.w)
 258           end
 259         end
 260
 261         local f = n.g+n.h
 262
 263         local mn, mx = 1, #open+1
 264
 265         while mn ~= mx do
 266           local m = math.floor((mn+mx)*0.5)
 267
 268           if open[m].f > f then
 269             mn = m+1
 270           else
 271             mx = m
 272           end
 273         end
 274
 275         n.f = f
 276
 277         table.insert(open, mn, n)
 278
 279       elseif n.s == 1 or n.s == 4 then
 280         local g = cd+d
 281         local f = g+n.h
 282         if f < n.f then
 283           n.p = current
 284           local of = n.f
 285
 286           local mn, mx = 1, #open
 287
 288           while mn ~= mx do
 289             local m = math.floor((mn+mx)*0.5)
 290             if open[m].f > of then
 291               mn = m+1
 292             else
 293               mx = m
 294             end
 295           end
 296
 297           while open[mn] ~= n do
 298             mn = mn + 1
 299           end
 300
 301           mx = #open
 302
 303           table.remove(open, mn)
 304
 305           while mn ~= mx do
 306             local m = math.floor((mn+mx)*0.5)
 307
 308             if open[m].f > f then
 309               mn = m+1
 310             else
 311               mx = m
 312             end
 313           end
 314
 315           n.f = f
 316           table.insert(open, mn, n)
 317         end
 318       end
 319     end
 320   end
 321 end
 322
 323 local function Graph_PrepareSearch(self)
 324   local open = self.open
 325   while #open > 0 do table.remove(open) end
 326   for _, n in ipairs(self.nodes) do
 327     n.s = 0
 328   end
 329 end
 330
 331 local function Graph_AddStartNode(self, n, g, end_list)
 332   local heuristic = self.h
 333   local open = self.open
 334
 335   n.p = n
 336
 337   if n.s == 3 then
 338     n.s = 4
 339     n.h = n.e*n.w
 340   elseif n.s == 0 then
 341     n.s = 1
 342
 343     local e = end_list[1]
 344     n.h = (heuristic(n, e)+e.e)*e.w
 345     for i = 2,#end_list do
 346       e = end_list[i]
 347       n.h = math.min(n.h, (heuristic(n, e)+e.e)*e.w)
 348     end
 349   else
 350     local of = n.f
 351     if g+n.h < of then
 352       local mn, mx = 1, #open
 353
 354       while mn ~= mx do
 355         local m = math.floor((mn+mx)*0.5)
 356         if open[m].f > of then
 357           mn = m+1
 358         else
 359           mx = m
 360         end
 361       end
 362
 363       while open[mn] ~= n do
 364         mn = mn + 1
 365       end
 366
 367       table.remove(open, mn)
 368     else
 369       return -- Don't want to insert the node a second time.
 370     end
 371   end
 372
 373   local f = g+n.h
 374
 375   local mn, mx = 1, #open+1
 376
 377   while mn ~= mx do
 378     local m = math.floor((mn+mx)*0.5)
 379
 380     if open[m].f > f then
 381       mn = m+1
 382     else
 383       mx = m
 384     end
 385   end
 386
 387   table.insert(open, mn, n)
 388
 389   n.g = g
 390   n.f = f
 391 end
 392
 393 local function Graph_DoSearch(self, end_list)
 394   local heuristic = self.h
 395   local open = self.open
 396   local end_count = #end_list
 397
 398   while #open > 0 do
 399     local current = table.remove(open)
 400
 401     if current.s == 3 or current.s == 4 then
 402       current.s = 2
 403       return current
 404     end
 405
 406     current.s = 2
 407
 408     local cd = current.g
 409
 410     for n, d in pairs(current.n) do
 411       if n.s == 0 or n.s == 3 then
 412         -- Haven't visited this node yet.
 413         n.p = current.p
 414         local f
 415
 416         if n.s == 3 then
 417           n.s = 4
 418           n.h = n.e*n.w
 419           n.g = cd+d
 420         else
 421           n.s = 1
 422           n.g = cd+d
 423
 424           local e = end_list[1]
 425           n.h = (heuristic(n, e)+e.e)*e.w
 426
 427           for i = 2,end_count do
 428             e = end_list[i]
 429             n.h = math.min(n.h, (heuristic(n, e)+e.e)*e.w)
 430           end
 431         end
 432
 433         local f = n.g+n.h
 434
 435         local mn, mx = 1, #open+1
 436
 437         while mn ~= mx do
 438           local m = math.floor((mn+mx)*0.5)
 439
 440           if open[m].f > f then
 441             mn = m+1
 442           else
 443             mx = m
 444           end
 445         end
 446
 447         n.f = f
 448
 449         table.insert(open, mn, n)
 450
 451       elseif n.s == 1 or n.s == 4 then
 452         local g = cd+d
 453         local f = g+n.h
 454         if f < n.f then
 455           n.p = current.p
 456           local of = n.f
 457
 458           local mn, mx = 1, #open
 459
 460           while mn ~= mx do
 461             local m = math.floor((mn+mx)*0.5)
 462             if open[m].f > of then
 463               mn = m+1
 464             else
 465               mx = m
 466             end
 467           end
 468
 469           while open[mn] ~= n do
 470             mn = mn + 1
 471           end
 472
 473           mx = #open
 474
 475           table.remove(open, mn)
 476
 477           while mn ~= mx do
 478             local m = math.floor((mn+mx)*0.5)
 479
 480             if open[m].f > f then
 481               mn = m+1
 482             else
 483               mx = m
 484             end
 485           end
 486
 487           n.f = f
 488           table.insert(open, mn, n)
 489         end
 490       end
 491     end
 492   end
 493 end
 494
 495 local removed = {}
 496
 497 local function Graph_DoFullSearch(self, end_list)
 498   local heuristic = self.h
 499   local open = self.open
 500   local end_count = #end_list
 501
 502   while #open > 0 do
 503     local current = table.remove(open)
 504
 505     if current.s == 3 or current.s == 4 then
 506       if end_count == 1 then
 507         for i = 1,#removed do
 508           table.insert(end_list, table.remove(removed))
 509         end
 510         return
 511       end
 512
 513       for i = 1,end_count do
 514         if end_list[i] == current then
 515           table.insert(removed, table.remove(end_list, i))
 516           break
 517         end
 518       end
 519
 520       end_count = end_count - 1
 521     end
 522
 523     current.s = 2
 524
 525     local cd = current.g
 526
 527     for n, d in pairs(current.n) do
 528       if n.s == 0 or n.s == 3 then
 529         -- Haven't visited this node yet.
 530         n.p = current.p
 531         local f
 532
 533         if n.s == 3 then
 534           n.s = 4
 535           n.h = n.e*n.w
 536           n.g = cd+d
 537         else
 538           n.s = 1
 539           n.g = cd+d
 540
 541           local e = end_list[1]
 542           n.h = (heuristic(n, e)+e.e)*e.w
 543           for i = 2,end_count do
 544             e = end_list[i]
 545             n.h = math.min(n.h, (heuristic(n, e)+e.e)*e.w)
 546           end
 547         end
 548
 549         local f = n.g+n.h
 550
 551         local mn, mx = 1, #open+1
 552
 553         while mn ~= mx do
 554           local m = math.floor((mn+mx)*0.5)
 555
 556           if open[m].f > f then
 557             mn = m+1
 558           else
 559             mx = m
 560           end
 561         end
 562
 563         n.f = f
 564
 565         table.insert(open, mn, n)
 566
 567       elseif n.s == 1 or n.s == 4 then
 568         local g = cd+d
 569         local f = g+n.h
 570         if f < n.f then
 571           n.p = current.p
 572           local of = n.f
 573
 574           local mn, mx = 1, #open
 575
 576           while mn ~= mx do
 577             local m = math.floor((mn+mx)*0.5)
 578             if open[m].f > of then
 579               mn = m+1
 580             else
 581               mx = m
 582             end
 583           end
 584
 585           while open[mn] ~= n do
 586             mn = mn + 1
 587           end
 588
 589           mx = #open
 590
 591           table.remove(open, mn)
 592
 593           while mn ~= mx do
 594             local m = math.floor((mn+mx)*0.5)
 595
 596             if open[m].f > f then
 597               mn = m+1
 598             else
 599               mx = m
 600             end
 601           end
 602
 603           n.f = f
 604           table.insert(open, mn, n)
 605         end
 606       end
 607     end
 608   end
 609
 610   for i, n in ipairs(end_list) do
 611     QuestHelper:TextOut(i..") Failed to reach: "..n.name..", s="..n.s)
 612   end
 613
 614   for i = 1,#removed do
 615     table.insert(end_list, table.remove(removed))
 616   end
 617
 618   for i, n in ipairs(end_list) do
 619     QuestHelper:TextOut(i..") End node: "..n.name..", s="..n.s)
 620   end
 621
 622   QuestHelper:Error("Boom!")
 623
 624   for i = 1,#removed do
 625     table.insert(end_list, table.remove(removed))
 626   end
 627 end
 628
 629 local function propLinks(node)
 630   if node.s ~= 1 then
 631     node.s = 1
 632     for n in pairs(node.n) do
 633       propLinks(n)
 634     end
 635   end
 636 end
 637
 638 function Graph_SanityCheck(self)
 639   for i = 1,#self.nodes do
 640     for _, n in ipairs(self.nodes) do
 641       n.s = 0
 642     end
 643
 644     propLinks(self.nodes[i])
 645
 646     for _, n in ipairs(self.nodes) do
 647       if n.s ~= 1 then
 648         QuestHelper:TextOut((n.name or "unknown").." isn't reachable from "..(self.nodes[i].name or "unknown"))
 649       end
 650     end
 651   end
 652 end
 653
 654 function QuestHelper:CreateGraph()
 655   local graph = self:CreateTable()
 656   graph.nodes = self:CreateTable()
 657   graph.end_nodes = self:CreateTable()
 658   graph.open = self:CreateTable()
 659
 660   graph.CreateNode = Graph_CreateNode
 661   graph.DestroyNode = Graph_DestroyNode
 662   graph.Reset = Graph_Reset
 663
 664   graph.SetHeuristic = Graph_SetHeuristic
 665   graph.PrepareSearch = Graph_PrepareSearch
 666
 667   graph.AddRouteStartNode = Graph_AddRouteStartNode
 668   graph.DoRouteSearch = Graph_DoRouteSearch
 669   graph.Search = Graph_Search
 670   graph.SanityCheck = Graph_SanityCheck
 671
 672   -- These don't deal with finding paths and instead only care about finding distances.
 673   graph.AddStartNode = Graph_AddStartNode
 674   graph.DoSearch = Graph_DoSearch
 675   graph.DoFullSearch = Graph_DoFullSearch
 676
 677   return graph
 678 end
 679
 680 function QuestHelper:ReleaseGraph(graph)
 681   graph:Reset()
 682   self:ReleaseTable(graph.nodes)
 683   self:ReleaseTable(graph.end_nodes)
 684   self:ReleaseTable(graph.open)
 685   self:ReleaseTable(graph)
 686 end
 687
 688 QuestHelper.world_graph = QuestHelper:CreateGraph()