package main
import (
"fmt"
"math/rand"
"time"
)
type block struct {
//编号
id int32
//是否障碍
isHinder bool
//父节点
parent *block
//G值 H值 F = G + H
gValue int32
hValue int32
}
// 总共格子数
var GRID_NUM int32 = 5000
// 行数
var ROW_NUM int32 = 50
// 列数
var COL_NUM int32 = 100
// 地图map
var Map map[int32]*block
// 生成地图 10*10 大小
func GenerateMap() {
Map = make(map[int32]*block)
for i := int32(1); i <= GRID_NUM; i++ {
bk := &block{
id: i,
isHinder: false,
}
Map[i] = bk
}
// 随机生成障碍物
//var tem int32 = 25
for j := 1; j < 500; j++ {
rand, _ := GetRandNumDown(1, GRID_NUM)
block, fd := Map[rand]
if fd {
block.isHinder = true
}
//tem += 10
}
}
// 计算给出的位置在哪一行哪一列
func GetRowCol(point int32) (int32, int32) {
row := point/COL_NUM + 1
col := point % COL_NUM
return row, col
}
// 找到相邻格子
func FindNeighbor(point int32) []int32 {
neighbor := make([]int32, 0)
if point < 1 || point > GRID_NUM {
return neighbor
}
// 上
up := point - COL_NUM
if up > 0 {
neighbor = append(neighbor, up)
}
// 下
down := point + COL_NUM
if down <= GRID_NUM {
neighbor = append(neighbor, down)
}
// 左
left := point - 1
if left > 0 && left%COL_NUM != 0 {
neighbor = append(neighbor, left)
// 左上
leftup := left - COL_NUM
if leftup > 0 {
neighbor = append(neighbor, leftup)
}
// 左下
leftdown := left + COL_NUM
if leftdown <= GRID_NUM {
neighbor = append(neighbor, leftdown)
}
}
// 右
right := point + 1
if right <= GRID_NUM && point%COL_NUM != 0 {
neighbor = append(neighbor, right)
// 右上
rightup := right - COL_NUM
if rightup > 0 {
neighbor = append(neighbor, rightup)
}
// 右下
rightdown := right + COL_NUM
if rightdown <= GRID_NUM {
neighbor = append(neighbor, rightdown)
}
}
return neighbor
}
// 计算B点 G H 值
func CountGH(pointA, pointB, pointC *block) {
if pointB.isHinder {
return
}
rowB, colB := GetRowCol(pointB.id)
rowC, colC := GetRowCol(pointC.id)
h1 := rowB - rowC
h2 := colB - colC
if h1 < 0 {
h1 = -h1
}
if h2 < 0 {
h2 = -h2
}
pointB.hValue = (h1 + h2) * 10
if pointA.id-COL_NUM == pointB.id || pointA.id+COL_NUM == pointB.id || pointA.id+1 == pointB.id || pointA.id-1 == pointB.id {
pointB.gValue = 10
} else {
pointB.gValue = 15
}
}
//地图寻路 传入起点 终点
func FindRoad(start, end int32) []int32 {
road := make([]int32, 0)
if start == end {
return road
}
//开放列表
OpenList := make(map[int32]*block)
//闭合列表
CloseList := make(map[int32]*block)
b_star, bfd := Map[start]
if !bfd || b_star.isHinder {
return road
}
b_end, efd := Map[end]
if !efd || b_end.isHinder {
return road
}
// 起点放入closelist
CloseList[start] = b_star
Run(b_star, b_end, OpenList, CloseList)
/
// 在开放列表中找最小值
var count int32 = 0
for {
if count >= 100000 {
fmt.Println("BREAK")
break
}
count++
if _, fd := OpenList[end]; fd {
//找到了
tail := b_end
for {
if tail.parent == nil {
break
}
road = append(road, tail.id)
tail = tail.parent
}
break
}
if len(OpenList) <= 0 {
//找不到路
break
}
var block_temp *block
for _, val := range OpenList {
if block_temp == nil {
block_temp = val
} else {
if val.gValue+val.hValue < block_temp.gValue+block_temp.hValue {
block_temp = val
}
}
}
// 找到后从开放列表中删除, 加入关闭列表
if block_temp != nil {
delete(OpenList, block_temp.id)
CloseList[block_temp.id] = block_temp
}
Run(block_temp, b_end, OpenList, CloseList)
}
//road = append(road, start)
return road
}
func Run(currentBlock, b_end *block, OpenList, CloseList map[int32]*block) {
// 找出邻居
neighbors := FindNeighbor(currentBlock.id)
for _, id := range neighbors {
block, ok := Map[id]
if ok {
// 已经在关闭列表,不考虑
_, fd := CloseList[block.id]
if fd {
continue
}
// 是障碍 不考虑
if block.isHinder {
continue
}
//
if _, fd := OpenList[block.id]; fd {
// 已经在开放列表
var g int32
if block.id+1 == currentBlock.id || block.id-1 == currentBlock.id || block.id+COL_NUM == currentBlock.id || block.id-COL_NUM == currentBlock.id {
g = 10
} else {
g = 15
}
if currentBlock.gValue+g < block.gValue {
block.parent = currentBlock
block.gValue = currentBlock.gValue + g
}
} else {
// 计算每个相邻格子的gh值
CountGH(currentBlock, block, b_end)
// 放入开放列表
OpenList[block.id] = block
block.parent = currentBlock
}
}
}
}
//获取[min, max)之间的随机数
func GetRandNumDown(min, max int32) (int32, error) {
if min > max {
return 0, fmt.Errorf("%d can not bigger than %d", min, max)
}
if min == max {
return min, nil
}
r := rand.New(rand.NewSource(time.Now().UnixNano()))
num := int32(r.Intn(int(max-min))) + min
return num, nil
}
func DrawMap(road []int32) {
if len(Map) != int(GRID_NUM) {
return
}
for i := int32(1); i <= GRID_NUM; i++ {
flag := false
for _, id := range road {
if i == id {
fmt.Print("\x1b[31m@\x1b[0m")
flag = true
}
}
if !flag {
block, _ := Map[i]
if block.isHinder {
fmt.Print("\x1b[33m#\x1b[0m")
} else {
fmt.Print("*")
}
}
if i%COL_NUM == 0 {
fmt.Print("\n")
}
}
}
func main() {
road := make([]int32, 0)
GenerateMap()
fmt.Println("原地图")
DrawMap(road)
fmt.Println("==================")
road = FindRoad(11, 4582)
if len(road) <= 0 {
fmt.Println("找不到路")
return
}
fmt.Println("路线图")
DrawMap(road)
fmt.Println("==================")
fmt.Printf("Road: %v", road)
}
转载于:https://my.oschina.net/u/569172/blog/649078