4 "github.com/beefsack/go-astar"
5 "github.com/dragonfireclient/hydra-dragonfire/convert"
6 "github.com/dragonfireclient/mt"
7 "github.com/yuin/gopher-lua"
12 type PathNode struct {
15 edges map[*PathNode]struct{}
18 const pathMaxTp float64 = 4.317 * 10.0 * 0.5
19 const pathMaxTpSq float64 = pathMaxTp * pathMaxTp
21 var pathDirs = [6][3]int16{
30 func pathAddPos(a, b [3]int16) [3]int16 {
37 func pathScalePos(v [3]int16, s int16) (r [3]int16) {
44 func pathDistSq(a, b [3]int16) float64 {
49 abs := math.Abs(float64(v - b[i]))
60 func pathAddEdge(a, b *PathNode) {
61 a.edges[b] = struct{}{}
62 b.edges[a] = struct{}{}
65 func pathAddNode(blk *MapBlk, pos [3]int16) (node *PathNode, ok bool) {
66 node, ok = blk.pathNodes[pos]
74 node.edges = map[*PathNode]struct{}{}
76 blk.pathNodes[pos] = node
80 func pathRemoveEdge(from, to *PathNode) {
81 delete(from.edges, to)
84 if len(from.edges) == 0 {
89 func pathRemoveNode(node *PathNode) {
90 for nbr := range node.edges {
91 pathRemoveEdge(nbr, node)
95 delete(node.blk.pathNodes, node.pos)
99 func pathCheckAddEdge(blk1, blk2 *MapBlk, pos1, pos2 [3]int16, mu *sync.Mutex) bool {
100 if pathDistSq(pos1, pos2) > pathMaxTpSq {
107 node1, _ := pathAddNode(blk1, pos1)
108 node2, _ := pathAddNode(blk2, pos2)
110 pathAddEdge(node1, node2)
114 func pathAddBlock(mp *Map, blk1 *MapBlk, blkpos1 [3]int16) {
115 blk1.pathNodes = map[[3]int16]*PathNode{}
116 nodpos1 := pathScalePos(blkpos1, 16)
119 var chans []chan [3]int16
120 var wg sync.WaitGroup
124 for _, dir := range pathDirs {
125 blkpos2 := pathAddPos(blkpos1, dir)
126 nodpos2 := pathScalePos(blkpos2, 16)
128 blk2, ok := mp.blocks[blkpos2]
133 ch := make(chan [3]int16, 4096)
134 chans = append(chans, ch)
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 {
148 pos2 := pathAddPos(nodpos2, [3]int16{int16(x), int16(y), int16(z)})
150 for _, pos1 := range positions {
151 if pathCheckAddEdge(blk1, blk2, pos1, pos2, &mu) {
159 if pathCheckAddEdge(blk1, blk2, pos1, pos2, &mu) {
162 positions = append(positions, pos1)
166 if len(positions) == 0 {
178 for _, ch := range chans {
182 for x := uint16(0); x < 16; x++ {
183 for z := uint16(0); z < 16; z++ {
184 for y := uint16(0); y < 16; y++ {
189 if blk1.data.Param0[x|(y<<4)|(z<<8)] != mt.Air {
193 for _, ch := range chans {
194 ch <- pathAddPos(nodpos1, [3]int16{int16(x), int16(y), int16(z)})
206 func pathRemoveBlock(blk *MapBlk) {
207 for _, node := range blk.pathNodes {
213 func (node *PathNode) PathNeighbors() (edges []astar.Pather) {
214 for node := range node.edges {
215 edges = append(edges, node)
217 for _, node := range node.blk.pathNodes {
218 edges = append(edges, node)
223 func (node *PathNode) PathNeighborCost(to astar.Pather) float64 {
224 return node.PathEstimatedCost(to)
227 func (node *PathNode) PathEstimatedCost(to astar.Pather) float64 {
228 return pathDistSq(node.pos, to.(*PathNode).pos)
231 func pathFind(mp *Map, pos [2][3]int16, l *lua.LState) lua.LValue {
239 blkpos, _ := mt.Pos2Blkpos(pos[i])
240 blk, ok := mp.blocks[blkpos]
245 abs[i].node, abs[i].ex = pathAddNode(blk, pos[i])
248 // reverse dst and src due to bug in astar package
249 path, _, found := astar.Path(abs[1].node, abs[0].node)
256 pathRemoveNode(abs[i].node)
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])}
265 l.RawSetInt(tbl, i+1, convert.PushVec3(l, lpos))