path.go

   1 package main
   2
   3 import (
   4         "github.com/beefsack/go-astar"
   5         "github.com/dragonfireclient/hydra-dragonfire/convert"
   6         "github.com/dragonfireclient/mt"
   7         "github.com/yuin/gopher-lua"
   8         "math"
   9         "sync"
  10 )
  11
  12 type PathNode struct {
  13         pos   [3]int16
  14         blk   *MapBlk
  15         edges map[*PathNode]struct{}
  16 }
  17
  18 const pathMaxTp float64 = 4.317 * 10.0 * 0.5
  19 const pathMaxTpSq float64 = pathMaxTp * pathMaxTp
  20
  21 var pathDirs = [6][3]int16{
  22         [3]int16{+1, 0, 0},
  23         [3]int16{-1, 0, 0},
  24         [3]int16{0, +1, 0},
  25         [3]int16{0, -1, 0},
  26         [3]int16{0, 0, +1},
  27         [3]int16{0, 0, -1},
  28 }
  29
  30 func pathAddPos(a, b [3]int16) [3]int16 {
  31         for i, v := range a {
  32                 b[i] += v
  33         }
  34         return b
  35 }
  36
  37 func pathScalePos(v [3]int16, s int16) (r [3]int16) {
  38         for i, x := range v {
  39                 r[i] = x * s
  40         }
  41         return
  42 }
  43
  44 func pathDistSq(a, b [3]int16) float64 {
  45         distSq := 0.0
  46
  47         for i, v := range a {
  48                 if i != 1 {
  49                         abs := math.Abs(float64(v - b[i]))
  50                         if abs > 0 {
  51                                 abs -= 1
  52                         }
  53                         distSq += abs
  54                 }
  55         }
  56
  57         return distSq
  58 }
  59
  60 func pathAddEdge(a, b *PathNode) {
  61         a.edges[b] = struct{}{}
  62         b.edges[a] = struct{}{}
  63 }
  64
  65 func pathAddNode(blk *MapBlk, pos [3]int16) (node *PathNode, ok bool) {
  66         node, ok = blk.pathNodes[pos]
  67         if ok {
  68                 return
  69         }
  70
  71         node = &PathNode{}
  72         node.pos = pos
  73         node.blk = blk
  74         node.edges = map[*PathNode]struct{}{}
  75
  76         blk.pathNodes[pos] = node
  77         return
  78 }
  79
  80 func pathRemoveEdge(from, to *PathNode) {
  81         delete(from.edges, to)
  82
  83         // garbage collect
  84         if len(from.edges) == 0 {
  85                 pathRemoveNode(from)
  86         }
  87 }
  88
  89 func pathRemoveNode(node *PathNode) {
  90         for nbr := range node.edges {
  91                 pathRemoveEdge(nbr, node)
  92         }
  93
  94         if node.blk != nil {
  95                 delete(node.blk.pathNodes, node.pos)
  96         }
  97 }
  98
  99 func pathCheckAddEdge(blk1, blk2 *MapBlk, pos1, pos2 [3]int16, mu *sync.Mutex) bool {
 100         if pathDistSq(pos1, pos2) > pathMaxTpSq {
 101                 return false
 102         }
 103
 104         mu.Lock()
 105         defer mu.Unlock()
 106
 107         node1, _ := pathAddNode(blk1, pos1)
 108         node2, _ := pathAddNode(blk2, pos2)
 109
 110         pathAddEdge(node1, node2)
 111         return true
 112 }
 113
 114 func pathAddBlock(mp *Map, blk1 *MapBlk, blkpos1 [3]int16) {
 115         blk1.pathNodes = map[[3]int16]*PathNode{}
 116         nodpos1 := pathScalePos(blkpos1, 16)
 117
 118         // sync stuff
 119         var chans []chan [3]int16
 120         var wg sync.WaitGroup
 121         var mu sync.Mutex
 122         var done bool
 123
 124         for _, dir := range pathDirs {
 125                 blkpos2 := pathAddPos(blkpos1, dir)
 126                 nodpos2 := pathScalePos(blkpos2, 16)
 127
 128                 blk2, ok := mp.blocks[blkpos2]
 129                 if !ok {
 130                         continue
 131                 }
 132
 133                 ch := make(chan [3]int16, 4096)
 134                 chans = append(chans, ch)
 135                 wg.Add(1)
 136
 137                 go func() {
 138                         defer wg.Done()
 139
 140                         var positions [][3]int16
 141                         for x := uint16(0); x < 16; x++ {
 142                                 for z := uint16(0); z < 16; z++ {
 143                                         for y := uint16(0); y < 16; y++ {
 144                                                 if blk2.data.Param0[x|(y<<4)|(z<<8)] != mt.Air {
 145                                                         continue
 146                                                 }
 147
 148                                                 pos2 := pathAddPos(nodpos2, [3]int16{int16(x), int16(y), int16(z)})
 149
 150                                                 for _, pos1 := range positions {
 151                                                         if pathCheckAddEdge(blk1, blk2, pos1, pos2, &mu) {
 152                                                                 return
 153                                                         }
 154                                                 }
 155
 156                                                 for ch != nil {
 157                                                         pos1, ok := <-ch
 158                                                         if ok {
 159                                                                 if pathCheckAddEdge(blk1, blk2, pos1, pos2, &mu) {
 160                                                                         return
 161                                                                 } else {
 162                                                                         positions = append(positions, pos1)
 163                                                                 }
 164                                                         } else {
 165                                                                 ch = nil
 166                                                                 if len(positions) == 0 {
 167                                                                         return
 168                                                                 }
 169                                                         }
 170                                                 }
 171                                         }
 172                                 }
 173                         }
 174                 }()
 175         }
 176
 177         go func() {
 178                 for _, ch := range chans {
 179                         defer close(ch)
 180                 }
 181
 182                 for x := uint16(0); x < 16; x++ {
 183                         for z := uint16(0); z < 16; z++ {
 184                                 for y := uint16(0); y < 16; y++ {
 185                                         if done {
 186                                                 return
 187                                         }
 188
 189                                         if blk1.data.Param0[x|(y<<4)|(z<<8)] != mt.Air {
 190                                                 continue
 191                                         }
 192
 193                                         for _, ch := range chans {
 194                                                 ch <- pathAddPos(nodpos1, [3]int16{int16(x), int16(y), int16(z)})
 195                                         }
 196                                         break
 197                                 }
 198                         }
 199                 }
 200         }()
 201
 202         wg.Wait()
 203         done = true
 204 }
 205
 206 func pathRemoveBlock(blk *MapBlk) {
 207         for _, node := range blk.pathNodes {
 208                 node.blk = nil
 209                 pathRemoveNode(node)
 210         }
 211 }
 212
 213 func (node *PathNode) PathNeighbors() (edges []astar.Pather) {
 214         for node := range node.edges {
 215                 edges = append(edges, node)
 216         }
 217         for _, node := range node.blk.pathNodes {
 218                 edges = append(edges, node)
 219         }
 220         return
 221 }
 222
 223 func (node *PathNode) PathNeighborCost(to astar.Pather) float64 {
 224         return node.PathEstimatedCost(to)
 225 }
 226
 227 func (node *PathNode) PathEstimatedCost(to astar.Pather) float64 {
 228         return pathDistSq(node.pos, to.(*PathNode).pos)
 229 }
 230
 231 func pathFind(mp *Map, pos [2][3]int16, l *lua.LState) lua.LValue {
 232         var abs [2]struct {
 233                 blk  *MapBlk
 234                 node *PathNode
 235                 ex   bool
 236         }
 237
 238         for i := range abs {
 239                 blkpos, _ := mt.Pos2Blkpos(pos[i])
 240                 blk, ok := mp.blocks[blkpos]
 241                 if !ok {
 242                         return lua.LNil
 243                 }
 244
 245                 abs[i].node, abs[i].ex = pathAddNode(blk, pos[i])
 246         }
 247
 248         // reverse dst and src due to bug in astar package
 249         path, _, found := astar.Path(abs[1].node, abs[0].node)
 250         if !found {
 251                 return lua.LNil
 252         }
 253
 254         for i := range abs {
 255                 if !abs[i].ex {
 256                         pathRemoveNode(abs[i].node)
 257                 }
 258         }
 259
 260         tbl := l.NewTable()
 261         for i, pather := range path {
 262                 pos := pather.(*PathNode).pos
 263                 lpos := [3]lua.LNumber{lua.LNumber(pos[0]), lua.LNumber(pos[1]), lua.LNumber(pos[2])}
 264
 265                 l.RawSetInt(tbl, i+1, convert.PushVec3(l, lpos))
 266         }
 267         return tbl
 268 }