From: Elias Fleckenstein Date: Fri, 3 Jun 2022 15:42:59 +0000 (+0200) Subject: Add path finding X-Git-Tag: v0.2.0~6 X-Git-Url: https://git.lizzy.rs/?a=commitdiff_plain;h=1f94c5aef9d9c69b940cbdbf1f9443f20f1e7ba9;p=hydra-dragonfire.git Add path finding --- diff --git a/builtin/vector.lua b/builtin/vector.lua index 654a839..1b3a3e7 100644 --- a/builtin/vector.lua +++ b/builtin/vector.lua @@ -11,6 +11,7 @@ local function arith_mt(...) __mul = wrap("*", ...), __div = wrap("/", ...), __mod = wrap("%", ...), + __index = {}, } end @@ -31,23 +32,42 @@ function mt_vec2:__neg() end function mt_vec2:__len() - return math.sqrt(self.x ^ 2 + self.y ^ 2) + return math.sqrt(self:dot(self)) +end + +function mt_vec2:__equ(other) + return type(other) == "table" and self.x == other.x and self.y == other.y end function mt_vec2:__tostring() return "(" .. self.x .. ", " .. self.y .. ")" end -mt_vec2.__index = { - validate = function(self) - assert(type(self.x) == "number") - assert(type(self.y) == "number") - return self - end, - round = function(self) - return vec2(math.round(self.x), math.round(self.y)) - end, -} +function mt_vec2.__index:validate() + assert(type(self.x) == "number") + assert(type(self.y) == "number") + return self +end + +function mt_vec2.__index:round() + return vec2(math.round(self.x), math.round(self.y)) +end + +function mt_vec2.__index:manhattan() + return math.abs(self.x) + math.abs(self.y) +end + +function mt_vec2.__index:volume() + return self.x * self.y +end + +function mt_vec2.__index:dot(other) + return self.x * other.x + self.y * other.y +end + +function mt_vec2.__index:norm() + return self / #self +end function vec2(a, b) local o = {} @@ -81,24 +101,51 @@ function mt_vec3:__neg() end function mt_vec3:__len() - return math.sqrt(self.x ^ 2 + self.y ^ 2 + self.z ^ 2) + return math.sqrt(self:dot(self)) +end + +function mt_vec3:__equ(other) + return type(other) == "table" and self.x == other.x and self.y == other.y and self.z == other.z end function mt_vec3:__tostring() return "(" .. self.x .. ", " .. self.y .. ", " .. self.z .. ")" end -mt_vec3.__index = { - validate = function(self) - assert(type(self.x) == "number") - assert(type(self.y) == "number") - assert(type(self.z) == "number") - return self - end, - round = function(self) - return vec3(math.floor(self.x), math.floor(self.y), math.floor(self.z)) - end, -} +function mt_vec3.__index:validate() + assert(type(self.x) == "number") + assert(type(self.y) == "number") + assert(type(self.z) == "number") + return self +end + +function mt_vec3.__index:round() + return vec3(math.floor(self.x), math.floor(self.y), math.floor(self.z)) +end + +function mt_vec3.__index:manhattan() + return math.abs(self.x) + math.abs(self.y) + math.abs(self.z) +end + +function mt_vec3.__index:volume() + return self.x * self.y * self.z +end + +function mt_vec3.__index:dot(other) + return self.x * other.x + self.y * other.y + self.z * other.z +end + +function mt_vec3.__index:cross(other) + return vec3( + self.y * other.z - self.z * other.y, + self.z * other.x - self.x * other.z, + self.x * other.y - self.y * other.x + ) +end + +function mt_vec3.__index:norm() + return self / #self +end function vec3(a, b, c) local o = {} @@ -127,28 +174,40 @@ function mt_box:__neg() return box(-self.min, -self.max) end +function mt_box:__eq(other) + return self.min == other.min and self.max == other.max +end + function mt_box:__tostring() return "[" .. self.min .. "; " .. self.max .. "]" end -mt_box.__index = { - contains = function(a, b) - if type(b) == "number" or b.x then - return a.min <= b and a.max >= b - else - return a.min <= b.min and a.max >= b.max - end - end, - validate = function(self) - if type(self.min) == "number" then - assert(type(self.max) == "number") - else - assert(not self.min.z == not self.max.z) - self.min:validate() - self.max:validate() - end - end, -} +function mt_box.__index:validate() + if type(self.min) == "number" then + assert(type(self.max) == "number") + else + assert(not self.min.z == not self.max.z) + self.min:validate() + self.max:validate() + end +end + +function mt_box.__index:volume() + local diff = self.max - self.min + if type(diff) == "number" then + return diff + else + return diff:volume() + end +end + +function mt_box.__index:contains(other) + if type(other) == "number" or other.x then + return self.min <= other and self.max >= other + else + return self.min <= other.min and self.max >= other.max + end +end function box(a, b) local o = {} diff --git a/comp_map.go b/comp_map.go index c45155e..eed5039 100644 --- a/comp_map.go +++ b/comp_map.go @@ -7,7 +7,7 @@ import ( type CompMap struct { client *Client - mapdata *Map + mapdata *Map userdata *lua.LUserData } diff --git a/example/pathfind.lua b/example/pathfind.lua new file mode 100755 index 0000000..a9b0723 --- /dev/null +++ b/example/pathfind.lua @@ -0,0 +1,22 @@ +#!/usr/bin/env hydra-dragonfire +local client = require("client")() + +client:enable("map") +client.map:set(hydra.map(true)) + +client:enable("pkts") +client.pkts:subscribe("chat_msg") + +client:connect() + +while true do + local evt = client:poll() + + if not evt or evt.type == "disconnect" or evt.type == "interrupt" then + break + elseif evt.type == "pkt" then + print("chatmsg") + end +end + +client:close() diff --git a/go.mod b/go.mod index 26efc76..5793149 100644 --- a/go.mod +++ b/go.mod @@ -7,3 +7,5 @@ require ( github.com/anon55555/mt v0.0.0-20210919124550-bcc58cb3048f github.com/yuin/gopher-lua v0.0.0-20220504180219-658193537a64 ) + +require github.com/beefsack/go-astar v0.0.0-20200827232313-4ecf9e304482 // indirect diff --git a/go.sum b/go.sum index 0717da6..9fcc542 100644 --- a/go.sum +++ b/go.sum @@ -2,5 +2,7 @@ github.com/HimbeerserverDE/srp v0.0.0 h1:Iy2GIF7DJphXXO9NjncLEBO6VsZd8Yhrlxl/qTr github.com/HimbeerserverDE/srp v0.0.0/go.mod h1:pxNH8S2nh4n2DWE0ToX5GnnDr/uEAuaAhJsCpkDLIWw= github.com/anon55555/mt v0.0.0-20210919124550-bcc58cb3048f h1:tZU8VPYLyRrG3Lj9zBZvTVF5tUGciC/2aUIgTcU4WaM= github.com/anon55555/mt v0.0.0-20210919124550-bcc58cb3048f/go.mod h1:jH4ER+ahjl7H6TczzK+q4V9sXY++U2Geh6/vt3r4Xvs= +github.com/beefsack/go-astar v0.0.0-20200827232313-4ecf9e304482 h1:p4g4uok3+r6Tg6fxXEQUAcMAX/WdK6WhkQW9s0jaT7k= +github.com/beefsack/go-astar v0.0.0-20200827232313-4ecf9e304482/go.mod h1:Cu3t5VeqE8kXjUBeNXWQprfuaP5UCIc5ggGjgMx9KFc= github.com/yuin/gopher-lua v0.0.0-20220504180219-658193537a64 h1:5mLPGnFdSsevFRFc9q3yYbBkB6tsm4aCwwQV/j1JQAQ= github.com/yuin/gopher-lua v0.0.0-20220504180219-658193537a64/go.mod h1:GBR0iDaNXjAgGg9zfCvksxSRnQx76gclCIb7kdAd1Pw= diff --git a/hydra.go b/hydra.go index 6da1822..6caff4c 100644 --- a/hydra.go +++ b/hydra.go @@ -40,7 +40,7 @@ var builtinFiles = []string{ var hydraFuncs = map[string]lua.LGFunction{ "client": l_client, - "map": l_map, + "map": l_map, "dtime": l_dtime, "poll": l_poll, "close": l_close, diff --git a/map.go b/map.go index 1ffae23..f877dca 100644 --- a/map.go +++ b/map.go @@ -7,15 +7,22 @@ import ( "sync" ) +type MapBlk struct { + data *mt.MapBlk + pathNodes map[[3]int16]*PathNode +} + type Map struct { mu sync.Mutex - blocks map[[3]int16]*mt.MapBlk + pathfind bool + blocks map[[3]int16]*MapBlk userdata *lua.LUserData } var mapFuncs = map[string]lua.LGFunction{ "block": l_map_block, "node": l_map_node, + "path": l_map_path, } func getMap(l *lua.LState, idx int) *Map { @@ -24,7 +31,7 @@ func getMap(l *lua.LState, idx int) *Map { func newMap(l *lua.LState) *Map { mp := &Map{} - mp.blocks = map[[3]int16]*mt.MapBlk{} + mp.blocks = map[[3]int16]*MapBlk{} mp.userdata = l.NewUserData() mp.userdata.Value = mp l.SetMetatable(mp.userdata, l.GetTypeMetatable("hydra.map")) @@ -34,15 +41,31 @@ func newMap(l *lua.LState) *Map { func (mp *Map) process(client *Client, pkt *mt.Pkt) { switch cmd := pkt.Cmd.(type) { case *mt.ToCltBlkData: - mp.mu.Lock() - mp.blocks[cmd.Blkpos] = &cmd.Blk - mp.mu.Unlock() + go func() { + mp.mu.Lock() + defer mp.mu.Unlock() + + blk := &MapBlk{} + blk.data = &cmd.Blk + + if mp.pathfind { + if oldblk, ok := mp.blocks[cmd.Blkpos]; ok { + pathRemoveBlock(oldblk) + } + + pathAddBlock(mp, blk, cmd.Blkpos) + } + + mp.blocks[cmd.Blkpos] = blk + }() + client.conn.SendCmd(&mt.ToSrvGotBlks{Blks: [][3]int16{cmd.Blkpos}}) } } func l_map(l *lua.LState) int { mp := newMap(l) + mp.pathfind = l.ToBool(1) l.Push(mp.userdata) return 1 } @@ -55,9 +78,9 @@ func l_map_block(l *lua.LState) int { mp.mu.Lock() defer mp.mu.Unlock() - block, ok := mp.blocks[blkpos] + blk, ok := mp.blocks[blkpos] if ok { - l.Push(convert.PushMapBlk(l, *block)) + l.Push(convert.PushMapBlk(l, *blk.data)) } else { l.Push(lua.LNil) } @@ -75,17 +98,17 @@ func l_map_node(l *lua.LState) int { mp.mu.Lock() defer mp.mu.Unlock() - block, block_exists := mp.blocks[blkpos] - if block_exists { - meta, meta_exists := block.NodeMetas[i] - if !meta_exists { + blk, blk_ok := mp.blocks[blkpos] + if blk_ok { + meta, meta_ok := blk.data.NodeMetas[i] + if !meta_ok { meta = &mt.NodeMeta{} } lnode := l.NewTable() - l.SetField(lnode, "param0", lua.LNumber(block.Param0[i])) - l.SetField(lnode, "param1", lua.LNumber(block.Param1[i])) - l.SetField(lnode, "param2", lua.LNumber(block.Param2[i])) + l.SetField(lnode, "param0", lua.LNumber(blk.data.Param0[i])) + l.SetField(lnode, "param1", lua.LNumber(blk.data.Param1[i])) + l.SetField(lnode, "param2", lua.LNumber(blk.data.Param2[i])) l.SetField(lnode, "meta", convert.PushNodeMeta(l, *meta)) l.Push(lnode) } else { @@ -94,3 +117,20 @@ func l_map_node(l *lua.LState) int { return 1 } + +func l_map_path(l *lua.LState) int { + mp := getMap(l, 1) + if !mp.pathfind { + panic("map not configured to support path finding") + } + + var src, dst [3]int16 + convert.ReadVec3Int16(l, l.Get(2), &src) + convert.ReadVec3Int16(l, l.Get(3), &dst) + + mp.mu.Lock() + defer mp.mu.Unlock() + + l.Push(pathFind(mp, [2][3]int16{src, dst}, l)) + return 1 +} diff --git a/path.go b/path.go new file mode 100644 index 0000000..d8d9a0b --- /dev/null +++ b/path.go @@ -0,0 +1,268 @@ +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 +}