A*算法寻路之Ogre实践

最新推荐文章于 2025-02-28 14:00:12 发布
最新推荐文章于 2025-02-28 14:00:12 发布
阅读量612 收藏
点赞数
分类专栏： OGRE 3D 文章标签： 算法 vector iterator float 工具 path

之前写过一篇Ogre里人物碰撞检测的文章碰撞检测工具类实现。上一篇写了A*算法的控制台程序。那么这篇文章就是2者的结合。这里将使用A*算法对地图里的一个机器人实现指示之后自动寻路的功能。

   基本功能如下：1.利用鼠标点击指示目的地。

   2.机器人收到指令后自己搜寻最优路径。

   3.机器人需要绕开障碍物，并且随着路径转换自己的方向。

  下面是几张效果图。

 

 

 

控制台输出的路径点坐标。

下面是实现：

首先把框架写好。使用BaseApplication快速搭建。我使用的是自己做的地图编辑器生成的地图。你可以随意在Ogre中建立一些障碍物。

需要的工具为  碰撞检测工具:CollisionTools类，自动寻路工具:AStarPathFinder类。

在继承BaseApplicaiton的类中需要的函数有：

view plaincopy to clipboardprint?
void createRobot();   
void robotIdle(float dTime);   
void robotMove(float dTime);   
void rotateBody(Ogre::Vector3 dir);   
void setPath();  
 void createRobot();
 void robotIdle(float dTime);
 void robotMove(float dTime);
 void rotateBody(Ogre::Vector3 dir);
 void setPath();

变量：

view plaincopy to clipboardprint?
Ogre::RaySceneQuery*    mSceneQuery;   
CollisionTools*         mCollisionTools;   
AStarPathFinder*        mPathFinder;   
Ogre::SceneNode*        mRobotNode;   
Ogre::Entity*           mRobotEnt;   
Ogre::Vector3           mNextPosisition;   
Ogre::Vector3           mDirection;   
bool                    mMoveNext;   
float                   mMoveSpeed;   
float                   mDistance;   
Ogre::Vector3           mStart;   
Ogre::Vector3           mDest;   
  
std::deque<Ogre::Vector3> mPathDeque;  
 Ogre::RaySceneQuery* mSceneQuery;
 CollisionTools*   mCollisionTools;
 AStarPathFinder*  mPathFinder;
 Ogre::SceneNode*  mRobotNode;
 Ogre::Entity*   mRobotEnt;
 Ogre::Vector3   mNextPosisition;
 Ogre::Vector3   mDirection;
 bool     mMoveNext;
 float     mMoveSpeed;
 float     mDistance;
 Ogre::Vector3   mStart;
 Ogre::Vector3   mDest;
 
 std::deque<Ogre::Vector3> mPathDeque;

在createScene中创建我们的机器人和碰撞检测工具以及路径搜寻工具，其中把机器人设置为不可碰撞查询，这样射线第一个点不会接触到机器人自己。场景中的其它物体需要设置为碰撞的（包括地面，需要点击目标点作射线查询）

