🔥 移除不合理astar设计

This commit is contained in:
kercylan98 2023-06-19 18:14:53 +08:00
parent ad1794a9e3
commit 6c5f0c01a1
13 changed files with 109 additions and 594 deletions

View File

@ -1,99 +1 @@
package astar package astar
import (
"container/heap"
"fmt"
"math"
)
// Pathfinding3D 寻路
// - grid: X、Y、Z
func Pathfinding3D(grid [][][]int, start, target Point3D, collisionRange int) ([]Point3D, error) {
path, err := aStar3D(grid, start, target, collisionRange)
if err != nil {
return nil, err
}
return path, nil
}
func aStar3D(grid [][][]int, start, goal Point3D, collisionRange int) ([]Point3D, error) {
startNode := &Node3D{point: start, g: 0, h: heuristic(start, goal)}
startNode.f = startNode.g + startNode.h
openList := make(Node3DList, 0)
heap.Init(&openList)
heap.Push(&openList, startNode)
closedList := make(map[Point3D]*Node3D)
for openList.Len() > 0 {
current := heap.Pop(&openList).(*Node3D)
if current.point == goal {
path := make([]Point3D, 0)
for current != nil {
path = append([]Point3D{current.point}, path...)
current = current.parent
}
return path, nil
}
closedList[current.point] = current
for _, neighborPoint := range neighbors(current.point, grid, collisionRange) {
if _, ok := closedList[neighborPoint]; ok {
continue
}
tentativeG := current.g + 1
neighborNode := &Node3D{point: neighborPoint, parent: current, g: tentativeG, h: heuristic(neighborPoint, goal)}
neighborNode.f = neighborNode.g + neighborNode.h
for _, openNode := range openList {
if openNode.point == neighborPoint && tentativeG >= openNode.g {
continue
}
}
heap.Push(&openList, neighborNode)
}
}
return nil, fmt.Errorf("no path found")
}
func heuristic(a, b Point3D) float64 {
return math.Abs(float64(a.X-b.X)) + math.Abs(float64(a.Y-b.Y)) + math.Abs(float64(a.Z-b.Z))
}
func inBounds(point Point3D, grid [][][]int) bool {
return point.X >= 0 && point.X < len(grid) && point.Y >= 0 && point.Y < len(grid[0]) && point.Z >= 0 && point.Z < len(grid[0][0])
}
func passable(point Point3D, grid [][][]int, collisionRange int) bool {
for x := -collisionRange; x <= collisionRange; x++ {
for y := -collisionRange; y <= collisionRange; y++ {
for z := -collisionRange; z <= collisionRange; z++ {
newPoint := Point3D{point.X + x, point.Y + y, point.Z + z}
if inBounds(newPoint, grid) && grid[newPoint.X][newPoint.Y][newPoint.Z] == 1 {
return false
}
}
}
}
return true
}
func neighbors(point Point3D, grid [][][]int, collisionRange int) []Point3D {
neighborPoints := []Point3D{
{point.X - 1, point.Y, point.Z}, {point.X + 1, point.Y, point.Z},
{point.X, point.Y - 1, point.Z}, {point.X, point.Y + 1, point.Z},
{point.X, point.Y, point.Z - 1}, {point.X, point.Y, point.Z + 1},
}
result := make([]Point3D, 0)
for _, neighbor := range neighborPoints {
if inBounds(neighbor, grid) && passable(neighbor, grid, collisionRange) {
result = append(result, neighbor)
}
}
return result
}
func abs(x int) int {
if x < 0 {
return -x
}
return x
}

View File

