294 lines
11 KiB
TypeScript
294 lines
11 KiB
TypeScript
|
import SKDataUtil from "../gear/SKDataUtil";
|
|||
|
|
|
|||
|
|
export default class PathUtil {
|
|||
|
|
|
|||
|
|
static getAvailabelPoint(r: number, l: number, mapArr: any[], rows: number, lines: number): any {
|
|||
|
|
if (r < 0) {
|
|||
|
|
r = 0;
|
|||
|
|
}
|
|||
|
|
if (l < 0) {
|
|||
|
|
l = 0;
|
|||
|
|
}
|
|||
|
|
if (r >= rows) {
|
|||
|
|
r = rows - 1;
|
|||
|
|
}
|
|||
|
|
if (l >= lines) {
|
|||
|
|
l = lines - 1;
|
|||
|
|
}
|
|||
|
|
let value = SKDataUtil.intOf2Array(mapArr, r, l, 0);
|
|||
|
|
if (value != 0) {
|
|||
|
|
return {
|
|||
|
|
r: r,
|
|||
|
|
l: l
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
let count = 1;
|
|||
|
|
for (let i = 0; i < 10000000; i++) {
|
|||
|
|
if (count > lines && count > rows) {
|
|||
|
|
return {
|
|||
|
|
r: -1,
|
|||
|
|
l: -1
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
if (r + count < rows && mapArr[r + count][l] != 0) {
|
|||
|
|
return {
|
|||
|
|
r: r + count,
|
|||
|
|
l: l
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
if (l + count < lines && mapArr[r][l + count] != 0) {
|
|||
|
|
return {
|
|||
|
|
r: r,
|
|||
|
|
l: l + count
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
if (r >= count && mapArr[r - count][l] != 0) {
|
|||
|
|
return {
|
|||
|
|
r: r - count,
|
|||
|
|
l: l
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
if (l >= count && mapArr[r][l - count] != 0) {
|
|||
|
|
return {
|
|||
|
|
r: r,
|
|||
|
|
l: l - count
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
if (r + count < rows && l + count < lines && mapArr[r + count][l + count] != 0) {
|
|||
|
|
return {
|
|||
|
|
r: r + count,
|
|||
|
|
l: l + count
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
if (r >= count && l >= count && mapArr[r - count][l - count] != 0) {
|
|||
|
|
return {
|
|||
|
|
r: r - count,
|
|||
|
|
l: l - count
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
if (r >= count && l + count < lines && mapArr[r - count][l + count] != 0) {
|
|||
|
|
return {
|
|||
|
|
r: r - count,
|
|||
|
|
l: l + count
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
if (l >= count && r + count < rows && mapArr[r + count][l - count] != 0) {
|
|||
|
|
return {
|
|||
|
|
r: r + count,
|
|||
|
|
l: l - count
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
count++;
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
// 寻路
|
|||
|
|
static searchRoad(start_r: number, start_l: number, end_r: number, end_l: number, mapArr: any[], rows: number, cols: number): any[] {
|
|||
|
|
if (end_r == -1 && end_l == -1) {
|
|||
|
|
return [];
|
|||
|
|
}
|
|||
|
|
let openList: any[] = [], //开启列表
|
|||
|
|
closeObjList: any = {}, //关闭列表索引
|
|||
|
|
openObjList: any = {}, //开启列表索引
|
|||
|
|
result: any[] = [], //结果数组
|
|||
|
|
result_index: number = 0; //终点位置
|
|||
|
|
if (start_r == end_r && start_l == end_l) {
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
openList.push({
|
|||
|
|
r: start_r,
|
|||
|
|
l: start_l,
|
|||
|
|
G: 0
|
|||
|
|
}); //把当前点加入到开启列表中，并且G是0
|
|||
|
|
openObjList[start_r * cols + start_l] = start_r * cols + start_l;
|
|||
|
|
do {
|
|||
|
|
var currentPoint = openList[0];
|
|||
|
|
if (openList.length > 1) {
|
|||
|
|
openList[0] = openList[openList.length - 1];
|
|||
|
|
this.minheap_filterdown(openList, 0, openObjList, cols);
|
|||
|
|
}
|
|||
|
|
openList.splice(openList.length - 1, 1);
|
|||
|
|
closeObjList[currentPoint.r * cols + currentPoint.l] = currentPoint;
|
|||
|
|
delete openObjList[currentPoint.r * cols + currentPoint.l];
|
|||
|
|
let surroundPoint = this.SurroundPoint(currentPoint);
|
|||
|
|
for (let i in surroundPoint) {
|
|||
|
|
let item: any = surroundPoint[i]; //获得周围的八个点
|
|||
|
|
if (
|
|||
|
|
item.r >= 0 && //判断是否在地图上
|
|||
|
|
item.l >= 0 &&
|
|||
|
|
item.r < rows &&
|
|||
|
|
item.l < cols &&
|
|||
|
|
mapArr[item.r][item.l] != 0 && //判断是否是障碍物
|
|||
|
|
!this.existInCloseList(item, closeObjList, cols) && //判断是否在关闭列表中
|
|||
|
|
mapArr[item.r][currentPoint.l] != 0 && //判断之间是否有障碍物，如果有障碍物是过不去的
|
|||
|
|
mapArr[currentPoint.r][item.l] != 0) {
|
|||
|
|
//g 到父节点的位置
|
|||
|
|
//如果是上下左右位置的则g等于10，斜对角的就是14
|
|||
|
|
var g = currentPoint.G + ((currentPoint.r - item.r) * (currentPoint.l - item.l) == 0 ? 10 : 14);
|
|||
|
|
let temp: number = this.existInOpenList(item, openObjList, cols);
|
|||
|
|
if (temp == null) { //如果不在开启列表中
|
|||
|
|
//计算H，通过水平和垂直距离进行确定
|
|||
|
|
item['H'] = Math.abs(end_r - item.r) * 10 + Math.abs(end_l - item.l) * 10;
|
|||
|
|
item['G'] = g;
|
|||
|
|
item['F'] = item.H + item.G;
|
|||
|
|
item['parent'] = currentPoint;
|
|||
|
|
openList.push(item);
|
|||
|
|
openObjList[item.r * cols + item.l] = openList.length - 1;
|
|||
|
|
if (item['H'] == 0) {
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
this.minheap_filterup(openList, openList.length - 1, openObjList, cols);
|
|||
|
|
} else { //存在在开启列表中，比较目前的g值和之前的g的大小
|
|||
|
|
let index: number = temp;
|
|||
|
|
//如果当前点的g更小
|
|||
|
|
if (g < openList[index].G) {
|
|||
|
|
openList[index].parent = currentPoint;
|
|||
|
|
openList[index].G = g;
|
|||
|
|
openList[index].F = g + openList[index].H;
|
|||
|
|
}
|
|||
|
|
this.minheap_filterup(openList, index, openObjList, cols);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//如果开启列表空了，没有通路，结果为空
|
|||
|
|
if (openList.length == 0) {
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
// openList.sort(this.sortF);//这一步是为了循环回去的时候，找出 F 值最小的, 将它从 "开启列表" 中移掉
|
|||
|
|
} while (!(result_index = this.existInOpenList({
|
|||
|
|
r: end_r,
|
|||
|
|
l: end_l
|
|||
|
|
}, openObjList, cols)));
|
|||
|
|
|
|||
|
|
//判断结果列表是否为空
|
|||
|
|
if (!result_index) {
|
|||
|
|
result = [];
|
|||
|
|
} else {
|
|||
|
|
var currentObj = openList[result_index];
|
|||
|
|
do {
|
|||
|
|
//把路劲节点添加到result当中
|
|||
|
|
result.unshift({
|
|||
|
|
r: currentObj.r,
|
|||
|
|
l: currentObj.l
|
|||
|
|
});
|
|||
|
|
currentObj = currentObj.parent;
|
|||
|
|
} while (currentObj.r != start_r || currentObj.l != start_l);
|
|||
|
|
}
|
|||
|
|
let alpha = 0.8;
|
|||
|
|
let beta = 0.2;
|
|||
|
|
let p = result.slice(0);
|
|||
|
|
for (let i = 1; i <= 8; i++) {
|
|||
|
|
for (let k = 2; k < p.length - 1; k++) {
|
|||
|
|
let t = p[k];
|
|||
|
|
let l = p[k - 1];
|
|||
|
|
let n = p[k + 1];
|
|||
|
|
t.l = t.l + alpha * (result[k].l - t.l) + beta * (l.l - 2 * t.l + n.l);
|
|||
|
|
t.r = t.r + alpha * (result[k].r - t.r) + beta * (l.r - 2 * t.r + n.r);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return p;
|
|||
|
|
// return result;
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* 小顶堆上升算法
|
|||
|
|
* list 小顶堆列表
|
|||
|
|
* pos 起始计算位置，即从改点开始上升
|
|||
|
|
* indexlist 地图节点索引，节点地图坐标对应list中的位置
|
|||
|
|
* cols 地图列数
|
|||
|
|
*/
|
|||
|
|
static minheap_filterup(list: any[], pos: number, indexlist: any, cols: number) {
|
|||
|
|
let c = pos; // 当前节点(current)的位置
|
|||
|
|
let p = Math.floor((c - 1) / 2); // 父(parent)结点的位置
|
|||
|
|
let tmp = list[c]; // 当前节点(current)
|
|||
|
|
while (c > 0) { // c>0 还未上升到顶部
|
|||
|
|
if (list[p].F <= tmp.F) // 父节点比当前节点小，上升结束
|
|||
|
|
break;
|
|||
|
|
else {
|
|||
|
|
list[c] = list[p]; // 父节点放到当前位置
|
|||
|
|
indexlist[list[p].r * cols + list[p].l] = c; //设置父节点的索引位置
|
|||
|
|
c = p; // 当前位置上升到父节点位置
|
|||
|
|
p = Math.floor((p - 1) / 2); // 重新计算父节点位置
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
list[c] = tmp; // 把传入节点放到上升位置
|
|||
|
|
indexlist[tmp.r * cols + tmp.l] = c; // 设置传入点的索引位置
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* 小顶堆下沉算法
|
|||
|
|
* list 小顶堆列表
|
|||
|
|
* pos 起始计算位置，即从改点开始上升
|
|||
|
|
* indexlist 地图节点索引，节点地图坐标对应list中的位置
|
|||
|
|
* cols 地图列数
|
|||
|
|
*/
|
|||
|
|
static minheap_filterdown(list: any[], pos: number, indexlist: any, cols: number) {
|
|||
|
|
let c = pos; // 当前(current)节点的位置
|
|||
|
|
let l = 2 * c + 1; // 左(left)孩子的位置
|
|||
|
|
let tmp = list[c]; // 当前(current)节点
|
|||
|
|
let end = list.length - 1; // 数组终点
|
|||
|
|
while (l <= end) {
|
|||
|
|
if (l < end && list[l].F > list[l + 1].F) // "l"是左孩子，"l+1"是右孩子
|
|||
|
|
l++; // 左右两孩子中选择较小者，即list[l+1]
|
|||
|
|
if (tmp.F <= list[l].F) // 当前节点比最小的子节点小，调整结束
|
|||
|
|
break;
|
|||
|
|
else {
|
|||
|
|
list[c] = list[l];
|
|||
|
|
indexlist[list[l].r * cols + list[l].l] = c;
|
|||
|
|
c = l;
|
|||
|
|
l = 2 * l + 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
list[c] = tmp;
|
|||
|
|
indexlist[tmp.r * cols + tmp.l] = c;
|
|||
|
|
};
|
|||
|
|
// 是否在开放列表中
|
|||
|
|
static existInOpenList(point: any, list: any, cols: number): number {
|
|||
|
|
let result: number = list[point.r * cols + point.l];
|
|||
|
|
return result;
|
|||
|
|
};
|
|||
|
|
//获取周围八个点的值
|
|||
|
|
static SurroundPoint(curPoint: any): any[] {
|
|||
|
|
let r = curPoint.r,
|
|||
|
|
l = curPoint.l;
|
|||
|
|
return [{
|
|||
|
|
r: r - 1,
|
|||
|
|
l: l - 1
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
r: r,
|
|||
|
|
l: l - 1
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
r: r + 1,
|
|||
|
|
l: l - 1
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
r: r + 1,
|
|||
|
|
l: l
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
r: r + 1,
|
|||
|
|
l: l + 1
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
r: r,
|
|||
|
|
l: l + 1
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
r: r - 1,
|
|||
|
|
l: l + 1
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
r: r - 1,
|
|||
|
|
l: l
|
|||
|
|
}
|
|||
|
|
];
|
|||
|
|
};
|
|||
|
|
// 是否在关闭列表中
|
|||
|
|
static existInCloseList(point: any, list: any, cols: number): boolean {
|
|||
|
|
if (list[point.r * cols + point.l]) {
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
return false;
|
|||
|
|
};
|
|||
|
|
}
|