混合式A星代码解析_4 修正优化算法smoothPath.cpp

5. 优化函数Smoother

5.1 tracePath()

• 功能:此函数的功能就是要找到一条从start节点到goal节点的路径path.
• 参数:此处带入的初始节点指针node是已经找到的goal的指针,i是第回溯到第一个节点了,path是回溯的路径.

本函数采用了递归调用的模式,不断去找node的前任节点node->getPred(),不断更新路径path,知道node为空指针nullptr,意味着已经到了start节点.

void Smoother::tracePath(const Node3D* node, int i, std::vector<Node3D> path) {
   
   
  if (node == nullptr) {
   
   
    this->path = path;
    return;
  }

  i++;
  path.push_back(*node);
  tracePath(node->getPred(), i, path);
}

5.2 smoothPath()

• 功能:把tracePath()回溯到的路径path进行优化
• 参数:

void Smoother::smoothPath(DynamicVoronoi& voronoi) {
   
   
  
}

• 步骤:

5.2.1 初始化参数

包括voronoi的尺寸,最大叠代次数和路径长度.

  // load the current voronoi diagram into the smoother
  this->voronoi = voronoi;
  this