@ -1,77 +0,0 @@
package astar
import (
"fmt"
"testing"
)
func TestPathfinding3D(t *testing.T) {
grid := [][][]int{
{
{0, 0, 0, 0, 0},
{0, 0, 0, 0, 0},
{0, 0, 0, 0, 0},
{0, 0, 0, 0, 0},
{0, 0, 0, 0, 0},
},
{
{0, 0, 0, 0, 0},
{0, 1, 1, 1, 0},
{0, 1, 0, 1, 0},
{0, 1, 1, 1, 0},
{0, 0, 0, 0, 0},
},
{
{1, 0, 0, 0, 0},
{0, 1, 0, 1, 0},
{0, 0, 0, 0, 0},
{0, 1, 0, 1, 0},
{0, 0, 0, 0, 0},
},
{
{0, 0, 0, 0, 0},
{0, 1, 1, 1, 0},
{0, 1, 0, 1, 0},
{0, 1, 1, 1, 0},
{0, 0, 0, 0, 0},
},
{
{0, 0, 0, 0, 0},
{0, 0, 0, 0, 0},
{0, 0, 0, 0, 0},
{0, 0, 0, 0, 0},
{0, 0, 0, 0, 0},
},
}
start := Point3D{2, 1, 0}
target := Point3D{2, 2, 2}
collisionRange := 0
path, err := Pathfinding3D(grid, start, target, collisionRange)
if err != nil {
panic(err)
}
fmt.Println(path)
printGrid(grid, start)
}
func printGrid(grid [][][]int, start Point3D) {
for z := 0; z < len(grid); z++ {
fmt.Printf("z = %d\n", z)
for y := 0; y < len(grid[z]); y++ {
for x := 0; x < len(grid[z][y]); x++ {
if x == start.X && y == start.Y && z == start.Z {
fmt.Print("S ")
} else if grid[z][y][x] == 0 {
fmt.Print(". ")
} else {
fmt.Print("# ")
}
}
fmt.Println()
}
fmt.Println()
}
}

4
utils/astar/node.go Normal file
View File

@ -0,0 +1,4 @@
package astar
type Node interface {
}

View File

@ -1,42 +0,0 @@
package astar
type Node3D struct {
point Point3D
parent *Node3D
g, h, f float64
index int
}
type Node3DList []*Node3D
func (slf *Node3DList) Len() int {
return len(*slf)
}
func (slf *Node3DList) Less(i, j int) bool {
nl := *slf
return nl[i].f < nl[j].f
}
func (slf *Node3DList) Swap(i, j int) {
nl := *slf
nl[i], nl[j] = nl[j], nl[i]
nl[i].index = i
nl[j].index = j
}
func (slf *Node3DList) Push(x interface{}) {
n := len(*slf)
node := x.(*Node3D)
node.index = n
*slf = append(*slf, node)
}
func (slf *Node3DList) Pop() interface{} {
old := *slf
n := len(old)
node := old[n-1]
node.index = -1
*slf = old[0 : n-1]
return node
}

View File

@ -1,5 +0,0 @@
package astar
type Point3D struct {
X, Y, Z int
}

View File