view plaincopy to clipboardprint?
void GameApp::createScene()   
{   
    mCamera->setPosition(193, 268, 222);   
    mCamera->lookAt(0, 0, 0);   
    mSceneMgr->setAmbientLight(ColourValue::Black);   
    mSceneMgr->setShadowTechnique(SHADOWTYPE_TEXTURE_MODULATIVE);   
    MapLoader mapLoader("./map/map_maze.map", ObjectFactory::getSingletonPtr());   
    mapLoader.loadMap();   
    createLight();   
    createGUI();   
    mSceneQuery = mSceneMgr->createRayQuery(Ray());   
    mCollisionTools = new CollisionTools(mSceneQuery);   
    mPathFinder = new AStarPathFinder;   
    createRobot();   
}   
void GameApp::createFrameListener()   
{   
    BaseApplication::createFrameListener();   
}   
void GameApp::createRobot()   
{   
    mRobotEnt = mSceneMgr->createEntity("robotAI", "robot.mesh");   
    mRobotNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("robotAINode");   
    mRobotNode->attachObject(mRobotEnt);   
    mRobotNode->setScale(0.3, 0.3, 0.3);   
    mRobotEnt->setQueryFlags(NONCOLLISION);   
}  
void GameApp::createScene()
{
 mCamera->setPosition(193, 268, 222);
 mCamera->lookAt(0, 0, 0);
 mSceneMgr->setAmbientLight(ColourValue::Black);
 mSceneMgr->setShadowTechnique(SHADOWTYPE_TEXTURE_MODULATIVE);
 MapLoader mapLoader("./map/map_maze.map", ObjectFactory::getSingletonPtr());
 mapLoader.loadMap();
 createLight();
 createGUI();
 mSceneQuery = mSceneMgr->createRayQuery(Ray());
 mCollisionTools = new CollisionTools(mSceneQuery);
 mPathFinder = new AStarPathFinder;
 createRobot();
}
void GameApp::createFrameListener()
{
 BaseApplication::createFrameListener();
}
void GameApp::createRobot()
{
 mRobotEnt = mSceneMgr->createEntity("robotAI", "robot.mesh");
 mRobotNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("robotAINode");
 mRobotNode->attachObject(mRobotEnt);
 mRobotNode->setScale(0.3, 0.3, 0.3);
 mRobotEnt->setQueryFlags(NONCOLLISION);
}

掩码如下：

view plaincopy to clipboardprint?
enum COLLISION_MASK   
{   
    COLLISION=1<<0,   
    NONCOLLISION=1<<1   
};  
 enum COLLISION_MASK
 {
  COLLISION=1<<0,
  NONCOLLISION=1<<1
 };
 

然后在鼠标点击的地方写上如下代码。

view plaincopy to clipboardprint?
bool GameApp::mousePressed( const OIS::MouseEvent &arg, OIS::MouseButtonID id )   
{   
    if(id == OIS::MB_Left)   
    {   
        CEGUI::Point mousePos = CEGUI::MouseCursor::getSingleton().getPosition();   
        Ray mouseRay = mCamera->getCameraToViewportRay(mousePos.d_x / float(arg.state.width),    
            mousePos.d_y / float(arg.state.height));   
        mSceneQuery->setRay(mouseRay);   
        RaySceneQueryResult& result = mSceneQuery->execute();   
        RaySceneQueryResult::iterator itr = result.begin();   
        if(itr != result.end() && itr->movable)   
        {   
            mNextPosisition = mouseRay.getPoint(itr->distance);   
            //坐标化为整数   
            mDest.x = std::floor(mNextPosisition.x + 0.5);   
            mDest.y = 0;   
            mDest.z = std::floor(mNextPosisition.z + 0.5);   
            mStart.x = std::floor(mRobotNode->getPosition().x + 0.5);   
            mStart.y = 0;   
            mStart.z = std::floor(mRobotNode->getPosition().z + 0.5);   
            mPathFinder->clear();   
            mPathFinder->findPath(mStart.x, mStart.z, mDest.x, mDest.z);   
            setPath();   
        }   
        mMoveNext = true;   
    }   
    return true;   
}  
bool GameApp::mousePressed( const OIS::MouseEvent &arg, OIS::MouseButtonID id )
{
 if(id == OIS::MB_Left)
 {
  CEGUI::Point mousePos = CEGUI::MouseCursor::getSingleton().getPosition();
  Ray mouseRay = mCamera->getCameraToViewportRay(mousePos.d_x / float(arg.state.width), 
   mousePos.d_y / float(arg.state.height));
  mSceneQuery->setRay(mouseRay);
  RaySceneQueryResult& result = mSceneQuery->execute();
  RaySceneQueryResult::iterator itr = result.begin();
  if(itr != result.end() && itr->movable)
  {
   mNextPosisition = mouseRay.getPoint(itr->distance);
   //坐标化为整数
   mDest.x = std::floor(mNextPosisition.x + 0.5);
   mDest.y = 0;
   mDest.z = std::floor(mNextPosisition.z + 0.5);
   mStart.x = std::floor(mRobotNode->getPosition().x + 0.5);
   mStart.y = 0;
   mStart.z = std::floor(mRobotNode->getPosition().z + 0.5);
   mPathFinder->clear();
   mPathFinder->findPath(mStart.x, mStart.z, mDest.x, mDest.z);
   setPath();
  }
  mMoveNext = true;
 }
 return true;
}

