Golang实现A星路径搜索算法

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