A*算法寻路之Ogre实践

之前写过一篇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轴上搜寻等。

  希望本文对你有帮助。

 

本文来自CSDN博客，转载请标明出处：http://blog.csdn.net/pizzazhang/archive/2011/03/20/6262265.aspx