Skip to content

Commit 865bca1

Browse files
bolom009bolom009
committed
feat: updated astar version, tests
1 parent 40e4c1f commit 865bca1

File tree

8 files changed

+102
-43
lines changed

8 files changed

+102
-43
lines changed

‎demo/recast/main.go‎

Lines changed: 18 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@ package main
22

33
import (
44
"context"
5+
"encoding/json"
56
"fmt"
67
"math"
78
"time"
@@ -41,9 +42,11 @@ func main() {
4142
}
4243

4344
var (
44-
screen = geom.Vector2{800, 600}
45-
camera = rl.NewCamera2D(rl.NewVector2(0, 0), rl.NewVector2(-screen.X/2, -screen.Y/2), 0, 0.5)
46-
path = make([]geom.Vector2, 0)
45+
screen = geom.Vector2{800, 600}
46+
camera = rl.NewCamera2D(rl.NewVector2(0, 0), rl.NewVector2(-screen.X/2, -screen.Y/2), 0, 0.5)
47+
path = make([]geom.Vector2, 0)
48+
//start = geom.Vector2{X: 192, Y: -267}
49+
//dest = geom.Vector2{X: -4, Y: 80}
4750
start = geom.Vector2{X: 202, Y: -268}
4851
dest = geom.Vector2{X: -4, Y: 84}
4952
extraObstacles = map[uint32]*mesh.Hole{}
@@ -71,12 +74,23 @@ func main() {
7174
path = pathfinder.Path(graphId, start, dest)
7275
pathTime = time.Since(t).String()
7376

74-
visGraph := pathfinder.Graph(graphId)
77+
visGraph := pathfinder.GraphWithSearchPath(graphId, start, dest)
7578
for _, edges := range visGraph {
7679
vertexCount++
7780
edgesCount += len(edges)
7881
}
7982

83+
m := make(map[string][]geom.Vector2)
84+
ccc := 0
85+
for k, v := range visGraph {
86+
m[fmt.Sprintf("{%v,%v}", k.X, k.Y)] = v
87+
ccc += len(v)
88+
}
89+
90+
fmt.Println("PATH", len(visGraph), ccc, path)
91+
b, _ := json.Marshal(m)
92+
fmt.Println(412412, string(b))
93+
8094
trianglesCount = len(recastGraph.Triangles())
8195
triangles := recastGraph.Triangles()
8296

‎go.mod‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ module github.com/bolom009/pathfind
33
go 1.25
44

55
require (
6-
github.com/bolom009/astar v1.0.1
6+
github.com/bolom009/astar v1.1.0
77
github.com/bolom009/delaunay v1.1.0
88
github.com/bolom009/geom v1.7.0
99
github.com/bolom009/go-clipper2 v1.1.4
@@ -15,7 +15,6 @@ require (
1515
require (
1616
github.com/davecgh/go-spew v1.1.1 // indirect
1717
github.com/ebitengine/purego v0.8.2 // indirect
18-
github.com/fzipp/astar v0.3.0 // indirect
1918
github.com/govalues/decimal v0.1.36 // indirect
2019
github.com/pmezard/go-difflib v1.0.0 // indirect
2120
golang.org/x/exp v0.0.0-20250911091902-df9299821621 // indirect

‎go.sum‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
1-
github.com/bolom009/astar v1.0.1 h1:be17S68tyYD6aZD0IMjU2sUX19w6nJhLbmBlx1aIJEg=
2-
github.com/bolom009/astar v1.0.1/go.mod h1:2AGXVxVkduNPNnkJ0pYpqkJCfCDfFtZgtQ/JyFeZN2k=
1+
github.com/bolom009/astar v1.1.0 h1:tuUI6BKeiC+p/7W29wkOh9AwjeKdK8T9MCOSE61krzU=
2+
github.com/bolom009/astar v1.1.0/go.mod h1:Dq4u0l/vaYDkevwNsIBNTVDrK9+J1ylYfDY7NkmU/gs=
33
github.com/bolom009/delaunay v1.1.0 h1:ezcqKjoW11tMj99aPPLkkzYujHhNsQXrEuZH0H/ZiC0=
44
github.com/bolom009/delaunay v1.1.0/go.mod h1:DPz4UjKa218LrCbPxN8eq86fxAbIHHhKKGLATMZ2DbQ=
55
github.com/bolom009/geom v1.7.0 h1:fmGnSMM5vz1EmjNrrJZlIz6SMJgzYspBMgQAerlWwdA=
@@ -10,8 +10,6 @@ github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c
1010
github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=
1111
github.com/ebitengine/purego v0.8.2 h1:jPPGWs2sZ1UgOSgD2bClL0MJIqu58nOmIcBuXr62z1I=
1212
github.com/ebitengine/purego v0.8.2/go.mod h1:iIjxzd6CiRiOG0UyXP+V1+jWqUXVjPKLAI0mRfJZTmQ=
13-
github.com/fzipp/astar v0.3.0 h1:ONok5YsDmFAVpBvKBP30wU3YDNVU101VRWBTRCUsGqA=
14-
github.com/fzipp/astar v0.3.0/go.mod h1:KWXlNb4EkWXPckLv812VKCBie5lg3I38jj05whtQozM=
1513
github.com/gen2brain/raylib-go/raygui v0.0.0-20250215042252-db8e47f0e5c5 h1:RBIUgStpzR3g5g7qIkK4+sSbYXVTQXiXGqRZ84j3+ZY=
1614
github.com/gen2brain/raylib-go/raygui v0.0.0-20250215042252-db8e47f0e5c5/go.mod h1:Ji/uPEko2AUkcyPLAelEUa+E8Npc89/XY5Fo/lS/e3I=
1715
github.com/gen2brain/raylib-go/raylib v0.0.0-20250215042252-db8e47f0e5c5 h1:k8ZAxLgb/p5TvCi5VHFHM8JdnjwShNK4A0bLIwbktAU=

‎graphs/graphs.go‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@ type NavGraph[Node comparable] interface {
2727
GetClosestPoint(Node) (Node, bool)
2828
IsRaycastHit(Node, Node) bool
2929
Cost(Node, Node) float32
30+
HashIndex(Node) int64
3031
}
3132

3233
// Graph is represented by an adjacency list.

‎graphs/grid/grid.go‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,27 @@ func (g *Grid) Cost(a, b geom.Vector2) float32 {
6868
return g.costFunc(a, b)
6969
}
7070

71+
const (
72+
scaleFactor = 1e6
73+
hashRnd = 1099511628211
74+
hashDefault uint64 = 14695981039346656037
75+
)
76+
77+
func (g *Grid) HashIndex(v geom.Vector2) int64 {
78+
qx := quantizeFloat(v.X)
79+
qy := quantizeFloat(v.Y)
80+
81+
var hash = hashDefault
82+
hash = (hash * hashRnd) ^ uint64(qx)
83+
hash = (hash * hashRnd) ^ uint64(qy)
84+
85+
return int64(hash)
86+
}
87+
88+
func quantizeFloat(f float32) int64 {
89+
return int64(f * scaleFactor)
90+
}
91+
7192
func (g *Grid) IsRaycastHit(start, end geom.Vector2) bool {
7293
return false
7394
}

‎graphs/recast/recast.go‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -420,6 +420,27 @@ func (r *Recast) Cost(a geom.Vector2, b geom.Vector2) float32 {
420420
return r.costFunc(a, b)
421421
}
422422

423+
const (
424+
scaleFactor = 1e6
425+
hashRnd = 1099511628211
426+
hashDefault uint64 = 14695981039346656037
427+
)
428+
429+
func (r *Recast) HashIndex(v geom.Vector2) int64 {
430+
qx := quantizeFloat(v.X)
431+
qy := quantizeFloat(v.Y)
432+
433+
var hash = hashDefault
434+
hash = (hash * hashRnd) ^ uint64(qx)
435+
hash = (hash * hashRnd) ^ uint64(qy)
436+
437+
return int64(hash)
438+
}
439+
440+
func quantizeFloat(f float32) int64 {
441+
return int64(f * scaleFactor)
442+
}
443+
423444
func (r *Recast) Triangles() []Triangle {
424445
return r.triangles
425446
}

‎graphs/recast/recast_test.go‎

Lines changed: 37 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -12,52 +12,57 @@ import (
1212

1313
const largeLocation = `{"polygons":[{"outer":[{"x":276.63,"y":-251.83},{"x":309.33,"y":-253.34},{"x":335.9,"y":-178.9},{"x":388.6,"y":-146.17},{"x":454.03,"y":-146.17},{"x":454.03,"y":-111.73},{"x":486.73,"y":-113.24},{"x":513.74,"y":-37.58},{"x":481.04,"y":-36.06},{"x":481.04,"y":1.9},{"x":341.29,"y":1.9},{"x":341.29,"y":-5.98},{"x":291.13,"y":1.21},{"x":297.64,"y":19.42},{"x":264.94,"y":20.94},{"x":264.94,"y":58.9},{"x":125.19,"y":58.9},{"x":125.19,"y":44.37},{"x":85.44,"y":44.37},{"x":90.19,"y":57.68},{"x":57.5,"y":59.19},{"x":57.5,"y":97.16},{"x":-82.25,"y":97.16},{"x":-82.25,"y":23.45},{"x":-54.07,"y":23.45},{"x":-54.07,"y":59.19},{"x":-27.06,"y":-16.48},{"x":-54.65,"y":-16.48},{"x":-54.65,"y":-50.91},{"x":-18.27,"y":-50.91},{"x":-18.72,"y":-69.19},{"x":-28.3,"y":-69.19},{"x":-45.8,"y":-99.5},{"x":-28.3,"y":-129.81},{"x":6.7,"y":-129.81},{"x":8.84,"y":-126.1},{"x":163.89,"y":-180.78},{"x":163.89,"y":-211.9},{"x":192.07,"y":-211.9},{"x":192.07,"y":-176.16},{"x":219.08,"y":-251.83},{"x":191.49,"y":-251.83},{"x":191.49,"y":-286.27},{"x":276.63,"y":-286.27}],"innerHoles":[{"points":[{"x":23.45,"y":-100.79},{"x":24.2,"y":-99.5},{"x":9.88,"y":-74.7},{"x":10.46,"y":-50.91},{"x":30.49,"y":-50.91},{"x":30.49,"y":-16.48},{"x":63.19,"y":-17.99},{"x":75.19,"y":15.63},{"x":125.19,"y":15.63},{"x":125.19,"y":-14.8},{"x":153.37,"y":-14.8},{"x":153.37,"y":20.94},{"x":180.38,"y":-54.73},{"x":152.79,"y":-54.73},{"x":152.79,"y":-89.17},{"x":184.98,"y":-89.17},{"x":201.57,"y":-138.2},{"x":163.89,"y":-138.2},{"x":163.89,"y":-150.32}],"viewable":true},{"points":[{"x":303.64,"y":-138.2},{"x":231.92,"y":-138.2},{"x":215.32,"y":-89.17},{"x":237.93,"y":-89.17},{"x":237.93,"y":-54.73},{"x":270.63,"y":-56.24},{"x":281.27,"y":-26.41},{"x":341.29,"y":-35.02},{"x":341.29,"y":-71.8},{"x":369.47,"y":-71.8},{"x":369.47,"y":-36.06},{"x":396.48,"y":-111.73},{"x":368.89,"y":-111.73},{"x":368.89,"y":-124.58},{"x":303.64,"y":-165.11}],"viewable":true},{"points":[{"x":-25.71,"y":-99.5},{"x":-18.26,"y":-86.59},{"x":-3.34,"y":-86.59},{"x":4.11,"y":-99.5},{"x":-3.34,"y":-112.41},{"x":-18.26,"y":-112.41}],"viewable":true}],"obstacles":[{"points":[{"x":33.92,"y":54.61},{"x":23.68,"y":54.61},{"x":23.68,"y":15.99},{"x":33.92,"y":15.99}],"viewable":true},{"points":[{"x":383.1,"y":-41.91},{"x":338.2,"y":-41.46},{"x":338.1,"y":-51.69},{"x":383.0,"y":-52.14}],"viewable":true},{"points":[{"x":-5.91,"y":20.15},{"x":-27.36,"y":52.27},{"x":-30.09,"y":50.45},{"x":-8.64,"y":18.33}],"viewable":true},{"points":[{"x":25.56,"y":-47.41},{"x":27.21,"y":-45.76},{"x":27.81,"y":-43.5},{"x":27.21,"y":-41.24},{"x":25.56,"y":-39.59},{"x":23.3,"y":-38.99},{"x":21.04,"y":-39.59},{"x":19.39,"y":-41.24},{"x":18.79,"y":-43.5},{"x":19.39,"y":-45.76},{"x":21.04,"y":-47.41},{"x":23.3,"y":-48.01}],"viewable":false},{"points":[{"x":-40.05,"y":-25.45},{"x":-39.01,"y":-24.42},{"x":-38.63,"y":-23.0},{"x":-39.01,"y":-21.58},{"x":-40.05,"y":-20.55},{"x":-41.47,"y":-20.17},{"x":-47.13,"y":-20.17},{"x":-48.55,"y":-20.55},{"x":-49.59,"y":-21.58},{"x":-49.97,"y":-23.0},{"x":-49.59,"y":-24.42},{"x":-48.55,"y":-25.45},{"x":-47.13,"y":-25.83},{"x":-41.47,"y":-25.83}],"viewable":false},{"points":[{"x":-53.71,"y":63.09},{"x":-46.69,"y":70.11},{"x":-44.12,"y":79.7},{"x":-46.69,"y":89.29},{"x":-53.71,"y":96.31},{"x":-63.3,"y":98.88},{"x":-72.89,"y":96.31},{"x":-79.91,"y":89.29},{"x":-82.48,"y":79.7},{"x":-79.91,"y":70.11},{"x":-72.89,"y":63.09},{"x":-63.3,"y":60.52}],"viewable":false},{"points":[{"x":-16.91,"y":-0.94},{"x":-15.16,"y":0.81},{"x":-14.52,"y":3.2},{"x":-15.16,"y":5.59},{"x":-16.91,"y":7.34},{"x":-19.3,"y":7.98},{"x":-21.69,"y":7.34},{"x":-23.44,"y":5.59},{"x":-24.08,"y":3.2},{"x":-23.44,"y":0.81},{"x":-21.69,"y":-0.94},{"x":-19.3,"y":-1.58}],"viewable":false},{"points":[{"x":9.57,"y":3.33},{"x":11.39,"y":7.7},{"x":9.57,"y":12.07},{"x":5.2,"y":13.89},{"x":0.83,"y":12.07},{"x":-0.99,"y":7.7},{"x":0.83,"y":3.33},{"x":5.2,"y":1.51}],"viewable":true},{"points":[{"x":3.42,"y":58.28},{"x":5.05,"y":58.94},{"x":13.36,"y":64.88},{"x":11.62,"y":70.57},{"x":5.18,"y":78.27},{"x":1.63,"y":78.85},{"x":-5.03,"y":79.37},{"x":-13.36,"y":73.9},{"x":-12.08,"y":64.58},{"x":-11.45,"y":63.54},{"x":-6.37,"y":58.62},{"x":-3.54,"y":58.03}],"viewable":false},{"points":[{"x":204.92,"y":-4.72},{"x":206.55,"y":-4.06},{"x":214.86,"y":1.88},{"x":213.12,"y":7.57},{"x":206.68,"y":15.27},{"x":203.13,"y":15.85},{"x":196.47,"y":16.37},{"x":188.14,"y":10.9},{"x":189.42,"y":1.58},{"x":190.05,"y":0.54},{"x":195.13,"y":-4.38},{"x":197.96,"y":-4.97}],"viewable":false},{"points":[{"x":51.84,"y":5.47},{"x":49.4,"y":7.67},{"x":44.4,"y":7.67},{"x":41.96,"y":5.47},{"x":42.15,"y":-5.5},{"x":51.65,"y":-5.5}],"viewable":false}]},{"outer":[{"x":-90.95,"y":-304.37},{"x":-86.3,"y":-305.79},{"x":-77.93,"y":-278.3},{"x":-82.34,"y":-276.95},{"x":-47.07,"y":-164.58},{"x":-45.02,"y":-165.31},{"x":-35.46,"y":-138.22},{"x":-205.38,"y":-78.29},{"x":-214.94,"y":-105.38},{"x":-211.7,"y":-106.51},{"x":-249.25,"y":-226.12},{"x":-250.3,"y":-225.81},{"x":-258.67,"y":-253.3},{"x":-230.47,"y":-262.26},{"x":-230.36,"y":-261.91},{"x":-118.44,"y":-296.0},{"x":-120.19,"y":-301.55},{"x":-92.77,"y":-310.16}],"innerHoles":[{"points":[{"x":-221.75,"y":-234.5},{"x":-184.58,"y":-116.08},{"x":-74.19,"y":-155.01},{"x":-109.84,"y":-268.58}],"viewable":true}],"obstacles":[]},{"outer":[{"x":32.0,"y":-200.2},{"x":14.5,"y":-169.89},{"x":-20.5,"y":-169.89},{"x":-38.0,"y":-200.2},{"x":-20.5,"y":-230.51},{"x":14.5,"y":-230.51}],"innerHoles":[{"points":[{"x":-17.91,"y":-200.2},{"x":-10.46,"y":-187.29},{"x":4.46,"y":-187.29},{"x":11.91,"y":-200.2},{"x":4.46,"y":-213.11},{"x":-10.46,"y":-213.11}],"viewable":true}],"obstacles":[]},{"outer":[{"x":129.6,"y":-227.3},{"x":112.1,"y":-196.99},{"x":77.1,"y":-196.99},{"x":59.6,"y":-227.3},{"x":77.1,"y":-257.61},{"x":112.1,"y":-257.61}],"innerHoles":[{"points":[{"x":79.69,"y":-227.3},{"x":87.14,"y":-214.39},{"x":102.06,"y":-214.39},{"x":109.51,"y":-227.3},{"x":102.06,"y":-240.21},{"x":87.14,"y":-240.21}],"viewable":true}],"obstacles":[{"points":[{"x":97.06,"y":-209.01},{"x":98.71,"y":-207.36},{"x":99.31,"y":-205.1},{"x":98.71,"y":-202.84},{"x":97.06,"y":-201.19},{"x":94.8,"y":-200.59},{"x":92.54,"y":-201.19},{"x":90.89,"y":-202.84},{"x":90.29,"y":-205.1},{"x":90.89,"y":-207.36},{"x":92.54,"y":-209.01},{"x":94.8,"y":-209.61}],"viewable":false}]}]}`
1414

15-
// BenchmarkAggregationGraph/out_area-16 15650 76188 ns/op 22649 B/op 10 allocs/op
16-
// BenchmarkAggregationGraph/inside_area-16 195895 5793 ns/op 19048 B/op 15 allocs/op
15+
// BenchmarkAggregationGraph/out_area-16 99735 11289 ns/op 23032 B/op 18 allocs/op
16+
// BenchmarkAggregationGraph/inside_area-16 221824 5815 ns/op 19048 B/op 15 allocs/op
1717
// BenchmarkAggregationGraph/direct-16 672487 1778 ns/op 352 B/op 4 allocs/op
1818
func BenchmarkAggregationGraph(b *testing.B) {
1919
rPolygons, err := loadLargeLocation()
2020
if err != nil {
2121
b.Fatal(err)
2222
}
2323

24-
var (
25-
recastGraph = NewRecast(rPolygons, WithSearchOutOfArea(true))
26-
// out area
27-
start1 = geom.Vector2{X: 192, Y: -267}
28-
dest1 = geom.Vector2{X: -4, Y: 80}
29-
//inside area
30-
start2 = geom.Vector2{X: 202, Y: -268}
31-
dest2 = geom.Vector2{X: -4, Y: 84}
32-
// direct
33-
start3 = geom.Vector2{X: 252, Y: -174}
34-
dest3 = geom.Vector2{X: 252, Y: -218}
35-
)
24+
tests := []struct {
25+
name string
26+
start geom.Vector2
27+
dest geom.Vector2
28+
}{
29+
{
30+
name: "search out of area",
31+
start: geom.Vector2{X: 192, Y: -267},
32+
dest: geom.Vector2{X: -4, Y: 80},
33+
},
34+
{
35+
name: "search inside area",
36+
start: geom.Vector2{X: 202, Y: -268},
37+
dest: geom.Vector2{X: -4, Y: 84},
38+
},
39+
{
40+
name: "search short distance inside area",
41+
start: geom.Vector2{X: 168, Y: 5},
42+
dest: geom.Vector2{X: 136, Y: -4},
43+
},
44+
{
45+
name: "search direct",
46+
start: geom.Vector2{X: 252, Y: -174},
47+
dest: geom.Vector2{X: 252, Y: -218},
48+
},
49+
}
3650

51+
recastGraph := NewRecast(rPolygons, WithSearchOutOfArea(true))
3752
_ = recastGraph.Generate(nil)
3853
//extraObstacles := generateExtraObstacles(150)
3954
//recastGraph.AddObstacles(extraObstacles...)
4055

4156
b.ResetTimer()
4257

43-
b.Run("out area", func(b *testing.B) {
44-
b.ReportAllocs()
45-
for b.Loop() {
46-
_ = recastGraph.AggregationGraph(start1, dest1, nil)
47-
}
48-
})
49-
b.Run("inside area", func(b *testing.B) {
50-
b.ReportAllocs()
51-
for b.Loop() {
52-
_ = recastGraph.AggregationGraph(start2, dest2, nil)
53-
}
54-
})
55-
b.Run("direct", func(b *testing.B) {
56-
b.ReportAllocs()
57-
for b.Loop() {
58-
_ = recastGraph.AggregationGraph(start3, dest3, nil)
59-
}
60-
})
58+
for _, tt := range tests {
59+
b.Run(tt.name, func(b *testing.B) {
60+
b.ReportAllocs()
61+
for b.Loop() {
62+
_ = recastGraph.AggregationGraph(tt.start, tt.dest, nil)
63+
}
64+
})
65+
}
6166
}
6267

6368
// BenchmarkRecast_Generate-16 1167 990037 ns/op 444285 B/op 8317 allocs/op

‎pathfinder.go‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ func (p *Pathfinder[Node]) Path(graphID int, start, dest Node, opts ...PathOptio
5656
return []Node{start, dest}
5757
}
5858

59-
path := astar.FindPath[Node](vis, start, dest, g.Cost, g.Cost)
59+
path := astar.FindPath[Node](vis, start, dest, g.HashIndex, g.Cost, g.Cost)
6060

6161
return path
6262
}

0 commit comments

Comments
 (0)