PathFinding.compute

// Each #kernel tells which function to compile; you can have many kernels
#pragma kernel CSMain

// Create a RenderTexture with enableRandomWrite flag and set it
// with cs.SetTexture
RWTexture2D<float4> Result;
#define _TOTAL_NODE_NUM 100
#define _SEEKER_NUM 10
#define _NODENUM_ _TOTAL_NODE_NUM * _SEEKER_NUM

uint GLB_gridSizeX, GLB_gridSizeY;
uint GLB_gridWorldSizeX, GLB_gridWorldSizeY;

struct NODE_NUM_TARGET
{
    int currentNodeNum;
    int targetNodeNum;
    uint __padding1;
    uint __padding2;
};

struct NODE
{
    int nodeNum;
    int parentNodeNum;
    uint walkable;
    uint gCost;
    uint hCost;
    uint seekerNum;
    uint __padding1;
    uint __padding2;
};

struct VEC2_INT
{
    int x;
    int y;
};

struct PATH
{
    int seekerNo;
    int nodeNum;
};

RWStructuredBuffer<NODE_NUM_TARGET> node_curDest_Buffer;
RWStructuredBuffer<NODE> agentsInfo_Buffer;
RWStructuredBuffer<PATH> pathData;

int InitNodeFull(inout NODE node)
{
    node.nodeNum = -99;
    node.parentNodeNum = -99;
    node.walkable = -99;
    node.gCost = 0;
    node.hCost = 0;
    node.seekerNum = -99;

    return 0;
}

int InitPath(int seekerNo)
{
    [allow_uav_condition]
    for (int i = 0; i < _NODENUM_; i++)//??
    {
        if (pathData[i].seekerNo == seekerNo)
        {
            pathData[i].nodeNum = -99;
        }
    }

    return 0;
}

int InitParent(inout NODE node)
{
    node.parentNodeNum = -99;
    return 0;
}

int ResetParentInfoForThisSeeker(uint seekerNum)
{
    [allow_uav_condition]
    for (uint i = 0; i < _NODENUM_; i++)
    {
        if (agentsInfo_Buffer[i].seekerNum == seekerNum)
        {
            InitParent(agentsInfo_Buffer[i]);
        }
    }
    return 0;
}

int GetNodeFromID(int nodeNum, uint seekerNo, inout NODE node)
{
    [allow_uav_condition]
    for (uint i = 0; i < _NODENUM_; i++)
    {
        if (agentsInfo_Buffer[i].seekerNum == seekerNo && agentsInfo_Buffer[i].nodeNum == nodeNum)
        {
            node = agentsInfo_Buffer[i];
        }
    }
    return 0;
}

int CountOnSet(NODE set[_TOTAL_NODE_NUM])
{
    int countNum = 0;

    [allow_uav_condition]
    for (int i = 0; i < _TOTAL_NODE_NUM; i++)
    {
        if (set[i].nodeNum < 0 || set[i].seekerNum < 0)
        {
            //continue;
        }
        else
        {
            countNum++;
        }
    }
    return countNum;
}

int GetFirstElement(NODE set[_TOTAL_NODE_NUM], inout NODE node)
{
    [allow_uav_condition]
    for (int i = 0; i < _TOTAL_NODE_NUM; i++)
    {
        if (set[i].nodeNum >= 0 && set[i].seekerNum >= 0)
        {
            node = set[i];
            break;
        }
    }

    return 0;
}

bool ContainsOnSet(NODE set[_TOTAL_NODE_NUM], NODE node)
{
    bool isContains = false;
    [allow_uav_condition]
    for (int i = 0; i < _TOTAL_NODE_NUM; i++)
    {
        if (set[i].nodeNum == node.nodeNum)
        {
            isContains = true;
        }
    }

    return isContains;
}

int PopulateSetSeekerNum(inout NODE set[_TOTAL_NODE_NUM], uint seekerNum)
{
    [allow_uav_condition]
    for (int i = 0; i < _TOTAL_NODE_NUM; i++)
    {
        set[i].seekerNum = seekerNum;
    }

    return 0;
}

int GetLowestCostNode(NODE openSet[_TOTAL_NODE_NUM], NODE closedSet[_TOTAL_NODE_NUM], inout NODE node)
{
    [allow_uav_condition]
    for (int i = 0; i < _TOTAL_NODE_NUM; i++)
    {
        if (openSet[i].nodeNum >= 0 && openSet[i].seekerNum >= 0)
        {
            if (ContainsOnSet(closedSet, openSet[i]) == true)
            {
                continue;
            }
            else
            {
                if ((openSet[i].gCost + openSet[i].hCost) <= (node.gCost + node.hCost) && openSet[i].hCost < node.hCost)
                {
                    node = openSet[i];
                }
            }
        }
    }

    return 0;
}

