C++ 实现JPS(完整代码)

m0_63608714xj

已于 2023-06-27 13:41:17 修改

阅读量536

点赞数 1

文章标签： c++ 数学建模算法

于 2023-06-26 17:30:36 首次发布

本文链接：https://blog.csdn.net/m0_63608714/article/details/131401482

版权

JpsMap.h

#pragma once
#include "MapManager.h"
#include <unordered_map>
#include <unordered_set>
#include <map>
#include <list>
#include <vector>
#include <set>
#include <unistd.h>
#include <algorithm>
#include <cmath>
#include <time.h>
#include <cstdlib>
#include <iostream>
#include <string>
#include <memory>
#include <functional>

class Node
{
    public:
    NodePtr parent;
    int32 x;
    int32 y;
    int32 gCost;
    int32 hCost;
    int32 dir;
    std::set<int32> forceL;

    Node( int32 _x, int32 _y )
    {
        parent = nullptr;
        x      = _x;
        y      = _y;
        gCost  = 0;
        hCost  = 0;
        dir    = -1;
        forceL = std::set<int32>();
    }

    ~Node(){}
};

namespace jps
{
    const int32 DirX[8] = { 1,  1,  0, -1, -1, -1,  0,  1 };
    const int32 DirY[8] = { 0,  1,  1,  1,  0, -1, -1, -1 };

    
    class JpsFind final
    {
        NodePtr startNode_;
        NodePtr endNode_;
        jpsmap::MapMgr map_mgr_;
        bool jpsN_;

        std::list<NodePtr> openL_;
        std::unordered_set<int32> closeL_;
        std::map<int32, NodePtr> nodeL_;

        public:
        void CalcPath();
        void AddToOpenL( const NodePtr& node );
        NodePtr FindByOpenL( const NodePtr& node );
        bool    FindByCloseL( const NodePtr& node );
        int32   GetDistance( int32 s_x, int32 s_y, int32 e_x, int32 e_y );
        std::vector<NodePtr> GetNeighbours( const NodePtr& node );
        std::vector<NodePtr> GetSuccessJumpNodeL( const NodePtr& node );
        void Init();
        void GetPath( int32 s_x, int32 s_y, int32 e_x, int32 e_y );
        NodePtr Jump( const NodePtr& node, int32 dir );
        NodePtr JumpInclined( const NodePtr& node, int32 dir );
        NodePtr JumpStr( const NodePtr& node, int32 dir );
        void FillMap()const;
        bool Walk( int32 x, int32 y )const;
        NodePtr GetNode( int32 x, int32 y );
        std::vector<NodePtr> GetForceNodeL( const NodePtr& node );
        std::vector<NodePtr> GetWholeJumpNodePath();
    };
}

JpsMap.cpp

#include "JpsMap.h"

namespace jps
{
    int32 JpsFind::GetDistance( int32 s_x, int32 s_y, int32 e_x, int32 e_y )
    {
        return std::sqrt(std::abs(e_x-s_x)*std::abs(e_x-s_x) + std::abs(e_y-s_y)*std::abs(e_y-s_y));
    }

    void JpsFind::AddToOpenL( const NodePtr& node )
    {
        if( !Walk(node->x, node->y) )
            return;

        if( FindByCloseL(node) )
            return;

        openL_.push_back(node);

        openL_.sort([](const NodePtr& l, const NodePtr& r)
            {
                return (l->gCost + l->hCost) < (r->gCost + r->hCost);
            });

    }

    NodePtr JpsFind::FindByOpenL( const NodePtr& node )
    {
        auto it = std::find_if(openL_.begin(), openL_.end(), [&node](const NodePtr& l)
            {
                return node->x == l->x && node->y == l->y;
            });
        
        bool is_open = false;

        if( it != openL_.end() )
            return *it;

        return NodePtr();
    }

    bool JpsFind::FindByCloseL( const NodePtr& node )
    {
        auto it = closeL_.find((node->x<<16) | node->y);
        if( it != closeL_.end() )
            return true;

        return false;
    }

    bool JpsFind::Walk( int32 x, int32 y )const
    {
        auto pos = map_mgr_.Find(x, y);
        if( pos && pos->walk )
            return true;

        return  false;
    }

    void JpsFind::FillMap()const
    {
        map_mgr_.FillMap();
    }

    NodePtr JpsFind::GetNode( int32 x, int32 y )
    {
        int32 key = (x<<16) | y;

        auto it = nodeL_.find(key);
        if( it != nodeL_.end() )
            return it->second;

        NodePtr node = NodePtr(new Node(x, y));
        if( !node )
            NodePtr();

        nodeL_[key] = node;

        return node;
    }

    void JpsFind::CalcPath()
    {
        while( !openL_.empty() )
        {