先是获得鼠标点击后射线查询到与地面的交点位置。这里查询不需要排序，因为需要最后一个查询到的物体-->地面。然后把目标点位置保存到mDest中。这里需要把坐标近似为整数，用了C库的floor函数。近似为整数的目的是为了让路径搜寻便于实施。（浮点运算较慢并且不好判断目标点） 鼠标点击后马上获得机器人的路径，在setPath中完成。

setPath代码如下：

view plaincopy to clipboardprint?
void GameApp::setPath()   
{   
    mPathDeque.clear();   
    std::vector<Point> path = mPathFinder->getPath();   
    if(!path.empty())   
    {   
        for(std::vector<Point>::reverse_iterator itr = path.rbegin(); itr != path.rend(); ++itr)   
        {   
            mPathDeque.push_front(Ogre::Vector3(itr->x, 0, itr->z));   
        }   
        mPathFinder->clear();   
    }   
}  
void GameApp::setPath()
{
 mPathDeque.clear();
 std::vector<Point> path = mPathFinder->getPath();
 if(!path.empty())
 {
  for(std::vector<Point>::reverse_iterator itr = path.rbegin(); itr != path.rend(); ++itr)
  {
   mPathDeque.push_front(Ogre::Vector3(itr->x, 0, itr->z));
  }
  mPathFinder->clear();
 }
}

上面的代码主要是把保存路径的vector的数据转移到一个deque结构中。你可以在路径搜寻工具中直接把路径保存到deque中。每次设定完路径都需要清理，以便下一次搜寻路径使用。  clear主要是把OPEN,CLOSED和PATH清空。

机器人的移动主要在robotMove中完成。代码如下：

view plaincopy to clipboardprint?
void GameApp::robotMove(float dTime)   
{   
    Ogre::AnimationState* anim = mRobotEnt->getAnimationState("Walk");   
    anim->setLoop(true);   
    anim->setEnabled(true);   
    anim->addTime(dTime);   
       
    if(!mPathDeque.empty())   
    {   
        std::cout<<mPathDeque.back().x<<","<<mPathDeque.back().y<<","<<mPathDeque.back().z<<std::endl;   
        Ogre::Vector3 dir = mPathDeque.back() - mRobotNode->getPosition();   
        rotateBody(dir);   
        mRobotNode->translate(dir * dTime * mMoveSpeed);   
        if((dir.normalise() < 0.1f))   
        {   
            mRobotNode->setPosition(mPathDeque.back());   
            mPathDeque.pop_back();   
        }   
    }   
    else  
    {   
        mMoveNext = false;   
    }   
}   
void GameApp::rotateBody(Ogre::Vector3 dir)   
{   
    Ogre::Vector3 src = mRobotNode->getOrientation() * Ogre::Vector3::UNIT_X;   
    if(1.0f + src.dotProduct(mDirection) < 0.0001f)   
    {   
        mRobotNode->yaw(Ogre::Degree(180));   
    }   
    else  
    {   
        Ogre::Quaternion qua = src.getRotationTo(dir);   
        mRobotNode->rotate(qua);   
    }   
}  
void GameApp::robotMove(float dTime)
{
 Ogre::AnimationState* anim = mRobotEnt->getAnimationState("Walk");
 anim->setLoop(true);
 anim->setEnabled(true);
 anim->addTime(dTime);
 
 if(!mPathDeque.empty())
 {
  std::cout<<mPathDeque.back().x<<","<<mPathDeque.back().y<<","<<mPathDeque.back().z<<std::endl;
  Ogre::Vector3 dir = mPathDeque.back() - mRobotNode->getPosition();
  rotateBody(dir);
  mRobotNode->translate(dir * dTime * mMoveSpeed);
  if((dir.normalise() < 0.1f))
  {
   mRobotNode->setPosition(mPathDeque.back());
   mPathDeque.pop_back();
  }
 }
 else
 {
  mMoveNext = false;
 }
}
void GameApp::rotateBody(Ogre::Vector3 dir)
{
 Ogre::Vector3 src = mRobotNode->getOrientation() * Ogre::Vector3::UNIT_X;
 if(1.0f + src.dotProduct(mDirection) < 0.0001f)
 {
  mRobotNode->yaw(Ogre::Degree(180));
 }
 else
 {
  Ogre::Quaternion qua = src.getRotationTo(dir);
  mRobotNode->rotate(qua);
 }
}

