Lpa* 源码 原理

Lpa* 路径（c++源码）

本代码和wiki 提供的伪代码基本一模一样，你可以看着wiki的代码来读我的代码。

思路

rhs(n) : rhs(s’)是节点 s’ 到起始节点s的代价,保存父节距离自己的距离，有人就好奇不计算g先，公式里面的g怎么来的，因为起点默认rhs是0，所以起点g也是0，然后其余的点就是INF。然后依次保存rhs值g值就来了。当你是别人的子节点的时候你的rhs就会被更新。但是你是父节点的时候，就会被用于判断。当这个节点第一次被经过的时候g都是INF。

g(n) : 当自己是父节点的时候保存rhs，在此之前先要和自己的rhs值比较，用于判断是不是最优的点。在起始节点的时候是：INF>0，rhs = 0，因为自己距离自己是0。事实上所有的g(n)是不用计算的，只是保存rhs，然后比较用的，我们只需要计算rhs就好了。

起点rhs是0，其余INF，然后所有节点g是INF.

g(n)>rhs(n) 搜索到一条更短的“捷径”
g(n)<rhs(n) 需要重新搜索计算最短路径

h ’s 对于终点的距离，通过曼哈顿或者欧拉就可以算出来

通过key1 和 key2 找出优先计算的路径块。
所以每个节点会被保存在priority_queue里面，key1和key2目的就是在priority_queue里面被用于当做排序的条件。

#include "Lpstar.h"

int direct[8][2]= {
   
        {
   -1,-1},//左上角
        {
   -1, 0},//正上 1
        {
   -1, 1},//右上角
        {
   0, -1},//左边 3
        {
   0, 1},//右边 4
        {
   1,-1},//左下角
        {
   1,0},//正下方 6
        {
   1,1}//右下角
};

std::vector<std::vector<int>> maze ={
   
    {
   0,0,0,0},
    {
   1,0,1,0},
    {
   1,0,1,0},
    {
   1,0,1,0},
    {
   1,0,1,0},
    {
   0,0,0,0},
};


int main()
{
   
    //画地图
    Lpstar lps(maze);
    int s[]={
   0,3};
    int g[]={
   5,0};
    lps.setStartAndGoal(s,g);
    //初始化
    lps.initilaize();
    //最短路径
    lps.computeShortestPath();
    //打印计算好的路径
    lps.print();
    
    //更改路的属性，原本是路，现在设置不可通行
    lps.setBlock(3,1,1);
    lps.setBlock(3,3,1);
    //查找地图上的路是否发生变化
    lps.replainning(maze);

    cout<<endl;
    try {
   
        lps.computeShortestPath();
    } catch (exception ex) {
   
        cout<<"机器人找不到出口，因为路被堵死了"<<endl;
    }

    lps.print();
    return 0;
}

//
//  globalVariable.h
//  infrastructure
//
//  Created by User on 2020/8/15.
//  Copyright © 2020 User. All rights reserved.
//

#ifndef globalVariable_h
#define globalVariable_h
#include <stdio.h>
#include<iomanip>
#include <vector>
#include <list>
#include<iostream>
#include <cmath>
#include <queue>
#define DIRECTIONS 8
#define INFINITE 99999

using namespace std;
extern int direct[8][2];
#endif /* globalVariable_h */

//
//  Lpstar.h
//  infrastructure
//
//  Created by User on 2020/7/21.
//  Copyright © 2020 User. All rights reserved.
//

#ifndef Lpstar_h
#define Lpstar_h

#include "globalVariable.h"


const int kCost1=1; //直移一格消耗
const int kCost2=1; //斜移一格消耗

struct Vertex
{
   
    
    int linkCost[8];
    
    int h,g,rhs;
    int key1,key2;
    int x,y;
    bool road;
//    int linkcost;
    Vertex(int _x,int _y):x(_x),y(_y)//变量初始化
    {
   
        
    }
    Vertex(int _x,