-
Notifications
You must be signed in to change notification settings - Fork 37
/
godetour.go
145 lines (125 loc) · 3.66 KB
/
godetour.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
package main
import (
"fmt"
"io/ioutil"
"os"
"unsafe"
detour "github.com/fananchong/recastnavigation-go/Detour"
)
const (
//copy from sample.h
SAMPLE_POLYAREA_GROUND int32 = 0
SAMPLE_POLYAREA_WATER int32 = 1
SAMPLE_POLYAREA_ROAD int32 = 2
SAMPLE_POLYAREA_DOOR int32 = 3
SAMPLE_POLYAREA_GRASS int32 = 4
SAMPLE_POLYAREA_JUMP int32 = 5
SAMPLE_POLYFLAGS_WALK uint16 = 0x01 // Ability to walk (ground, grass, road)
SAMPLE_POLYFLAGS_SWIM uint16 = 0x02 // Ability to swim (water).
SAMPLE_POLYFLAGS_DOOR uint16 = 0x04 // Ability to move through doors.
SAMPLE_POLYFLAGS_JUMP uint16 = 0x08 // Ability to jump.
SAMPLE_POLYFLAGS_DISABLED uint16 = 0x10 // Disabled polygon
SAMPLE_POLYFLAGS_ALL uint16 = 0xffff // All abilities.
)
var (
navMesh *detour.DtNavMesh
navQuery *detour.DtNavMeshQuery
filter *detour.DtQueryFilter
polyPickExt [3]float32
)
func init() {
navMesh = detour.DtAllocNavMesh()
navQuery = detour.DtAllocNavMeshQuery()
filter = detour.DtAllocDtQueryFilter()
filter.SetIncludeFlags(SAMPLE_POLYFLAGS_ALL ^ SAMPLE_POLYFLAGS_DISABLED)
filter.SetExcludeFlags(0)
polyPickExt[0] = 2
polyPickExt[1] = 4
polyPickExt[2] = 2
f, err := os.Open("navmesh.data")
if err != nil {
panic(err)
}
defer f.Close()
buf, err := ioutil.ReadAll(f)
if err != nil {
panic(err)
}
params := (*detour.DtNavMeshParams)(unsafe.Pointer(&buf[0]))
if detour.DtStatusFailed(navMesh.Init(params)) {
panic("buildTiledNavigation: Could not init navmesh.")
}
if detour.DtStatusFailed(navQuery.Init(navMesh, 2048)) {
panic("buildTiledNavigation: Could not init Detour navmesh query.")
}
paramSize := detour.DtAlign4(int(unsafe.Sizeof(detour.DtNavMeshParams{})))
buf = buf[paramSize:]
var x, y, dataSize *int32
var count int32
for {
if len(buf) < 4 {
break
}
x = (*int32)(unsafe.Pointer(&buf[0]))
buf = buf[4:]
if len(buf) < 4 {
panic("init 1")
}
y = (*int32)(unsafe.Pointer(&buf[0]))
buf = buf[4:]
if len(buf) < 4 {
panic("init 2")
}
dataSize = (*int32)(unsafe.Pointer(&buf[0]))
buf = buf[4:]
if len(buf) < int(*dataSize) {
panic("init 3")
}
data := buf[:*dataSize]
buf = buf[*dataSize:]
if *dataSize > 0 {
navMesh.RemoveTile(navMesh.GetTileRefAt(*x, *y, 0), nil, nil)
status := navMesh.AddTile(data, int(*dataSize), detour.DT_TILE_FREE_DATA, 0, nil)
if !detour.DtStatusFailed(status) {
count++
}
}
}
fmt.Println("success count:", count)
}
func GoFindPath(start, end, ptlst []float32, ptCount *int, maxPolys int) {
var startRef detour.DtPolyRef
var endRef detour.DtPolyRef
status := navQuery.FindNearestPoly(start, polyPickExt[:], filter, &startRef, nil)
if detour.DtStatusFailed(status) {
fmt.Println("startref falied")
return
}
status = navQuery.FindNearestPoly(end, polyPickExt[:], filter, &endRef, nil)
if detour.DtStatusFailed(status) {
fmt.Println("startref falied")
return
}
fmt.Println("startRef:", startRef, " endRef:", endRef)
polys := make([]detour.DtPolyRef, maxPolys)
var npolys int
navQuery.FindPath(startRef, endRef, start, end, filter, polys, &npolys, maxPolys)
if npolys > 0 {
fmt.Println("findPath npolys:", npolys)
for i := 0; i < npolys; i++ {
fmt.Println(polys[i])
}
fmt.Println()
var epos [3]float32
detour.DtVcopy(epos[:], end)
if polys[npolys-1] != endRef {
navQuery.ClosestPointOnPoly(polys[npolys-1], end, epos[:], nil)
}
straightPathFlags := make([]detour.DtStraightPathFlags, maxPolys)
straightPathPolys := make([]detour.DtPolyRef, maxPolys)
navQuery.FindStraightPath(start, epos[:], polys, npolys, ptlst, straightPathFlags,
straightPathPolys, ptCount, maxPolys, 0)
} else {
fmt.Println("find path failed")
}
}