jps(Jump Point Search)跳点寻路算法代码实现

置顶 已于 2022-10-08 15:14:12 修改
阅读量1.5k 收藏 6
点赞数
分类专栏： 游戏 文章标签： jps寻路
于 2022-07-16 17:27:36 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/lsccsl/article/details/125822102
版权

算法原理已经有网友详细说明了

链接如下

JPS(jump point search)寻路算法_[奋斗不止]的博客-优快云博客_jps寻路JPS(jump point search)寻路算法JPS（jump point search）跳跃点寻路算法是对AStar寻路算法的一个改进。AStar 算法在扩展节点时会把所有相邻的节点考虑进去，当地图比较大时，openList 中的节点数量会很多，搜索效率较低。在两点之间没有障碍物时，那么中间的节点对我们来说是没有用的，不希望添加到 openList如下可以看到JPS 算法不是每个节点都紧密连接的，而是跳跃性的或者说只有需要转弯的节点(也叫拐点)才会被考虑，所以叫跳跃点(Jump Poinhttps://blog.youkuaiyun.com/LIQIANGEASTSUN/article/details/118766080笔者理解算法的过程，类似于从起点开始扩散，强迫邻居类似于拐点, 在有强迫邻居处需要转弯搜索

地图搜索路径

 地图搜索过程中拐点，以及相应的点之间树状关系

与a*算法比对会快

 同样的寻路a*的消耗与jps消耗对比，单位是毫秒

 go版本实现的jps代码:

github地址

lingo/MapJPS.go at main · lsccsl/lingo · GitHublin go. Contribute to lsccsl/lingo development by creating an account on GitHub.https://github.com/lsccsl/lingo/blob/main/src/lin/lin_common/MapJPS.go

jps代码：

package lin_common

import (
	"fmt"
	"math"
	"sort"
)

const (
	JPS_WEIGHT_slash = 14
	JPS_WEIGHT_straight = 10
	JPS_WEIGHT_scale = 10
)

func calJPSEndWeight(src Coord2d, dst Coord2d) int {
	//曼哈顿
	return int(math.Abs(float64(src.X - dst.X)) + math.Abs(float64(src.Y - dst.Y))) * JPS_WEIGHT_scale
}

type JPS_DIR int
const (
	JPS_DIR_up    JPS_DIR = 0
	JPS_DIR_down  JPS_DIR = 1
	JPS_DIR_left  JPS_DIR = 2
	JPS_DIR_right JPS_DIR = 3

	JPS_DIR_up_left    JPS_DIR = 4
	JPS_DIR_up_right   JPS_DIR = 5
	JPS_DIR_down_left  JPS_DIR = 6
	JPS_DIR_down_right JPS_DIR = 7

	JPS_DIR_MAX  JPS_DIR = 8
)

var array_dirJPS = [JPS_DIR_MAX]Coord2d{
	{0,-1},
	{0,1},
	{-1,0},
	{1,0},
	{-1,-1},
	{1,-1},
	{-1,1},
	{1,1},
}

type JPSNode struct {
	parent *JPSNode
	subNode []*JPSNode

	totalWeight int

	pos Coord2d
	startWeight int
	endWeight int
	forceNeighbor []Coord2d
}
type MAP_JSP_HISTORY_PATH map[Coord2d]*JPSNode
type JSPMgr struct {
	pMapData *MapData
	root *JPSNode
	nodes []*JPSNode
	mapHistoryPath MAP_JSP_HISTORY_PATH

	src Coord2d
	dst Coord2d
}
func (pthis*JSPMgr)Len() int {
	return len(pthis.nodes)
}
func (pthis*JSPMgr)Less(i, j int) bool {
	if i < 0 || i >= len(pthis.nodes) {
		return false
	}
	if j < 0 || j >= len(pthis.nodes) {
		return false
	}

	node_i := pthis.nodes[i]
	node_j := pthis.nodes[j]
	if node_i.totalWeight > node_j.totalWeight {
		return true
	}
	return false
}
func (pthis*JSPMgr)Swap(i, j int) {
	if i < 0 || i >= len(pthis.nodes) {
		return
	}
	if j < 0 || j >= len(pthis.nodes) {
		return
	}
	nodeTmp := pthis.nodes[i]
	pthis.nodes[i] = pthis.nodes[j]
	pthis.nodes[j] = nodeTmp
}
func (pthis*JSPMgr)addNode(node *JPSNode, parent *JPSNode) {
	if node == nil {
		return
	}

	if pthis.isInHistory(node.pos) {
		LogErr("already in history")
		return
	}

	node.totalWeight = node.startWeight + node.endWeight
	pthis.nodes = append(pthis.nodes, node)
	sort.Sort(pthis)
	pthis.addHistory(node)

	if node == pthis.root {
		return
	}

	if pthis.root == nil {
		pthis.root = node
	} else {
		if parent == nil {
			node.parent = pthis.root
		} else {
			node.parent = parent
		}
	}

	if node.parent != nil {
		node.parent.subNode = append(node.parent.subNode, node)
	}
}
func (pthis*JSPMgr)getNearestNode() *JPSNode {
	if len(pthis.nodes) == 0 {
		return nil
	}
	lastIdx := len(pthis.nodes) - 1
	node := pthis.nodes[lastIdx]
	pthis.nodes = pthis.nodes[:lastIdx]
	return node
}
func (pthis*JSPMgr)isInHistory(pos Coord2d) bool {
	_, ok := pthis.mapHistoryPath[pos]
	return ok
}
func (pthis*JSPMgr)addHistory(node *JPSNode) {
	pthis.mapHistoryPath[node.pos] = node
}

func getDirVector(dir JPS_DIR) *Coord2d{
	return &array_dirJPS[dir]
}

func (pthis*MapData)hasForceNeighbor(jpsMgr *JSPMgr, searchPos Coord2d, dir JPS_DIR) (bFindForceNeighbor bool, forceNeighbor []Coord2d) {
	bFindForceNeighbor = false
	forceNeighbor = nil

	if jpsMgr == nil {
		return
	}

	if jpsMgr.dst.IsEqual(&searchPos) {
		bFindForceNeighbor = true
		return
	}

	switch dir {
	case JPS_DIR_up:
		posLeft  := Coord2d{searchPos.X - 1, searchPos.Y}
		posRight := Coord2d{searchPos.X + 1, searchPos.Y}
		if pthis.CoordIsBlock(posLeft) {
			posN := Coord2d{posLeft.X, posLeft.Y - 1}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}
		if pthis.CoordIsBlock(posRight) {
			posN := Coord2d{posRight.X, posRight.Y - 1}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}

	case JPS_DIR_down:
		posLeft  := Coord2d{searchPos.X - 1, searchPos.Y}
		posRight := Coord2d{searchPos.X + 1, searchPos.Y}
		if pthis.CoordIsBlock(posLeft) {
			posN := Coord2d{posLeft.X, posLeft.Y + 1}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}
		if pthis.CoordIsBlock(posRight) {
			posN := Coord2d{posRight.X, posRight.Y + 1}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}

	case JPS_DIR_left:
		posUp   := Coord2d{searchPos.X, searchPos.Y - 1}
		posDown := Coord2d{searchPos.X, searchPos.Y + 1}
		if pthis.CoordIsBlock(posUp) {
			posN := Coord2d{posUp.X - 1, posUp.Y}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}
		if pthis.CoordIsBlock(posDown) {
			posN := Coord2d{posDown.X - 1, posDown.Y}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}

	case JPS_DIR_right:
		posUp   := Coord2d{searchPos.X, searchPos.Y - 1}
		posDown := Coord2d{searchPos.X, searchPos.Y + 1}

		if pthis.CoordIsBlock(posUp) {
			posN := Coord2d{posUp.X + 1, posUp.Y}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}
		if pthis.CoordIsBlock(posDown) {
			posN := Coord2d{posDown.X + 1, posDown.Y}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}

	case JPS_DIR_up_left:
		/*
           _ _ N
           _ @ *
		   N * \
		*/
		posRight := Coord2d{searchPos.X + 1, searchPos.Y}
		posDown  := Coord2d{searchPos.X, searchPos.Y + 1}
		if pthis.CoordIsBlock(posRight) {
			posN := Coord2d{posRight.X, posRight.Y - 1}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}
		if pthis.CoordIsBlock(posDown) {
			posN := Coord2d{posDown.X - 1, posDown.Y}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}

	case JPS_DIR_up_right:
		/*
            N _ _
            * @ _
		    / * N
		*/
		posLeft := Coord2d{searchPos.X - 1, searchPos.Y}
		posDown := Coord2d{searchPos.X, searchPos.Y + 1}
		if pthis.CoordIsBlock(posLeft) {
			posN := Coord2d{posLeft.X, posLeft.Y - 1}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}
		if pthis.CoordIsBlock(posDown) {
			posN := Coord2d{posDown.X + 1, posDown.Y}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}

	case JPS_DIR_down_left:
		/*
          N * /
          _ @ *
          _ _ N
		*/
		posUp    := Coord2d{searchPos.X, searchPos.Y - 1}
		posRight := Coord2d{searchPos.X + 1, searchPos.Y}
		if pthis.CoordIsBlock(posUp) {
			posN := Coord2d{posUp.X - 1, posUp.Y}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}
		if pthis.CoordIsBlock(posRight) {
			posN := Coord2d{posRight.X, posRight.Y + 1}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}

	case JPS_DIR_down_right:
		/*
            \ * N
            * @ _
            N _ _
		*/
		posUp   := Coord2d{searchPos.X, searchPos.Y - 1}
		posLeft := Coord2d{searchPos.X - 1, searchPos.Y}
		if pthis.CoordIsBlock(posUp) {
			posN := Coord2d{posUp.X + 1, posUp.Y}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}
		if pthis.CoordIsBlock(posLeft) {
			posN := Coord2d{posLeft.X, posLeft.Y + 1}
			if !pthis.CoordIsBlock(posN) {
				bFindForceNeighbor = true
				forceNeighbor = append(forceNeighbor, posN)
			}
		}
	}

	return
}

func (pthis*MapData)searchHorVer(jpsMgr *JSPMgr, curNode *JPSNode, curPos Coord2d, enDir JPS_DIR, bAdd bool) (findJump bool) {
	findJump = false
	dir := getDirVector(enDir)
	if dir == nil {
		return
	}
	searchPos := curPos
	curWeightAdd := 0
	for {
		searchPos = Coord2d{searchPos.X + dir.X, searchPos.Y + dir.Y}
		curWeightAdd += JPS_WEIGHT_straight

		if pthis.IsBlock(searchPos.X, searchPos.Y) {
			break
		}

		bFindForceNeighbor, forceNeighbor := pthis.hasForceNeighbor(jpsMgr, searchPos, enDir)
		if bFindForceNeighbor {
			findJump = true
			if bAdd && !jpsMgr.isInHistory(searchPos){
				jp := &JPSNode{
					pos:searchPos,
					endWeight:calJPSEndWeight(searchPos, jpsMgr.dst),
					startWeight:curNode.startWeight + curWeightAdd,
					forceNeighbor:forceNeighbor,
				}

				// add pos to open list
				jpsMgr.addNode(jp, curNode)

				for _, val := range forceNeighbor {
					neighborJP := &JPSNode{
						pos:val,
						endWeight:calJPSEndWeight(val, jpsMgr.dst),
						startWeight:jp.startWeight + JPS_WEIGHT_slash,
						forceNeighbor:nil,
					}
					jpsMgr.addNode(neighborJP, jp)
				}
				//pthis.DumpJPSMap("../resource/jumppath.bmp", nil, jpsMgr)
			}
			break
		}
	}

	return
}

func (pthis*MapData)searchSlash(jpsMgr *JSPMgr, curNode *JPSNode, curPos Coord2d, enDir JPS_DIR) {

	bFindForceNeighbor, forceNeighbor := pthis.hasForceNeighbor(jpsMgr, curPos, enDir)
	if bFindForceNeighbor {
		curNode.forceNeighbor = append(curNode.forceNeighbor, forceNeighbor...)

		if forceNeighbor != nil {
			for _, val := range forceNeighbor {
				neighborJP := &JPSNode{
					pos:val,
					endWeight:calJPSEndWeight(val, jpsMgr.dst),
					startWeight:curNode.startWeight + JPS_WEIGHT_slash,
					forceNeighbor:nil,
				}
				jpsMgr.addNode(neighborJP, curNode)
			}
		}
	}

	dir := getDirVector(enDir)
	newPos := Coord2d{curPos.X, curPos.Y}
	curWeightAdd := 0
	for {
		newPos = Coord2d{newPos.X + dir.X, newPos.Y + dir.Y}
		curWeightAdd += JPS_WEIGHT_slash

		if pthis.CoordIsBlock(newPos) {
			break
		}

		bFindForceNeighbor, forceNeighbor := pthis.hasForceNeighbor(jpsMgr, newPos, enDir)
		if bFindForceNeighbor && !jpsMgr.isInHistory(newPos){
			jp := &JPSNode{
				pos:newPos,
				endWeight:calJPSEndWeight(newPos, jpsMgr.dst),
				startWeight:curNode.startWeight + curWeightAdd,
				forceNeighbor:forceNeighbor,
			}

			// add pos to open list
			jpsMgr.addNode(jp, curNode)

			if forceNeighbor != nil {
				for _, val := range forceNeighbor {
					neighborJP := &JPSNode{
						pos:val,
						endWeight:calJPSEndWeight(val, jpsMgr.dst),
						startWeight:jp.startWeight + JPS_WEIGHT_slash,
						forceNeighbor:nil,
					}
					jpsMgr.addNode(neighborJP, jp)
				}
			}
			break
		}

		findJump := false
		// 横向 纵向搜索
		switch enDir {
		case JPS_DIR_up_left:
			findJump = pthis.searchHorVer(jpsMgr, curNode, newPos, JPS_DIR_up, false)
			if !findJump {
				findJump = pthis.searchHorVer(jpsMgr, curNode, newPos, JPS_DIR_left, false)
			}

		case JPS_DIR_up_right:
			findJump = pthis.searchHorVer(jpsMgr, curNode, newPos, JPS_DIR_up, false)
			if !findJump {
				findJump = pthis.searchHorVer(jpsMgr, curNode, newPos, JPS_DIR_right, false)
			}

		case JPS_DIR_down_left:
			findJump = pthis.searchHorVer(jpsMgr, curNode, newPos, JPS_DIR_down, false)
			if !findJump {
				findJump = pthis.searchHorVer(jpsMgr, curNode, newPos, JPS_DIR_left, false)
			}

		case JPS_DIR_down_right:
			findJump = pthis.searchHorVer(jpsMgr, curNode, newPos, JPS_DIR_down, false)
			if !findJump {
				findJump = pthis.searchHorVer(jpsMgr, curNode, newPos, JPS_DIR_right, false)
			}
		}

		if findJump && !jpsMgr.isInHistory(newPos){
			jp := &JPSNode{
				pos:newPos,
				endWeight:calJPSEndWeight(newPos, jpsMgr.dst),
				startWeight:curNode.startWeight + curWeightAdd,
				forceNeighbor:nil,
			}

			jpsMgr.addNode(jp, curNode)
			//pthis.DumpJPSMap("../resource/jumppath.bmp", nil, jpsMgr)
		}
	}
}

type SearchDirData struct{
	dir JPS_DIR
	pos Coord2d
}
type TmpRelativeData struct {
	relativePos Coord2d
	pos Coord2d
}
func (pthis*MapData)PathJPS(src Coord2d, dst Coord2d) (path []Coord2d, jpsMgr *JSPMgr) {

	defer func() {
		err := recover()
		if err != nil {
			fmt.Println(err)
		}
	}()

	/*
		(1)节点 A 是起点、终点.
		(2)节点A 至少有一个强迫邻居. 以横向x为例,说明此时被阻挡了,并且越过这个点之后,有新的连通区域
		(3)父节点在斜方向(斜向搜索)，节点A的水平或者垂直方向上有满足 (1)、(2) 的节点 用于转向

	(1).如果节点A没有父方向P(起点)
	则直线方向按照 (上下左右)四个方向， dirList = {上、下、左、右}
	斜方向按照(左上、右上、右下、左下)四个方向搜索 dirList = {左上、右上、右下、左下}
	(2).如果节点A有父方向P
	则 PA=（X,Y）
	将PA分解为水平 horizontalDir=(X,0)，垂直 verticalDir=(0,Y)

	还是先考虑水平和垂直的搜索方向,dirList = {}
	如果 horizontalDir=(X,0) != (0, 0) 即 X ！= 0 则 将 horizontalDir 加入到 dirList
	如果 verticalDir=(0,Y) !=(0, 0) 即 Y ！= 0 则 将 verticalDir 加入到 dirList
	直线方向搜索 dirList 中的方向

	然后是斜向 dirList = {}
	如果 PA=(X,Y)，X ！= 0 且 Y ！= 0， 则将 PA方向加入到 dirList
	如果 A有强迫邻居 {N1, N2, N3…},则将 AN1，AN2，AN3，。。。都加入到 dirList

	强近邻居是斜的,所以要分解

	————————————————
	版权声明：本文为优快云博主「[奋斗不止]」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
	原文链接：https://blog.youkuaiyun.com/LIQIANGEASTSUN/article/details/118766080
	http://qiao.github.io/PathFinding.js/visual/
	*/

	startNode := &JPSNode{
		parent: nil,
		pos:src,
		endWeight:calJPSEndWeight(src, dst),
	}
	startNode.totalWeight = startNode.endWeight

	jpsMgr = &JSPMgr{
		pMapData:pthis,
		root:startNode,
		mapHistoryPath: make(MAP_JSP_HISTORY_PATH),
		src:src,
		dst:dst,
	}
	jpsMgr.addNode(startNode, nil)

	node := startNode

	for {
		curNode := jpsMgr.getNearestNode()
		if curNode == nil {
			node = curNode
			//pthis.DumpJPSMap("../resource/jumppath.bmp", nil, jpsMgr)
			break
		}
		curPos := curNode.pos

		if curPos.IsNear(&dst) {
			//pthis.DumpJPSMap("../resource/jumppath.bmp", nil, jpsMgr)
			node = curNode
			break
		}

		var straightDir []SearchDirData
		var slashDir []SearchDirData

		if curNode.parent != nil {
			// 根据父节点相对位置
			relativeParentDir := curNode.pos.Dec(&curNode.parent.pos)
			relativeData := TmpRelativeData{relativeParentDir, curPos}

			if relativeData.relativePos.X > 0 {
				straightDir = append(straightDir, SearchDirData{JPS_DIR_right,relativeData.pos})
			} else if relativeData.relativePos.X < 0 {
				straightDir = append(straightDir, SearchDirData{JPS_DIR_left,relativeData.pos})
			}
			if relativeData.relativePos.Y > 0 {
				straightDir = append(straightDir, SearchDirData{JPS_DIR_down,relativeData.pos})
			} else if relativeData.relativePos.Y < 0 {
				straightDir = append(straightDir, SearchDirData{JPS_DIR_up,relativeData.pos})
			}

			if relativeData.relativePos.X > 0 && relativeData.relativePos.Y > 0 {
				slashDir = append(slashDir, SearchDirData{JPS_DIR_down_right,relativeData.pos})
			}
			if relativeData.relativePos.X > 0 && relativeData.relativePos.Y < 0 {
				slashDir = append(slashDir, SearchDirData{JPS_DIR_up_right,relativeData.pos})
			}
			if relativeData.relativePos.X < 0 && relativeData.relativePos.Y > 0 {
				slashDir = append(slashDir, SearchDirData{JPS_DIR_down_left,relativeData.pos})
			}
			if relativeData.relativePos.X < 0 && relativeData.relativePos.Y < 0 {
				slashDir = append(slashDir, SearchDirData{JPS_DIR_up_left,relativeData.pos})
			}
		} else {
			for i := JPS_DIR_up; i <= JPS_DIR_right; i ++ {
				straightDir = append(straightDir, SearchDirData{i, curPos})
			}

			for  i := JPS_DIR_up_left; i <= JPS_DIR_down_right; i ++ {
				slashDir = append(slashDir, SearchDirData{i, curPos})
			}
		}

		for _, val := range straightDir {
			pthis.searchHorVer(jpsMgr, curNode, val.pos, val.dir, true)
		}

		for _, val := range slashDir {
			pthis.searchSlash(jpsMgr, curNode, val.pos, val.dir)
		}
	}

	if node != nil {
		for ;node != nil; {
			path = append(path, node.pos)
			node = node.parent
		}
		return
	}

	return
}

/*func (pthis*MapData)PathSearch(src Coord2d, dst Coord2d) (path []Coord2d) {
	pathJPS, _ := pthis.PathJPS(src, dst)
	if pathJPS == nil {
		return
	}

	pathLen := len(pathJPS)
	if pathLen <= 1 {
		return
	}

	xdiff := 0
	ydiff := 0
	var pathMerge []Coord2d
	pathMerge = append(pathMerge, pathJPS[0])
	for i := 0; i < pathLen - 1; i ++ {
		pos1 := pathJPS[i]
		pos2 := pathJPS[i + 1]

		curXDiff := pos2.X - pos1.X
		curYDiff := pos2.Y - pos1.Y

		dotX := xdiff * curXDiff
		dotY := ydiff * curYDiff

		if dotX < 0 || dotY < 0 {
			pathMerge = append(pathMerge, pathJPS[i])
		}

		if curXDiff != 0 {
			xdiff = curXDiff
		}
		if curYDiff != 0 {
			ydiff = curYDiff
		}
	}

	pathMerge = append(pathMerge, pathJPS[pathLen - 1])

	pathMergeLen := len(pathMerge)
	for i := 0; i < pathMergeLen - 1; i ++ {
		pos1 := pathMerge[i]
		pos2 := pathMerge[i + 1]
		pathSeg, _ := pthis.PathSearchAStart(pos1, pos2)
		path = append(path, pathSeg...)

		//fileName := "../resource/jump_path_seg_" + strconv.FormatInt(int64(i), 10) + ".bmp"
		//pthis.DumpMap(fileName, pathSeg, &pos1, &pos2, searchMgr)
	}

	//pthis.DumpMap("../resource/jump_merge_path.bmp", path, &src, &dst, nil)

	return
}*/

func (pthis*MapData)DumpNodeSub(searchMgr *JSPMgr, node *JPSNode, bmp * Bitmap) {
	tmpBMP := bmp.BmpData
	widBytePitch := bmp.widBytePitch
	for _, val := range node.subNode {
		coordDiff := val.pos.Dec(&node.pos)
		if coordDiff.X != 0 {
			coordDiff.X = coordDiff.X / int(math.Abs(float64(coordDiff.X)))
		}
		if coordDiff.Y != 0 {
			coordDiff.Y = coordDiff.Y / int(math.Abs(float64(coordDiff.Y)))
		}

		pos := node.pos.Add(&coordDiff)
		for {
			idx := pos.Y * widBytePitch + pos.X * 3
			if idx < 0 || idx >= bmp.GetPitchByteWidth() * bmp.GetHeight() {
				break
			}
			tmpBMP[idx + 0] = 0xff
			tmpBMP[idx + 1] = 0
			tmpBMP[idx + 2] = 0xff

			pos = pos.Add(&coordDiff)
			if pos.IsNear(&val.pos) {
				break
			}
		}

		pthis.DumpNodeSub(searchMgr, val, bmp)
	}
}

func (pthis*MapData)DumpJPSMap(strMapFile string, path []Coord2d, searchMgr *JSPMgr) {

	widBytePitch := CalWidBytePitch(pthis.widReal, 24)
	dataLen := widBytePitch * pthis.hei
	tmpBMP := make([]uint8, dataLen)
	bmp := CreateBMP(pthis.widReal, pthis.hei, 24, nil)
	bmp.BmpData = tmpBMP

	//draw map
	for j := 0; j < pthis.hei; j ++ {
		for i := 0; i < pthis.widReal; i ++ {
			var clr uint8 = 0xff
			if pthis.IsBlock(i, j) {
				clr = 0
			}
			newIdx := j * widBytePitch + i * 3
			tmpBMP[newIdx + 0] = clr
			tmpBMP[newIdx + 1] = clr
			tmpBMP[newIdx + 2] = clr
		}
	}

	// draw node tree
	pthis.DumpNodeSub(searchMgr, searchMgr.root, bmp)

	if searchMgr != nil {
		for key, node := range searchMgr.mapHistoryPath {
			idx := key.Y*widBytePitch + key.X * 3
			tmpBMP[idx+0] = 0
			tmpBMP[idx+1] = 0xff
			tmpBMP[idx+2] = 0

			for _, val := range node.forceNeighbor {
				idx := val.Y*widBytePitch + val.X * 3
				tmpBMP[idx+0] = 0
				tmpBMP[idx+1] = 0
				tmpBMP[idx+2] = 0xff
			}
		}
	}

/*	if path != nil {
		for _, val := range path {
			idx := val.Y*widBytePitch + val.X * 3
			tmpBMP[idx+0] = 0
			tmpBMP[idx+1] = 0
			tmpBMP[idx+2] = 0xff
		}
	}*/

	{
		idx := searchMgr.src.Y*widBytePitch + searchMgr.src.X * 3
		tmpBMP[idx+0] = 0xff
		tmpBMP[idx+1] = 0
		tmpBMP[idx+2] = 0
	}
	{
		idx := searchMgr.dst.Y*widBytePitch + searchMgr.dst.X * 3
		tmpBMP[idx+0] = 0xff
		tmpBMP[idx+1] = 0
		tmpBMP[idx+2] = 0
	}

	bmp.WriteBmp(strMapFile)
}

对应的地图以及位图读取代码

地图代码:

package lin_common

import "math"

const (
	WEIGHT_slash = 14
	WEIGHT_straight = 10
	WEIGHT_scale = 10
)

type Coord2d struct {
	X int
	Y int
}

func (pthis*Coord2d)Add(r*Coord2d) Coord2d {
	return Coord2d{pthis.X + r.X, pthis.Y + r.Y}
}
func (pthis*Coord2d)Dec(r*Coord2d) Coord2d {
	return Coord2d{pthis.X - r.X, pthis.Y - r.Y}
}
func (pthis*Coord2d)IsEqual(r*Coord2d) bool {
	return pthis.X ==  r.X && pthis.Y == r.Y
}
func (pthis*Coord2d)IsNear(r*Coord2d) bool {
	return math.Abs(float64(pthis.X -  r.X)) <= 1 && math.Abs(float64(pthis.Y - r.Y)) <= 1
}

type MapData struct {
	widReal int
	widBytePitch int
	hei int

	mapBit []uint8
}

func (pthis*MapData)LoadMap(mapFile string)error{
	bmp := Bitmap{}
	err := bmp.ReadBmp(mapFile)
	if err != nil {
		return err
	}

	pthis.mapBit = bmp.BmpData
	pthis.widReal = bmp.GetRealWidth()
	pthis.widBytePitch = bmp.GetPitchByteWidth()
	pthis.hei = bmp.GetHeight()

	return nil
}

func (pthis*MapData)CoordIsBlock(pos Coord2d)bool{
	return pthis.IsBlock(pos.X, pos.Y)
}
func (pthis*MapData)IsBlock(x int, y int)bool{
	if x < 0 || x >= pthis.widReal {
		return true
	}
	if y < 0 || y >= pthis.hei {
		return true
	}

	idxByte := y * pthis.widBytePitch + x/8
	idxBit := 7 - x % 8
	posByte := pthis.mapBit[idxByte]
	posBit := posByte & (1 << idxBit)

	return posBit == 0
}




func (pthis*MapData)DumpMap(strMapFile string, path []Coord2d, src * Coord2d , dst * Coord2d, searchMgr *SearchMgr) {

	widBytePitch := CalWidBytePitch(pthis.widReal, 24)
	dataLen := widBytePitch * pthis.hei
	tmpBMP := make([]uint8, dataLen)

	for j := 0; j < pthis.hei; j ++ {
		for i := 0; i < pthis.widReal; i ++ {
			var clr uint8 = 0xff
			if pthis.IsBlock(i, j) {
				clr = 0
			}
			newIdx := j * widBytePitch + i * 3
			tmpBMP[newIdx + 0] = clr
			tmpBMP[newIdx + 1] = clr
			tmpBMP[newIdx + 2] = clr
		}
	}

	if searchMgr != nil {
		for key, _ := range searchMgr.mapHistoryPath {
			idx := key.Y*widBytePitch + key.X * 3
			tmpBMP[idx+0] = 0
			tmpBMP[idx+1] = 0xff
			tmpBMP[idx+2] = 0
		}
	}

	if path != nil {
		for _, val := range path {
			idx := val.Y*widBytePitch + val.X * 3
			tmpBMP[idx+0] = 0
			tmpBMP[idx+1] = 0
			tmpBMP[idx+2] = 0xff
		}
	}

	if src != nil {
		idx := src.Y*widBytePitch + src.X * 3
		tmpBMP[idx+0] = 0xff
		tmpBMP[idx+1] = 0
		tmpBMP[idx+2] = 0
	}
	if dst != nil {
		idx := dst.Y*widBytePitch + dst.X * 3
		tmpBMP[idx+0] = 0xff
		tmpBMP[idx+1] = 0
		tmpBMP[idx+2] = 0
	}

	bmp := CreateBMP(pthis.widReal, pthis.hei, 24, tmpBMP)

	bmp.WriteBmp(strMapFile)
}

位图读取代码:

package lin_common

import (
	"encoding/binary"
	"os"
)

/*
typedef struct tagBITMAPFILEHEADER
{
UINT16 bfType;
DWORD bfSize;
UINT16 bfReserved1;
UINT16 bfReserved2;
DWORD bfOffBits;
} BITMAPFILEHEADER;
*/

const (
	BITMAP_BIN_HEADER int = 40
	BITMAP_HEADER_TOTAL int = 54
)

type BitmapFileHeader struct {
	BfType uint16 // BM
	BfSize uint32
	BfReserved1 uint16 // 0
	BfReserved2 uint16 // 0
	BfOffBits uint32
}//14bit

type BitmapInfoHeader struct {
	Size   uint32
	Width   int32
	Height   int32
	Places   uint16
	BitCount  uint16
	Compression uint32
	SizeImage  uint32
	XperlsPerMeter int32
	YperlsPerMeter int32
	ClsrUsed  uint32
	ClrImportant uint32
}//40bit

type Bitmap struct{
	BfHeader BitmapFileHeader
	Biheader BitmapInfoHeader

	widBytePitch int

	BmpData []uint8
	BinPalette []uint8
}

func CalWidBytePitch(wid int, bitCount int) int {
	widByte := (int(wid) * int(bitCount) + 7)/8
	return ((widByte + 3)/4) * 4
}

func (pthis*Bitmap)calWidBytePitch() {
	pthis.widBytePitch = CalWidBytePitch(int(pthis.Biheader.Width), int(pthis.Biheader.BitCount))
/*	widByte := (int(pthis.Biheader.Width) * int(pthis.Biheader.BitCount) + 7)/8
	pthis.widBytePitch = ((widByte + 3)/4) * 4*/
}

func (pthis*Bitmap)ReadBmp(bmpFile string)error{
	file, err := os.Open(bmpFile)
	if err != nil {
		return err
	}

	defer file.Close()

	//type拆成两个byte来读
	//Read第二个参数字节序一般windows/linux大部分都是LittleEndian,苹果系统用BigEndian
	//binary.Read(file, binary.LittleEndian, &pthis.headA)
	//binary.Read(file, binary.LittleEndian, &pthis.headB)
	err = binary.Read(file, binary.LittleEndian, &pthis.BfHeader.BfType)
	if err != nil {
		return err
	}

	//文件大小
	err = binary.Read(file, binary.LittleEndian, &pthis.BfHeader.BfSize)
	if err != nil {
		return err
	}

	//预留字节
	err = binary.Read(file, binary.LittleEndian, &pthis.BfHeader.BfReserved1)
	if err != nil {
		return err
	}
	err = binary.Read(file, binary.LittleEndian, &pthis.BfHeader.BfReserved2)
	if err != nil {
		return err
	}

	//偏移字节
	err = binary.Read(file, binary.LittleEndian, &pthis.BfHeader.BfOffBits)
	if err != nil {
		return err
	}

	err = binary.Read(file, binary.LittleEndian, &pthis.Biheader)
	if err != nil {
		return err
	}
	pthis.calWidBytePitch()

	szPalette := int(pthis.BfHeader.BfOffBits) - BITMAP_HEADER_TOTAL
	if szPalette > 0 {
		pthis.BinPalette = make([]uint8, szPalette)
		err = binary.Read(file, binary.LittleEndian, pthis.BinPalette)
		if err != nil {
			return err
		}
	}

	pthis.BmpData = make([]uint8, pthis.Biheader.SizeImage)
	err = binary.Read(file, binary.LittleEndian, pthis.BmpData)
	if err != nil {
		return err
	}
	return nil
}

func (pthis*Bitmap)WriteBmp(bmpFile string)error{
	file, err := os.Create(bmpFile)
	if err != nil {
		return err
	}

	defer func() {
		if file != nil {
			file.Sync()
			file.Close()
		}
	}()

	// write BM
	err = binary.Write(file, binary.LittleEndian, uint8('B'))
	if err != nil {
		return err
	}
	err = binary.Write(file, binary.LittleEndian, uint8('M'))
	if err != nil {
		return err
	}

	headerLen := BITMAP_HEADER_TOTAL
	if pthis.BinPalette != nil {
		headerLen += len(pthis.BinPalette)
	}
	//write total file size
	err = binary.Write(file, binary.LittleEndian, uint32(headerLen + len(pthis.BmpData)))
	if err != nil {
		return err
	}

	//write reserve
	err = binary.Write(file, binary.LittleEndian, uint16(0))
	if err != nil {
		return err
	}
	err = binary.Write(file, binary.LittleEndian, uint16(0))
	if err != nil {
		return err
	}

	//write offset
	err = binary.Write(file, binary.LittleEndian, uint32(headerLen))
	if err != nil {
		return err
	}

	//write bitmap info
	err = binary.Write(file, binary.LittleEndian, &pthis.Biheader)
	if err != nil {
		return err
	}

	//write palette
	err = binary.Write(file, binary.LittleEndian, pthis.BinPalette)
	if err != nil {
		return err
	}

	//write bitmap bin data
	err = binary.Write(file, binary.LittleEndian, pthis.BmpData)
	if err != nil {
		return err
	}
	return nil
}

func (pthis*Bitmap)GetRealWidth() int {
	return int(pthis.Biheader.Width)
}

func (pthis*Bitmap)GetPitchByteWidth() int {
	return pthis.widBytePitch
}

func (pthis*Bitmap)GetHeight() int {
	return int(pthis.Biheader.Height)
}

func CreateBMP(w int, h int, bitCount int, binBMP []uint8) *Bitmap {
	bmp := &Bitmap{}

	bmp.BfHeader.BfSize = uint32(BITMAP_HEADER_TOTAL + len(binBMP))
	bmp.BfHeader.BfOffBits = uint32(BITMAP_HEADER_TOTAL)

	bmp.Biheader.Size = uint32(BITMAP_BIN_HEADER)
	bmp.Biheader.Width = int32(w)
	bmp.Biheader.Height = int32(h)
	bmp.Biheader.Places = 1
	bmp.Biheader.BitCount = uint16(bitCount)
	bmp.Biheader.Compression = 0
	bmp.Biheader.SizeImage = uint32(len(binBMP))
	bmp.Biheader.XperlsPerMeter = 0
	bmp.Biheader.YperlsPerMeter = 0
	bmp.Biheader.ClsrUsed = 0
	bmp.Biheader.ClrImportant = 0

	bmp.calWidBytePitch()

	if binBMP != nil {
		bmp.BmpData = make([]uint8, len(binBMP))
		copy(bmp.BmpData, binBMP)
	}

	return bmp
}

【全局路径规划模块图搜索算法】jump point search(JPS)跳点搜索算法（附C++代码）
qq_35635374的博客
04-28 670
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对** jump point search(JPS)跳点搜索算法**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章提示：以下是本篇文章正文内容如果节点n是x的邻居，并且节点n的邻居有阻挡（不可行走的格子），并且从parent(x)、x、n的路径长度比其他任何从parent(x)到n且不经过x的路径短，其中parent(x)为路径中x的前一个点，则n为x的强迫邻居，x为n的跳点。
C++ 实现JPS(完整代码)
m0_63608714的博客
06-26 902
【代码】C++ 实现JPS(完整代码)
JPS(jump point search)寻路算法
热门推荐
lq
07-16 1万+
JPS(jump point search)寻路算法 JPS（jump point search）跳跃点寻路算法是对AStar寻路算法的一个改进。 AStar 算法在扩展节点时会把所有相邻的节点考虑进去，当地图比较大时，openList 中的节点数量会很多，搜索效率较低。 在两点之间没有障碍物时，那么中间的节点对我们来说是没有用的，不希望添加到 openList 如下 可以看到JPS 算法不是每个节点都紧密连接的，而是跳跃性的或者说只有需要转弯的节点(也叫拐点)才会被考虑，所以叫跳跃点(Jump Poin
跳点搜索算法JPS及其优化（万字长文）
王杰的博客
06-24 1万+
欢迎关注作者git博客 1.引言   寻路算法用途众多，例如在游戏和地图中。A*算法已经众所周知，对于其优化也是层出不穷，然而性能并没有取得突破性进展。本文介绍JPS的效率、多线程、内存、路径优化算法。为了测试搜索算法的优化性能，实验中设置游戏场景使得起点和终点差距200个格子，需要寻路10000次。结果发现，A*寻路总时间约2.6074x1011纳秒（一秒为109纳秒）；基础版JPS寻路总时间1...
C++实现 JPS算法(Jump Point Search)
m0_63608714的博客
03-17 2230
C++实现 JPS算法(Jump Point Search)
第2章 第3节-跳点算法JPS
zheshui69163的博客
08-09 1714
1
JPS(Jump Point Search) C# 跳点寻路算法完整代码实现
ITdqr的博客
09-05 1630
纯C#实现JPS(jump point search) 寻路算法代码提供给初学者学习
JPS（Jump Point Search）寻路及实现代码分析
燕临江下的蛋
06-10 1万+
JPS（Jump Point Search）寻路 参考资料：2018腾讯移动游戏技术评审标准与实践案例 https://blog.youkuaiyun.com/yuxikuo_1/article/details/50406651 http://blog.sina.com.cn/s/blog_4a5c75d40102wo5l.html https://github.com/SkylerAlvarez/JumpP...
JPS+(Jump point search plus)寻路算法
lq
03-19 1348
JPS+ 是在 Jps 的基础上对地图进行了预处理，将数据记录在节点上 JPS 是运行时计算节点在某个方向上的跳点 JPS+ 预处理则是将 每个节点每个方向上的跳点、障碍信息记录，在运行时，不需要经过计算，直接获取某个方向上的跳点，节省大量时间和空间，提升计算速度 JPS+ 地图预处理 前提条件 1：地图必须是静态的，预处理时和运行时地图必须是一样的，不对在运行时添加、删除、修改节点状态 2：预处理需要每个节点存储一些数据，对内存占用较大 3：预处理需要时间较长，地图内每个节点都要进行数据处理，时间复杂
路径规划——Jump Point Search算法
最新发布
weixin_51995147的博客
08-20 2825
Jump Point Search (JPS，跳点搜索算法) 是一种优化的路径搜索算法，主要用于加速A*算法在网格图上的搜索过程，特别是在规则网格（如四向或八向网格）上表现出色。JPS通过跳过一些冗余节点，减少了需要扩展的节点数量，从而加速了搜索过程。
JPS（Jump Point Search）A*跳点搜索
08-16
JPS算法的实现！（C#版本）
jpsplus+goalbounding寻路算法
10-28
据说被A*快100倍的寻路算法，2015年提出。jps plus是在jps算法基础上对地图做了预处理，记录了一些跳点信息，加快寻找下一个跳点的速度；goalbinding做了另一些预处理，记录一些方向信息，避免往错误方向上寻找跳点。附上一个mfc实现jps+算法的demo
交互图象JPS代码
12-29
交互图象JPS代码 网页图象交互。
JPS算法的python实现及其可视化
08-06
在文件目录启动命令行窗口，输入python Main.py -v map.bmp即可运行
JPS（Jump Point Search）跳点搜索路径规划算法回顾
慕羽★的博客
06-17 2607
本篇文章主要回顾一下几年前学的JPS跳点搜索规划算法的相关内容，之前学的时候没有进行概括总结，现在补上
JPS 跳点算法源码分析
qq_40606107的博客
12-19 1919
JPS算法源码分析
路径规划 | 图解跳点搜索JPS算法(附ROS C++/Python/Matlab仿真)
FRIGIDWINTER的博客
02-20 9441
跳点搜索JPS算法在障碍密集的情况下，计算速度比传统A*算法提升若干数量级。本文图解JPS算法原理，并提供ROS C++、Python、Matlab三种仿真环境的实验代码
JPS寻路算法
weixin_34342207的博客
12-18 653
2019独角兽企业重金招聘Python工程师标准>>> ...
一文弄懂基于图搜索的路径规划算法JPS（有python代码）
勿忘初心,方得始终
09-14 4472
一文弄懂基于图搜索的路径规划算法JPS（有python代码）
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值