3d地图的a*寻路_3d版本的a星寻路

对于2d地图的a星算法其实很多。原理都是一样的，我做这个的3d的寻路其实只是在2d地图数据中加入了一个高度的数据，而在权重中由原来的两个坐标的勾股定理变为三维坐标系的勾股定理，以前我做cocos2dx开发时曾写过一个lua版的a星，就用这个作为说明吧（涉及a星的原理就不必再说明了，很多博客都会有）：

其中的mapInfo可以更改为如下

self.MapInfo = 
    {
            [8] = {{"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}, {"obstacle" = 0, "height" = 255}},

...

地图的点也需要记录一个高度值

local xSub = math.abs(x - endx)
function MapPoint:init(x, y, h)
self.parent = nil;     --父节点，最后回溯使用
self.x = x;            --x坐标
self.y = y;            --y坐标

self.h = h;  --高度
    self.cost = 0;         --寻路到此点累计的消耗
    self.dist = 0;         --该点距离目的地直线距离(大约)
    self.power = 0;        --该点的累计消耗以及直线距离总和
end

再来看看具体寻路的代码：

--[[
 * @authors: liangjian
 * @date:    2014-09-01 21:30:09
 * @desc: a*算法
--]]


module( "Astar", package.seeall )


function new()
    local obj = {}
    setmetatable(obj, {__index = Astar})
    obj:init()
    return obj
end


function Astar:init()
    self.openList = {};     --打开，表示可以进行寻路的节点列表
    self.closeList = {};    --关闭，表示节点已经搜索过
    self.mapData = {};      --地图数据
    self.width = 0;         --地图的宽
    self.height = 0;        --地图的高
    self.startX = 0;        --开始点x
    self.startY = 0;        --开始点y
    self.endX = 0;          --终点x
    self.endY = 0;          --终点y
    self.path = {};         --保存路径
end
--设置地图数据
function Astar:setMapData(mapData)
    if mapData ~= nil and #mapData ~= 0 then
        self.mapData = mapData;
        self.width = #self.mapData[1];
        self.height = #self.mapData;
    end
end
--搜索，寻路前准备
function Astar:search(startX, startY, endX, endY)
    self.startX = startX;
    self.startY = startY;
    self.endX = endX;
    self.endY = endY;
    
    --判断是否超出地图外
    if (self.startX<0 or self.startX>=self.width) or (self.startY<0 or self.startY>=self.height) then
        print("出发点不在地图范围内！");
        return;
    elseif (self.endX<0 or self.endX>=self.width) or (self.endY<0 or self.endY>=self.height) then
        print("目的点不在地图范围内！");
        return;
    end
     
    --判断目的地以及出发地是否符合,1为障碍物，0通行
    if self.mapData[startY + 1][startX + 1] == 1 then
        print("出发地为障碍点！");
        return;
    end
    if self.mapData[endY + 1][endX + 1] == 1 then
        print("目的地为障碍点！");
        return;
    end
    
    --判断出发地与目的地是否相同
    if self.startX == self.endX and self.startY == self.endY then
        print("目的地与出发地相同！");
        return;
    end
    
    --根节点，第一个放进开放列表中
    local root = MapPoint.new(startX, startY);
    table.insert(self.openList, root);
    
    
    --开始寻路
    self:start(endX, endY);
    if self.path==nil or #self.path==0 then
        print("没有找到路径！");
    else
        print("输出路径:\n");
        for i = 1, #self.path do
            print("("..self.path[i].x..","..self.path[i].y..")\n");
        end
    end
end


--开始寻路，是个递归函数
function Astar:start(endX, endY)
    if self.openList == nil or #self.openList == 0 then return 0; end
    
    local least = self.openList[1];
    if least.x == endX and least.y == endY then
        self:recallGetPath(least)
        print("找到路径了！");
        return self.path;  
    end
    
    --八个方向检查
    self:check(least.x-1, least.y,   10, least, endX, endY) --左
    self:check(least.x+1, least.y,   10, least, endX, endY) --右
    self:check(least.x,   least.y+1, 10, least, endX, endY) --上
    self:check(least.x,   least.y-1, 10, least, endX, endY) --下
    self:check(least.x-1, least.y+1, 14, least, endX, endY) --左上
    self:check(least.x+1, least.y+1, 14, least, endX, endY) --右上
    self:check(least.x-1, least.y-1, 14, least, endX, endY) --左下
    self:check(least.x+1, least.y-1, 14, least, endX, endY) --右下
    
    --将当前节点放进关闭节点中
    table.insert(self.closeList, least)
    --将此节点从打开列表移除
    table.remove(self.openList, 1)
    self.openList = self:sortByPower(self.openList)
    
    self:start(endX, endY)
end


--检查是否通路
function Astar:check(x, y, cost, parent, endX, endY)
if (x<0 or x>=self.width) or (y<0 or y>=self.height) then
  print("该点处于地图外！");
  return;
end

if self:isContain(x, y, self.closeList) ~= -1 then
        print("该点已经访问过！");
        return;
end

    if self.mapData[y + 1][x + 1] == 1 then
        print("该点为障碍物！");
        return;
    end
    
    --累计到此点的消耗
    local node = MapPoint.new(x, y);
    node.parent = parent
    node.cost = parent.cost + cost
    
    --如果访问到开放列表中的点，则将此点重置为消耗最小的路径,否则添加到开放列表
    local index = self:isContain(x, y, self.openList)
    if index ~= -1 then
        if node.cost<self.openList[index].cost then
            node.dist = self.openList[index].dist
            node.power = node.dist + node.cost
            table.remove(self.openList, index)
            table.insert(self.openList, index, node)
    end
    else
        node.dist = self:getDist(x, y, endX, endY)
        node.power = node.dist + node.cost
        table.insert(self.openList, node)
    end
end


--计算点到目的地距离
function Astar:getDist(x, y, endx, endy)
    local xSub = math.abs(x - endx)
    local ySub = math.abs(y - endy)
    
    return math.sqrt(xSub*xSub + ySub*ySub)
end


--开启列表按照消耗排序
function Astar:sortByPower(list)
local sortList = {};
local size = #list
    for i = 1, size do
        local minIndex = 1
        for j = 2, #list do
            if list[minIndex].power > list[j].power then
                minIndex = j;
            end
        end
        table.insert(sortList, list[minIndex]);
        table.remove(list, minIndex);
end
    return sortList;
end


--判断点是否包含，返回索引值
function Astar:isContain(x, y, list)
for i = 1, #list do
if x == list[i].x and y == list[i].y then
 return i;
end
end

return -1;
end


--回溯找到路径
function Astar:recallGetPath(node)
    table.insert(self.path, 1, {x = node.x, y = node.y})
    if node.parent ~= nil then
        self:recallGetPath(node.parent)
    end
end


--没有找到路径
function Astar:notFoundPath()
self.path = {}
end

需要修改getDist(x, y, endx, endy)函数，增加一个高度差值

function(x, y, h,  endx, endy, endz)

local xSub = math.abs(x - endx)
    local ySub = math.abs(y - endy)

local zSub = math.abs(z - endz)

return 勾股定理

end