上面代码中先判断路径队列是否为空，如果不为空的话，就让机器人按照队列的每个点一次行走。没走完一个点把这个点从队列中移除。通过一个距离判断的值来近似机器人是否到达目标点。

然后在路径搜寻工具里添加碰撞检测的判断：

view plaincopy to clipboardprint?
void AStarPathFinder::updateNode(PathNode* bestNode, int sx, int sz, int dx, int dz)   
{   
    PathNode* child = new PathNode;   
    child->h = (dx - sx) * (dx - sx) + (dz - sz) * (dz - sz);   
    child->g = bestNode->g + TILESIZE;   
    child->f = child->g + child->h;   
    child->point.x = sx; child->point.z = sz;   
    //跳过CLOSED表中的节点和不可通过的节点   
    //   
    Ogre::Vector3 dir = Ogre::Vector3(sx, 0, sz) - Ogre::Vector3(bestNode->point.x, 0, bestNode->point.z);   
    if(findNodeInClosed(child) ||   
        GameApp::getSingleton().getCollisionTools()->collisionWithMovable(Ogre::Vector3(bestNode->point.x, 5, bestNode->point.z), dir))   
        return;   
    //   
    else if(!findNodeInClosed(child) && !findNodeInOpen(child))   
    {   
        //如果不在OPEN和CLOSED表中，把这个节点的父节点设为当前节点   
        child->parent = bestNode;   
        //并且把这个节点放入OPEN表   
        OPEN.insert(child);   
    }   
    else if(findNodeInOpen(child))   
    {   
        //如果在OPEN表中，比较当前节点的G值+TILESIZE和此节点原来的G值，如果新G值小那么更新   
        //G值，并且把这个节点的父节点设为当前节点   
        if((bestNode->g + TILESIZE) < child->g)   
        {   
            child->g = bestNode->g + TILESIZE;   
            child->parent = bestNode;   
        }   
    }   
}  
void AStarPathFinder::updateNode(PathNode* bestNode, int sx, int sz, int dx, int dz)
{
 PathNode* child = new PathNode;
 child->h = (dx - sx) * (dx - sx) + (dz - sz) * (dz - sz);
 child->g = bestNode->g + TILESIZE;
 child->f = child->g + child->h;
 child->point.x = sx; child->point.z = sz;
 //跳过CLOSED表中的节点和不可通过的节点
 //
 Ogre::Vector3 dir = Ogre::Vector3(sx, 0, sz) - Ogre::Vector3(bestNode->point.x, 0, bestNode->point.z);
 if(findNodeInClosed(child) ||
  GameApp::getSingleton().getCollisionTools()->collisionWithMovable(Ogre::Vector3(bestNode->point.x, 5, bestNode->point.z), dir))
  return;
 //
 else if(!findNodeInClosed(child) && !findNodeInOpen(child))
 {
  //如果不在OPEN和CLOSED表中，把这个节点的父节点设为当前节点
  child->parent = bestNode;
  //并且把这个节点放入OPEN表
  OPEN.insert(child);
 }
 else if(findNodeInOpen(child))
 {
  //如果在OPEN表中，比较当前节点的G值+TILESIZE和此节点原来的G值，如果新G值小那么更新
  //G值，并且把这个节点的父节点设为当前节点
  if((bestNode->g + TILESIZE) < child->g)
  {
   child->g = bestNode->g + TILESIZE;
   child->parent = bestNode;
  }
 }
}

  这样就OK了。 一个搜寻最优路径的演示就做完了。同样在上篇文章中提到的优化问题本文并没有解决。比如对角线的G值其实要比直线邻居的G值大些，无法在Y轴上搜寻等。

  希望本文对你有帮助。