@ -52,26 +52,91 @@ func (slf *NavMesh[V]) Find(point geometry.Point[V], maxDistance V) (distance V,
return minDistance, pointOnClosest, closest.Shape return minDistance, pointOnClosest, closest.Shape
} }
// FindPath 使用此导航网格查找从起点到终点的路径。
func (slf *NavMesh[V]) FindPath(start, end geometry.Point[V]) (result []geometry.Point[V]) {
var startShape, endShape *shape[V]
var startDistance, endDistance = V(-1), V(-1)
for _, meshShape := range slf.meshShapes {
br := meshShape.BoundingRadius()
distance := geometry.CalcDistance(geometry.DoublePointToCoordinate(meshShape.Centroid(), start))
if (distance <= startDistance || startDistance == V(-1)) && distance <= br && meshShape.Contains(start) {
startShape = meshShape
startDistance = distance
}
distance = geometry.CalcDistance(geometry.DoublePointToCoordinate(meshShape.Centroid(), end))
if (distance <= endDistance || endDistance == V(-1)) && distance <= br && meshShape.Contains(end) {
endShape = meshShape
endDistance = distance
}
}
if endShape == nil && slf.meshShrinkAmount > V(0) {
for _, meshShape := range slf.meshShapes {
br := meshShape.BoundingRadius() + slf.meshShrinkAmount
distance := geometry.CalcDistance(geometry.DoublePointToCoordinate(meshShape.Centroid(), end))
if distance <= br {
_, projectionDistance := geometry.ProjectionPointToShape(end, meshShape.Shape)
if projectionDistance <= slf.meshShrinkAmount && projectionDistance < endDistance {
endShape = meshShape
endDistance = projectionDistance
}
}
}
}
if endShape == nil {
return
}
if startShape == nil && slf.meshShrinkAmount > 0 {
for _, meshShape := range slf.meshShapes {
br := meshShape.BoundingRadius() + slf.meshShrinkAmount
distance := geometry.CalcDistance(geometry.DoublePointToCoordinate(meshShape.Centroid(), start))
if distance <= br {
_, projectionDistance := geometry.ProjectionPointToShape(start, meshShape.Shape)
if projectionDistance <= slf.meshShrinkAmount && projectionDistance < startDistance {
startShape = meshShape
startDistance = projectionDistance
}
}
}
}
if startShape == nil {
return
}
if startShape == endShape {
return append(result, start, end)
}
}
func (slf *NavMesh[V]) aStar() {
}
func (slf *NavMesh[V]) generateLink() { func (slf *NavMesh[V]) generateLink() {
refer := len(slf.meshShapes) refer := len(slf.meshShapes)
for i := 0; i < refer; i++ { for i := 0; i < refer; i++ {
shapePkg := slf.meshShapes[i] shapePkg := slf.meshShapes[i]
shape := shapePkg.Shape shapeCentroid := shapePkg.Centroid()
shapeCentroid := geometry.CalcRectangleCentroid(shape) shapeBoundingRadius := shapePkg.BoundingRadius()
shapeBoundingRadius := geometry.CalcBoundingRadiusWithCentroid(shape, shapeCentroid) shapeEdges := shapePkg.Edges()
shapeEdges := shape.Edges()
for t := i + 1; t < len(slf.meshShapes); t++ { for t := i + 1; t < len(slf.meshShapes); t++ {
targetShapePkg := slf.meshShapes[t] targetShapePkg := slf.meshShapes[t]
targetShape := targetShapePkg.Shape targetShapeCentroid := targetShapePkg.Centroid()
targetShapeCentroid := geometry.CalcRectangleCentroid(targetShape) targetShapeBoundingRadius := targetShapePkg.BoundingRadius()
targetShapeBoundingRadius := geometry.CalcBoundingRadiusWithCentroid(shape, targetShapeCentroid)
centroidDistance := geometry.CalcDistance(geometry.DoublePointToCoordinate(shapeCentroid, targetShapeCentroid)) centroidDistance := geometry.CalcDistance(geometry.DoublePointToCoordinate(shapeCentroid, targetShapeCentroid))
if centroidDistance > shapeBoundingRadius+targetShapeBoundingRadius { if centroidDistance > shapeBoundingRadius+targetShapeBoundingRadius {
continue continue
} }
for _, shapeEdge := range shapeEdges { for _, shapeEdge := range shapeEdges {
for _, targetEdge := range targetShape.Edges() { for _, targetEdge := range targetShapePkg.Edges() {
if !geometry.CalcLineIsCollinear(shapeEdge, targetEdge, V(maths.GetDefaultTolerance())) { if !geometry.CalcLineIsCollinear(shapeEdge, targetEdge, V(maths.GetDefaultTolerance())) {
continue continue
} }

View File

@ -8,6 +8,9 @@ import (
func newShape[V generic.SignedNumber](s geometry.Shape[V]) *shape[V] { func newShape[V generic.SignedNumber](s geometry.Shape[V]) *shape[V] {
return &shape[V]{ return &shape[V]{
Shape: s, Shape: s,
centroid: geometry.CalcRectangleCentroid(s),
boundingRadius: geometry.CalcBoundingRadius(s),
edges: s.Edges(),
} }
} }
@ -15,4 +18,30 @@ type shape[V generic.SignedNumber] struct {
geometry.Shape[V] geometry.Shape[V]
links []*shape[V] links []*shape[V]
portals []geometry.Line[V] portals []geometry.Line[V]
boundingRadius V
centroid geometry.Point[V]
edges []geometry.Line[V]
weight V
x, y V
}
func (slf *shape[V]) Edges() []geometry.Line[V] {
return slf.edges
}
func (slf *shape[V]) BoundingRadius() V {
return slf.boundingRadius
}
func (slf *shape[V]) Centroid() geometry.Point[V] {
return slf.centroid
}
func (slf *shape[V]) IsWall() bool {
return slf.weight == 0
}
func (slf *shape[V]) GetCost(point geometry.Point[V]) V {
return geometry.CalcDistance(geometry.DoublePointToCoordinate(slf.Centroid(), point))
} }

View File

@ -1,27 +0,0 @@
package path
type h []*Node
func (slf *h) Len() int {
return len(*slf)
}
func (slf *h) Less(i, j int) bool {
return (*slf)[i].cost < (*slf)[j].cost
}
func (slf *h) Swap(i, j int) {
(*slf)[i], (*slf)[j] = (*slf)[j], (*slf)[i]
}
func (slf *h) Push(x any) {
*slf = append(*slf, x.(*Node))
}
func (slf *h) Pop() any {
old := *slf
n := len(old)
x := old[n-1]
*slf = old[0 : n-1]
return x
}

View File

@ -1,89 +0,0 @@
package path
import (
"github.com/kercylan98/minotaur/utils/geometry"
)
func NewLandform(pos int, features ...*LandformFeature) *Landform {
if len(features) == 0 {
panic("path landforms without any features")
}
pl := &Landform{
pos: pos,
features: features,
}
return pl
}
// Landform 路径地貌表示路径地图上的一个点
// - 末尾特征将决定该路径地貌是否可行走
type Landform struct {
width int // 所在路径覆盖宽度
pos int // 位置
features []*LandformFeature // 地貌特征
original []*LandformFeature // 原始地貌
totalCost float64 // 总消耗
}
// GetCoordinate 获取这个路径地貌指向的 x 和 y 坐标
// - 建议通过 GetPos 来进行获取,这样可以避免一次转换
func (slf *Landform) GetCoordinate() (x, y int) {
return geometry.PosToCoordinate(slf.width, slf.pos)
}
// GetPos 获取这个路径地貌指向的 pos 位置
func (slf *Landform) GetPos() int {
return slf.pos
}
// GetTotalCost 获取这个路径地貌的总特征消耗
func (slf *Landform) GetTotalCost() float64 {
return slf.totalCost
}
// Walkable 指示了该路径地貌是否可以在上面行走或者应该避免在上面行走
func (slf *Landform) Walkable() bool {
return slf.features[len(slf.features)-1].Walkable()
}
// GetFeatures 获取这个路径地貌的特征
func (slf *Landform) GetFeatures() []*LandformFeature {
return slf.features
}
// SetFeatures 设置这个路径地貌的特征
func (slf *Landform) SetFeatures(features ...*LandformFeature) {
slf.features = features
slf.original = nil
slf.totalCost = 0
for _, feature := range slf.features {
slf.totalCost += feature.GetCost()
}
}
// SetFeaturesWithRecoverable 通过可恢复的方式设置这个路径地貌的特征
// - 使用该函数设置地貌特征后续也应该继续使用该函数进行地貌特征修改,如果中途使用 SetFeatures 修改地貌特征后将导致 Recover 不可用
func (slf *Landform) SetFeaturesWithRecoverable(features ...*LandformFeature) {
if slf.original == nil {
slf.original = slf.features
}
slf.features = features
slf.totalCost = 0
for _, feature := range slf.features {
slf.totalCost += feature.GetCost()
}
}
// Recover 恢复这个路径地貌的特征
func (slf *Landform) Recover() {
if slf.original == nil {
return
}
slf.features = slf.original
slf.original = nil
slf.totalCost = 0
for _, feature := range slf.features {
slf.totalCost += feature.GetCost()
}
}

View File

@ -1,28 +0,0 @@
package path
var (
LandformFeatureRoad = NewLandformFeature(1, true) // 道路
LandformFeatureWater = NewLandformFeature(1.75, true) // 水
)
// NewLandformFeature 返回一个新的路径地貌特征, cost 将表示在该地貌上行走的消耗walkable 则表示了该地貌是否支持行走
// - 在基于 Terrain 使用时, LandformFeature 必须被声明为全局变量后再进行使用,例如 LandformFeatureRoad
func NewLandformFeature(cost float64, walkable bool) *LandformFeature {
return &LandformFeature{cost: cost, walkable: walkable}
}
// LandformFeature 表示了路径地貌的特征
type LandformFeature struct {
cost float64 // 移动消耗
walkable bool // 适宜步行
}
// GetCost 获取在该路径地貌行走的消耗,这影响了该路径地貌是否是理想的选择
func (slf *LandformFeature) GetCost() float64 {
return slf.cost
}
// Walkable 指示了该路径地貌是否可以在上面行走或者应该避免在上面行走
func (slf *LandformFeature) Walkable() bool {
return slf.walkable
}

View File

@ -1,7 +0,0 @@
package path
type Node struct {
landform *Landform
parent *Node
cost float64
}

View File

@ -1,14 +0,0 @@
package path
type Path struct {
points []*Landform
currentIndex int
}
func (slf *Path) GetPoints() []*Landform {
return slf.points
}
func (slf *Path) GetCurrentIndex() int {
return slf.currentIndex
}

View File

@ -1,196 +0,0 @@
package path
import (
"container/heap"
"github.com/kercylan98/minotaur/utils/geometry"
)
// NewTerrain 返回一个大小为 width 和 height 的新的路径覆盖信息landformWidth 和 landformHeight 将对每
// 个路径地貌的尺寸进行描述,用于世界位置的转化,例如当 landformWidth 和 landformHeight 为 2, 2 时,路径
// 地貌位于 2, 3 的情况下,它的世界位置将是 4, 6
// - 地貌特征将默认统一为 PathLandformFeatureRoad
func NewTerrain(width, height, landformWidth, landformHeight int) *Terrain {
path := &Terrain{
features: map[*LandformFeature]map[int]struct{}{},
width: width,
height: height,
landformWidth: landformWidth,
landformHeight: landformHeight,
}
path.matrix = make([]*Landform, width*height)
for pos := 0; pos < len(path.matrix); pos++ {
path.matrix[pos] = NewLandform(pos, LandformFeatureRoad)
}
path.refreshFeatures()
return path
}
// NewPathWithMatrix 基于特定的 matrix 返回一个新的路径覆盖信息
// - 可参照于 NewTerrain
func NewPathWithMatrix(matrix []*Landform, width, height, landformWidth, landformHeight int) *Terrain {
path := &Terrain{
matrix: matrix,
width: width,
height: height,
landformWidth: landformWidth,
landformHeight: landformHeight,
}
path.refreshFeatures()
return path
}
type Terrain struct {
matrix []*Landform // 矩阵
width, height int // 矩阵宽高
landformWidth, landformHeight int // 地貌宽高
features map[*LandformFeature]map[int]struct{} // 标注了特定特征的路径地貌位置
}
// Get 返回 x 和 y 指向的地貌信息
// - 通常更建议使用 GetWithPos 进行获取,因为这样可以减少一次转换
func (slf *Terrain) Get(x, y int) *Landform {
return slf.matrix[geometry.CoordinateToPos(slf.width, x, y)]
}
// GetWithPos 返回 pos 指向的地貌信息
func (slf *Terrain) GetWithPos(pos int) *Landform {
return slf.matrix[pos]
}
// GetHeight 返回这个路径覆盖的范围高度
func (slf *Terrain) GetHeight() int {
return slf.height
}
// GetWidth 返回这个路径覆盖的范围宽度
func (slf *Terrain) GetWidth() int {
return slf.width
}
// GetAll 获取所有地貌信息
func (slf *Terrain) GetAll() []*Landform {
return slf.matrix
}
// GetPath 返回一个从起点位置到目标位置的路径
// - 可以通过参数 diagonals 控制是否支持沿对角线进行移动
// - 可以通过参数 wallsBlockDiagonals 控制在进行对角线移动时是否允许穿越不可通行的区域
func (slf *Terrain) GetPath(startPos, endPos int, diagonals, wallsBlockDiagonals bool) *Path {
start, end := slf.GetWithPos(startPos), slf.GetWithPos(endPos)
var nodes h
var checkedLandforms = make(map[int]struct{})
var path = new(Path)
heap.Push(&nodes, &Node{landform: end, cost: end.GetTotalCost()})
if !start.Walkable() || !end.Walkable() {
return nil
}
for {
if len(nodes) == 0 {
break
}
node := heap.Pop(&nodes).(*Node)
if node.landform == start {
var t = node
for true {
path.points = append(path.points, t.landform)
t = t.parent
if t == nil {
break
}
}
break
}
for _, adjacent := range geometry.GetAdjacentTranslatePos(slf.matrix, slf.width, node.landform.pos) {
landform := slf.GetWithPos(adjacent)
n := &Node{landform: landform, parent: node, cost: landform.GetTotalCost() + node.cost}
if _, exist := checkedLandforms[adjacent]; n.landform.Walkable() && !exist {
heap.Push(&nodes, n)
checkedLandforms[adjacent] = struct{}{}
}
}
if diagonals {
var up, down, left, right bool
if upPos := node.landform.pos - slf.width; upPos >= 0 {
up = slf.GetWithPos(upPos).Walkable()
}
if downPos := node.landform.pos + slf.width; downPos < len(slf.matrix) {
down = slf.GetWithPos(downPos).Walkable()
}
row := node.landform.pos / slf.width
if leftPos := node.landform.pos - 1; row == (leftPos / slf.width) {
left = slf.GetWithPos(leftPos).Walkable()
}
if rightPos := node.landform.pos + 1; row == (rightPos / slf.width) {
right = slf.GetWithPos(rightPos).Walkable()
}
diagonalCost := .414
size := len(slf.matrix)
currentRow := node.landform.pos / slf.width
if topLeft := node.landform.pos - slf.width - 1; topLeft >= 0 && currentRow-1 == (topLeft/slf.width) {
landform := slf.GetWithPos(topLeft)
n := &Node{landform: landform, parent: node, cost: landform.GetTotalCost() + node.cost + diagonalCost}
if _, exist := checkedLandforms[topLeft]; n.landform.Walkable() && !exist && (!wallsBlockDiagonals || (left && up)) {
heap.Push(&nodes, n)
checkedLandforms[topLeft] = struct{}{}
}
}
if topRight := node.landform.pos - slf.width + 1; topRight >= 0 && currentRow-1 == (topRight/slf.width) {
landform := slf.GetWithPos(topRight)
n := &Node{landform: landform, parent: node, cost: landform.GetTotalCost() + node.cost + diagonalCost}
if _, exist := checkedLandforms[topRight]; n.landform.Walkable() && !exist && (!wallsBlockDiagonals || (right && up)) {
heap.Push(&nodes, n)
checkedLandforms[topRight] = struct{}{}
}
}
if bottomLeft := node.landform.pos + slf.width - 1; bottomLeft < size && currentRow+1 == (bottomLeft/slf.width) {
landform := slf.GetWithPos(bottomLeft)
n := &Node{landform: landform, parent: node, cost: landform.GetTotalCost() + node.cost + diagonalCost}
if _, exist := checkedLandforms[bottomLeft]; n.landform.Walkable() && !exist && (!wallsBlockDiagonals || (left && down)) {
heap.Push(&nodes, n)
checkedLandforms[bottomLeft] = struct{}{}
}
}
if bottomRight := node.landform.pos + slf.width + 1; bottomRight < size && currentRow+1 == (bottomRight/slf.width) {
landform := slf.GetWithPos(bottomRight)
n := &Node{landform: landform, parent: node, cost: landform.GetTotalCost() + node.cost + diagonalCost}
if _, exist := checkedLandforms[bottomRight]; n.landform.Walkable() && !exist && (!wallsBlockDiagonals || (right && down)) {
heap.Push(&nodes, n)
checkedLandforms[bottomRight] = struct{}{}
}
}
}
}
return path
}
// 刷新地貌特征标注信息
// - 冗余:已使用过的地貌特征类型在未使用后不会被删除
func (slf *Terrain) refreshFeatures() {
for _, positions := range slf.features {
for pos := range positions {
delete(positions, pos)
}
}
for pos, landform := range slf.matrix {
for _, feature := range landform.GetFeatures() {
positions, exist := slf.features[feature]
if !exist {
positions = map[int]struct{}{}
slf.features[feature] = positions
}
positions[pos] = struct{}{}
}
}
}