int RemoveFromSet(inout NODE set[_TOTAL_NODE_NUM], NODE data)
{
    [allow_uav_condition]
    for (uint i = 0; i < _TOTAL_NODE_NUM; i++)
    {
        if (set[i].nodeNum == data.nodeNum)
        {
            InitNodeFull(set[i]);
        }
    }

    return 0;
}

int AddToSet(inout NODE set[_TOTAL_NODE_NUM], NODE data)
{
    bool added = false;
    [allow_uav_condition]
    for (uint i = 0; i < _TOTAL_NODE_NUM; i++)
    {
        if (added == false)
        {
            if (set[i].nodeNum < 0 || set[i].seekerNum < 0)
            {
                set[i] = data;
                added = true;
            }
        }
    }

    return 0;
}

VEC2_INT ToVEC2_INT(int nodeNumFlat1D)
{
    int x = 0;
    int y = 0;
    uint nIJ = (uint)nodeNumFlat1D;

    uint cNum = nIJ + 1;
    uint cSizex = GLB_gridSizeX + 1;
    if (cNum % cSizex == 0)
    {
        y = cNum / cSizex;
    }
    else
    {
        y = (cNum / cSizex) + 1;
    }
    y = y - 1;

    uint rCount = cSizex * y;
    x = (int)(nIJ - rCount);

    VEC2_INT id;
    id.x = x;
    id.y = y;

    return id;
}

uint To1D_NODENUM(VEC2_INT id)
{
    return id.x + (id.y * (GLB_gridSizeX + 1));
}

int GetDistance(NODE nodeA, NODE nodeB)
{
    VEC2_INT nodeA_2D = ToVEC2_INT(nodeA.nodeNum);
    VEC2_INT nodeB_2D = ToVEC2_INT(nodeB.nodeNum);

    int dstX = abs(nodeA_2D.x - nodeB_2D.x);
    int dstY = abs(nodeA_2D.y - nodeB_2D.y);

    if (dstX > dstY)
    {
        return 14 * dstY + 10 * (dstX - dstY);
    }
    else
    {
        return 14 * dstX + 10 * (dstY - dstX);
    }
}

bool IsEqual(NODE a, NODE b)
{
    if (a.nodeNum == b.nodeNum && a.seekerNum == b.seekerNum)
    {
        return true;
    }
    else
    {
        return false;
    }
}



int UpdateCostData(NODE currentNode, NODE targetNode, NODE neighborNode, uint seekerNo, uint new_g_Cost)
{
    [allow_uav_condition]
    for (int i = 0; i < _NODENUM_; i++)
    {
        if (agentsInfo_Buffer[i].seekerNum == seekerNo)
        {
            if (neighborNode.nodeNum == agentsInfo_Buffer[i].nodeNum)
            {
                agentsInfo_Buffer[i].gCost = new_g_Cost;
                agentsInfo_Buffer[i].hCost = GetDistance(neighborNode, targetNode);
                agentsInfo_Buffer[i].parentNodeNum = currentNode.nodeNum;
                break;
            }
        }

    }

    return 0;
}

int UpdateNeighbours(inout NODE neighbors[8], NODE curNode, uint seekerNo)
{
    [allow_uav_condition]
    for (uint i = 0; i < 8; i++)
    {
        InitNodeFull(neighbors[i]);
    }

    [allow_uav_condition]
    for (int x = -1; x <= 1; x++)
    {
        [allow_uav_condition]
        for (int y = -1; y <= 1; y++)
        {
            if (x == 0 && y == 0)
                continue;

            VEC2_INT node2D = ToVEC2_INT(curNode.nodeNum);

            int checkX = node2D.x + x;
            int checkY = node2D.y + y;

            int gridSizeX_CNV = (int)GLB_gridSizeX;
            int gridSizeY_CNV = (int)GLB_gridSizeY;

            if (checkX >= 0 && checkX < gridSizeX_CNV && checkY >= 0 && checkY < gridSizeY_CNV)
            {
                VEC2_INT v;
                v.x = checkX;
                v.y = checkY;
                int newNodeNum = To1D_NODENUM(v);
                NODE thisNeighbor;
                GetNodeFromID(newNodeNum, seekerNo, thisNeighbor);
                //AddToNeighbor(thisNeighbor, seekerNo);

                bool added = false;
                [allow_uav_condition]
                for (uint i = 0; i < 8; i++)
                {
                    if (added == false)
                    {
                        if (neighbors[i].nodeNum < 0)
                        {
                            neighbors[i] = thisNeighbor;
                            //break;
                            added = true;
                        }
                    }
                }
            }
        }
    }

    return 0;
}