详解混合A*算法Hybrid A*
zgw555555的专栏
05-26 2686
混合A算法（Hybrid A）是一种改进的路径规划算法，特别针对车辆动力学进行了优化。
Unity 简单的可视化A*算法寻路网格布置以及调试工具的实现（二）寻路网格自动生成
qq_44725964的博客
05-22 1098
Unity 简单的可视化A*算法寻路网格布置以及调试工具的实现（二）寻路网格自动生成 概述 本文记录我的一个A*算法的寻路网格以及可视化测试工具的的实现思路。我的前一篇博客实现了基本功能：Unity 简单的可视化A*算法寻路网格布置以及调试工具的实现（一） 但是在实际操作中需要布置的寻路网格可能会比较大比较复杂，如果每个结点都手动设置的话会很麻烦，于是我又增加了根据地形自动生成寻路网格的功能。 一、准备工作 在将上一篇博客中场景中做好的NavigationBlock物体拖入到Assets/Resources
混合A*算法(Hybrid A*)
热门推荐
qq_31815513的博客
06-21 3万+
1、混合A*算法(Hybrid A*)简介 本课题为了完成在已知环境下的侧方位停车和倒车入库的任务规划，采用斯坦福大学所提出来的Hybrid A*算法，并基于开源机器人操作系统其进行实现。其中，在2010年斯坦福的Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo等人在国际顶级期刊International Journal of Robotics Reaserch上首次提出混合A*算法(Hybrid A*算法)用于解决在侧方位停车和倒车入库过程中的路径规划
算法学习——Hybrid A* 算法
最新发布
小墨迹的博客
02-28 1035
Hybrid A* 算法通过结合 A* 的高效搜索能力和车辆运动学约束，适用于具有运动学约束的路径规划问题。通过合理设置分辨率参数、启发式函数权重和运动学参数，可以优化路径规划的效率和精度。
OGRE手册与A*寻路算法原理详解
5. **A*算法的变种**： - 为了适应不同场景的需求，A*算法有很多变种，如LPA*（Linear Predictive A*）、D*系列算法等。 - 这些算法在处理动态环境（如障碍物移动）或在求解多目标路径问题时各有其特点。 ### A*...
Ogre游戏程序 源码
10-19
通过深入阅读和分析`AITest`的源代码，我们可以学习到如何利用Ogre实现游戏AI，包括寻路算法、智能体行为树、状态机等。同时，也可以从中学习到如何在Ogre框架下实现游戏逻辑、交互以及优化性能。 总结来说，Ogre...
RecastNavigation寻路技术在VS2008环境中的应用示例
"寻路ogre" 标签暗示该寻路库与Ogre 3D图形引擎有关。Ogre（Object-Oriented Graphics Rendering Engine）是一个面向对象、灵活的3D引擎。它用于创建独立于硬件的实时3D应用程序。Ogre的灵活和直观的设计允许开发者...
Ogre框架开发指南
06-04
AI方面，可以使用行为树或简单的寻路算法（如A*），让角色在游戏中做出智能决策。 第四个项目可能关注网络同步和多人协作，这对于多人在线游戏至关重要。Ogre本身并不直接处理网络编程，但可以与开源的网络库如ENet...
ogre 路径系统 Argorha_Pathfinding_04_src
12-07
A*（A-star）算法是最常用的路径搜索算法之一，因为它既高效又具有最优性。在"Argorha_Pathfinding_04_src"中，我们可能会看到A*算法的实现，它通过计算每个节点的代价和预计代价，找到从起点到终点的最短路径。 在...
混合A星算法-Hybrid_Astar（matlab）
12-02
以车辆的运动学模型为节点，以当前点到终点的Astar距离和RS距离两者最大的距离作为H(n)函数的估计代价，使用matlab实现（2016a以上版本）
基于Hybrid A*的路径规划(Matlab实现)
03-06
这个代码资源提供了一个实现了hybrid A*算法的程序框架，它可以帮助你快速构建自己的路径规划系统。该代码资源的核心部分包括以下几个方面： 网格地图表示：该代码资源提供了一种有效的方式来表示环境地图，以便进行路径搜索。它可以将地图划分为网格，并记录每个网格的状态信息，如是否可通过、代价等。 连续和离散搜索：hybrid A*算法结合了连续和离散搜索，以在连续空间中进行快速搜索，并在离散空间中进行精确路径规划。该代码资源实现了这两种搜索方法，并提供了相应的接口供你使用。 启发式函数：为了加速路径搜索过程，该代码资源还提供了一些启发式函数，用于评估节点的优先级。这些启发式函数可以帮助算法更好地选择下一个扩展节点，以便更快地找到最优路径。 通过使用这个基于hybrid A*的路径规划代码资源，你可以快速构建一个高效的路径规划系统，以应对各种复杂环境。它提供了一个可靠的算法框架，使你能够专注于解决具体的路径规划问题。
matlaba算法代码-hybrid_A_star:本仓库是对混合A*算法的matlab复现
05-19
matlab a算法代码 本仓库是对混合A*算法的matlab复现 算法结果图 如何使用 1.在matlab中直接运行EntryPoint.m文件即可，坐标的航向phi取值范围是[-pi,pi] 注意： 1.本仓库提供两个代码版本，其中without_comments分支是原作者的版本，并且修复了从左边直线行驶轨迹错误的bug(原因是VehicleCollisionCheck出错)。 2.master分支是本人根据源代码进行修改，添加注释方便理解，同时也修改了VechicleCollisionCheck的代码。 文件组织 代码解释可以参考！
Hybrid A* （混合A*）路径规划算法
qq_29320587的博客
06-21 4266
HybridAStar算法主要是在A* 基础上考虑车辆动力学相关的搜索算法，其放弃了A* 中对搜索空间离散化，利用Dubins或者Reeps-Shepp来考虑自车能够前进的位置，使得规划的轨迹满足车辆的非完整性约束，但是该Hybrid A* 牺牲了算法的完备性和最优性。1.在连续空间上扩张节点搜索最短路径2.路径平滑Hybrid A* 算法与A* 算法的差异对比如下表所示如下图所示，最左侧图是A算法的结果，最右侧是Hybrid A算法。（中间为A* 算法变种）
[规划算法]Hybrid A *算法原理
Forwarding3的博客
07-15 2984
hybrid A*算法原理
Hybrid Astar 算法剖析和实现（五）
穿越临界点的博客
05-21 1256
本篇主要介绍了节点拓展的核心思想，并给大家揭开了**Hybrid**这个字的神秘面纱。下一篇，我们对其进行代码实现，敬请期待。
Baidu Apollo代码解析之Open Space Planner中的Hybrid A*
silver bullet
12-10 8064
大家好，我已经把优快云上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/c_1132958996826546176）。以后，我会把日常的思考放在优快云上，梳理过的精华文章放在知乎上，希望大家可以多多交流，互相学习。 目录 1. 节点的定义 2.Hybrid A*的规划主流程 3. 碰撞检测 4. ReedS...
在Ogre中实现先进的寻路算法：导航网格寻路
LuoyinanRoy的专栏
07-08 6455
相比传统的路点寻路，包括走格子寻路，导航网格寻路算是很先进的寻路算法，总是寻找最短路径，和最少的节点。 我也在引擎中做了一个， 1）先划分障碍区域 2）合并障碍区域 3）计算出导航网格，可以是三角形或者多边形，我采用三角形 4）采用A×寻三角形 5）寻出的路经修正 以上效果在地编中测试，绿色为合并后的障碍区域，粉色为计算出的导航网格，都是三角形。 白
天龙八部技术分析报告
痞子龙3D编程
05-07 6694
刘洋通过一段时间对天龙八部这款游戏进行分析研究，主要从客户端入手，从技术角度学习天龙八部游戏中各种实现机制。最终写成此文，献给搜狐游戏，以表敬意。本文主要分为三个部分，分别是1 天龙八部基本技术实现技术分析，2 天龙八部中使用的资源文件分析3 天龙八部中出现的问题及改进意见。本报告实属本人分析总结，其中必定会有很多错漏之处，望给予指正。一：天龙八部的基本实现技术：所用图形引擎： OgreOGRE（
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值