--- /dev/null
+package main
+
+import (
+ "github.com/anon55555/mt"
+ "github.com/beefsack/go-astar"
+ "github.com/dragonfireclient/hydra-dragonfire/convert"
+ "github.com/yuin/gopher-lua"
+ "math"
+ "sync"
+)
+
+type PathNode struct {
+ pos [3]int16
+ blk *MapBlk
+ edges map[*PathNode]struct{}
+}
+
+const pathMaxTp float64 = 4.317 * 10.0 * 0.5
+const pathMaxTpSq float64 = pathMaxTp * pathMaxTp
+
+var pathDirs = [6][3]int16{
+ [3]int16{+1, 0, 0},
+ [3]int16{-1, 0, 0},
+ [3]int16{0, +1, 0},
+ [3]int16{0, -1, 0},
+ [3]int16{0, 0, +1},
+ [3]int16{0, 0, -1},
+}
+
+func pathAddPos(a, b [3]int16) [3]int16 {
+ for i, v := range a {
+ b[i] += v
+ }
+ return b
+}
+
+func pathScalePos(v [3]int16, s int16) (r [3]int16) {
+ for i, x := range v {
+ r[i] = x * s
+ }
+ return
+}
+
+func pathDistSq(a, b [3]int16) float64 {
+ distSq := 0.0
+
+ for i, v := range a {
+ if i != 1 {
+ abs := math.Abs(float64(v - b[i]))
+ if abs > 0 {
+ abs -= 1
+ }
+ distSq += abs
+ }
+ }
+
+ return distSq
+}
+
+func pathAddEdge(a, b *PathNode) {
+ a.edges[b] = struct{}{}
+ b.edges[a] = struct{}{}
+}
+
+func pathAddNode(blk *MapBlk, pos [3]int16) (node *PathNode, ok bool) {
+ node, ok = blk.pathNodes[pos]
+ if ok {
+ return
+ }
+
+ node = &PathNode{}
+ node.pos = pos
+ node.blk = blk
+ node.edges = map[*PathNode]struct{}{}
+
+ blk.pathNodes[pos] = node
+ return
+}
+
+func pathRemoveEdge(from, to *PathNode) {
+ delete(from.edges, to)
+
+ // garbage collect
+ if len(from.edges) == 0 {
+ pathRemoveNode(from)
+ }
+}
+
+func pathRemoveNode(node *PathNode) {
+ for nbr := range node.edges {
+ pathRemoveEdge(nbr, node)
+ }
+
+ if node.blk != nil {
+ delete(node.blk.pathNodes, node.pos)
+ }
+}
+
+func pathCheckAddEdge(blk1, blk2 *MapBlk, pos1, pos2 [3]int16, mu *sync.Mutex) bool {
+ if pathDistSq(pos1, pos2) > pathMaxTpSq {
+ return false
+ }
+
+ mu.Lock()
+ defer mu.Unlock()
+
+ node1, _ := pathAddNode(blk1, pos1)
+ node2, _ := pathAddNode(blk2, pos2)
+
+ pathAddEdge(node1, node2)
+ return true
+}
+
+func pathAddBlock(mp *Map, blk1 *MapBlk, blkpos1 [3]int16) {
+ blk1.pathNodes = map[[3]int16]*PathNode{}
+ nodpos1 := pathScalePos(blkpos1, 16)
+
+ // sync stuff
+ var chans []chan [3]int16
+ var wg sync.WaitGroup
+ var mu sync.Mutex
+ var done bool
+
+ for _, dir := range pathDirs {
+ blkpos2 := pathAddPos(blkpos1, dir)
+ nodpos2 := pathScalePos(blkpos2, 16)
+
+ blk2, ok := mp.blocks[blkpos2]
+ if !ok {
+ continue
+ }
+
+ ch := make(chan [3]int16, 4096)
+ chans = append(chans, ch)
+ wg.Add(1)
+
+ go func() {
+ defer wg.Done()
+
+ var positions [][3]int16
+ for x := uint16(0); x < 16; x++ {
+ for z := uint16(0); z < 16; z++ {
+ for y := uint16(0); y < 16; y++ {
+ if blk2.data.Param0[x|(y<<4)|(z<<8)] != mt.Air {
+ continue
+ }
+
+ pos2 := pathAddPos(nodpos2, [3]int16{int16(x), int16(y), int16(z)})
+
+ for _, pos1 := range positions {
+ if pathCheckAddEdge(blk1, blk2, pos1, pos2, &mu) {
+ return
+ }
+ }
+
+ for ch != nil {
+ pos1, ok := <-ch
+ if ok {
+ if pathCheckAddEdge(blk1, blk2, pos1, pos2, &mu) {
+ return
+ } else {
+ positions = append(positions, pos1)
+ }
+ } else {
+ ch = nil
+ if len(positions) == 0 {
+ return
+ }
+ }
+ }
+ }
+ }
+ }
+ }()
+ }
+
+ go func() {
+ for _, ch := range chans {
+ defer close(ch)
+ }
+
+ for x := uint16(0); x < 16; x++ {
+ for z := uint16(0); z < 16; z++ {
+ for y := uint16(0); y < 16; y++ {
+ if done {
+ return
+ }
+
+ if blk1.data.Param0[x|(y<<4)|(z<<8)] != mt.Air {
+ continue
+ }
+
+ for _, ch := range chans {
+ ch <- pathAddPos(nodpos1, [3]int16{int16(x), int16(y), int16(z)})
+ }
+ break
+ }
+ }
+ }
+ }()
+
+ wg.Wait()
+ done = true
+}
+
+func pathRemoveBlock(blk *MapBlk) {
+ for _, node := range blk.pathNodes {
+ node.blk = nil
+ pathRemoveNode(node)
+ }
+}
+
+func (node *PathNode) PathNeighbors() (edges []astar.Pather) {
+ for node := range node.edges {
+ edges = append(edges, node)
+ }
+ for _, node := range node.blk.pathNodes {
+ edges = append(edges, node)
+ }
+ return
+}
+
+func (node *PathNode) PathNeighborCost(to astar.Pather) float64 {
+ return node.PathEstimatedCost(to)
+}
+
+func (node *PathNode) PathEstimatedCost(to astar.Pather) float64 {
+ return pathDistSq(node.pos, to.(*PathNode).pos)
+}
+
+func pathFind(mp *Map, pos [2][3]int16, l *lua.LState) lua.LValue {
+ var abs [2]struct {
+ blk *MapBlk
+ node *PathNode
+ ex bool
+ }
+
+ for i := range abs {
+ blkpos, _ := mt.Pos2Blkpos(pos[i])
+ blk, ok := mp.blocks[blkpos]
+ if !ok {
+ return lua.LNil
+ }
+
+ abs[i].node, abs[i].ex = pathAddNode(blk, pos[i])
+ }
+
+ // reverse dst and src due to bug in astar package
+ path, _, found := astar.Path(abs[1].node, abs[0].node)
+ if !found {
+ return lua.LNil
+ }
+
+ for i := range abs {
+ if !abs[i].ex {
+ pathRemoveNode(abs[i].node)
+ }
+ }
+
+ tbl := l.NewTable()
+ for i, pather := range path {
+ pos := pather.(*PathNode).pos
+ lpos := [3]lua.LNumber{lua.LNumber(pos[0]), lua.LNumber(pos[1]), lua.LNumber(pos[2])}
+
+ l.RawSetInt(tbl, i+1, convert.PushVec3(l, lpos))
+ }
+ return tbl
+}
projects / hydra-dragonfire.git / blobdiff