int UpdatePathData(int nodeNumList[_TOTAL_NODE_NUM], int seekerNo)
{
    [allow_uav_condition]
    for (int i = 0; i < _TOTAL_NODE_NUM; i++)
    {
        bool added = false;
        [allow_uav_condition]
        for (int j = 0; j < _NODENUM_; j++)
        {
            if (pathData[j].seekerNo == seekerNo)
            {
                if (added == false)
                {
                    if (pathData[j].nodeNum < 0)
                    {
                        pathData[j].nodeNum = nodeNumList[i];
                        added = true;
                    }
                }
            }
        }
    }

    return 0;
}

int RetracePath(NODE startNode, NODE endNode, int seekerNo)
{
    NODE currentNode = endNode;

    int nodeNumList[_TOTAL_NODE_NUM];
    [allow_uav_condition]
    for (int k = 0; k < _TOTAL_NODE_NUM; k++)
    {
        nodeNumList[k] = -99;
    }

    InitPath(seekerNo);

    [allow_uav_condition]
    while (IsEqual(currentNode, startNode) != true)
    {
        //AddToPath(currentNode.nodeNum, seekerNo);
        [allow_uav_condition]
        for (int l = 0; l < _TOTAL_NODE_NUM; l++)
        {
            if (nodeNumList[l] < 0)
            {
                nodeNumList[l] = currentNode.nodeNum;
                break;
            }

        }
        GetNodeFromID(currentNode.parentNodeNum, seekerNo, currentNode);
    }

    //ReversePath(seekerNo);

    int tmpList[_TOTAL_NODE_NUM];

    [allow_uav_condition]
    for (int m = 0; m < _TOTAL_NODE_NUM; m++)
    {
        tmpList[m] = nodeNumList[m];
    }

    [allow_uav_condition]
    for (int n = 0; n < _TOTAL_NODE_NUM; n++)
    {
        nodeNumList[n] = -99;
    }

    int r = 0;

    [allow_uav_condition]
    for (int s = _TOTAL_NODE_NUM - 1; s >= 0; s--)
    {
        if (tmpList[s] < 0)
        {
            //continue;
        }
        else
        {
            nodeNumList[r] = tmpList[s];
            r++;
        }
    }
    UpdatePathData(nodeNumList, seekerNo);

    return 0;
}
//


int FindPath(int startPosID, int targetPosID, uint seekerNo)
{
    ResetParentInfoForThisSeeker(seekerNo);
    NODE startNode;
    NODE targetNode;
    GetNodeFromID(startPosID, seekerNo, startNode);
    GetNodeFromID(targetPosID, seekerNo, targetNode);

    NODE openSet[_TOTAL_NODE_NUM];
    NODE closedSet[_TOTAL_NODE_NUM];

    [allow_uav_condition]
    for (int i = 0; i < _TOTAL_NODE_NUM; i++)
    {
        InitNodeFull(openSet[i]);
        InitNodeFull(closedSet[i]);
    }
    openSet[0] = startNode;

    uint counter = 0;
    [allow_uav_condition]
    while (CountOnSet(openSet) > 0)
    {
        NODE currentNode;
        GetFirstElement(openSet, currentNode);

        if (counter > 0)
        {
            GetLowestCostNode(openSet, closedSet, currentNode);
        }

        RemoveFromSet(openSet, currentNode);
        AddToSet(closedSet, currentNode);

        if (IsEqual(currentNode, targetNode) == true)
        {
            RetracePath(startNode, targetNode, seekerNo);
            return 0;
        }


        NODE neighbors[8];

        UpdateNeighbours(neighbors, currentNode, seekerNo);

        [allow_uav_condition]
        for (int n = 0; n < 8; n++)
        {
            if (neighbors[n].nodeNum < 0)
            {
                continue;
            }
            if (neighbors[n].walkable == 0 || ContainsOnSet(closedSet, neighbors[n]) == true)
            {
                continue;
            }

            uint newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbors[n]);
            if (newMovementCostToNeighbour < neighbors[n].gCost || ContainsOnSet(openSet, neighbors[n]) == false)
            {
                UpdateCostData(currentNode, targetNode, neighbors[n], seekerNo, newMovementCostToNeighbour);

                if (ContainsOnSet(openSet, neighbors[n]) == false)
                {
                    AddToSet(openSet, neighbors[n]);
                }
            }
        }

        counter++;
    }

    return 0;
}


[numthreads(8,8,1)]
void CSMain (uint3 id : SV_DispatchThreadID)
{
    // TODO: insert actual code here!

    //Result[id.xy] = float4(id.x & id.y, (id.x & 15)/15.0, (id.y & 15)/15.0, 0.0);

    int sd = FindPath(node_curDest_Buffer[id.x].currentNodeNum, node_curDest_Buffer[id.x].targetNodeNum, id.x);
}