//Calculate the shortest path with obstacles from each cell to the target cell we want to reach
        //Is called Dynamic Programming in "Programming self-driving car" but is the same as a flow map
        //Is called holonomic-with-obstacles in the reports
        public static void DynamicProgramming(Map map, IntVector2 targetPos)
        {
            int mapWidth = map.MapWidth;


            //Debug.DrawLine(map.cellData[targetPos.x, targetPos.z].centerPos, Vector3.zero, Color.red, 15f);


            //The final flow field will be stored here, so init it
            FlowFieldNode[,] nodesArray = new FlowFieldNode[mapWidth, mapWidth];

            for (int x = 0; x < mapWidth; x++)
            {
                for (int z = 0; z < mapWidth; z++)
                {
                    bool isWalkable = map.cellData[x, z].isObstacleInCell ? false : true;

                    FlowFieldNode node = new FlowFieldNode(isWalkable, map.cellData[x, z].centerPos, new IntVector2(x, z));

                    nodesArray[x, z] = node;
                }
            }

            //A flow field can have several start nodes, but in this case we have just one, which is the cell we want to reach
            List <FlowFieldNode> startNodes = new List <FlowFieldNode>();

            startNodes.Add(nodesArray[targetPos.x, targetPos.z]);


            //Generate the flow field
            FlowField.Generate(startNodes, nodesArray, includeCorners: true);


            //Save the values
            flowFieldHeuristics = new float[mapWidth, mapWidth];

            for (int x = 0; x < mapWidth; x++)
            {
                for (int z = 0; z < mapWidth; z++)
                {
                    //Save the flow field because we will use it to display it on a texture
                    map.cellData[x, z].distanceToTarget = nodesArray[x, z].totalCostFlowField;

                    //This heuristics has to be discounted by a value to be admissible to never overestimate the actual cost
                    flowFieldHeuristics[x, z] = nodesArray[x, z].totalCostFlowField * 0.92621f;
                }
            }


            //Debug.Log("Distance flow field: " + nodesArray[targetPos.x, targetPos.z].totalCostFlowField);
            //Debug.Log("Distance flow field: " + nodesArray[0, 0].totalCostFlowField);
        }
Esempio n. 2
0
        //Generate the flow field showing distance to closest obstacle from each cell
        private void GenerateObstacleFlowField(Map map, bool check8Cells)
        {
            int mapWidth = map.MapWidth;

            //The flow field will be stored in this array
            FlowFieldNode[,] flowField = new FlowFieldNode[mapWidth, mapWidth];

            //Init
            Cell[,] cellData = map.cellData;

            for (int x = 0; x < mapWidth; x++)
            {
                for (int z = 0; z < mapWidth; z++)
                {
                    //All nodes are walkable because we are generating the flow from each obstacle
                    bool isWalkable = true;

                    FlowFieldNode node = new FlowFieldNode(isWalkable, cellData[x, z].centerPos, new IntVector2(x, z));

                    flowField[x, z] = node;
                }
            }

            //A flow field can have several start nodes, which are the obstacles in this case
            List <FlowFieldNode> startNodes = new List <FlowFieldNode>();

            for (int x = 0; x < mapWidth; x++)
            {
                for (int z = 0; z < mapWidth; z++)
                {
                    //If this is an obstacle
                    if (cellData[x, z].isObstacleInCell)
                    {
                        startNodes.Add(flowField[x, z]);
                    }
                }
            }

            //Generate the flow field
            FlowField.Generate(startNodes, flowField, check8Cells);


            //Add the values to the celldata that belongs to the map
            for (int x = 0; x < mapWidth; x++)
            {
                for (int z = 0; z < mapWidth; z++)
                {
                    cellData[x, z].distanceToClosestObstacle = flowField[x, z].totalCostFlowField;
                }
            }
        }
        //
        // Find the distance from each cell to the nearest voronoi edge
        //

        //This can be done with a flow field from each edge
        private static void FindDistanceFromEdgeToCell(VoronoiFieldCell[,] voronoiField)
        {
            int mapWidth = voronoiField.GetLength(0);

            //The flow field will be stored in this array
            FlowFieldNode[,] flowField = new FlowFieldNode[mapWidth, mapWidth];

            //Init
            for (int x = 0; x < mapWidth; x++)
            {
                for (int z = 0; z < mapWidth; z++)
                {
                    //All nodes are walkable because we are generating the flow from each obstacle
                    bool isWalkable = voronoiField[x, z].isObstacle ? false : true;

                    FlowFieldNode node = new FlowFieldNode(isWalkable, voronoiField[x, z].worldPos, new IntVector2(x, z));

                    //node.region = voronoiField[x, z].region;

                    flowField[x, z] = node;
                }
            }

            //A flow field can have several start nodes, which are the obstacles in this case
            List <FlowFieldNode> startNodes = new List <FlowFieldNode>();

            for (int x = 0; x < mapWidth; x++)
            {
                for (int z = 0; z < mapWidth; z++)
                {
                    //If this is an edge
                    if (voronoiField[x, z].isVoronoiEdge)
                    {
                        startNodes.Add(flowField[x, z]);
                    }
                }
            }


            //Generate the flow field
            FlowField.Generate(startNodes, flowField, includeCorners: true);


            //Add the values to the celldata that belongs to the map
            for (int x = 0; x < mapWidth; x++)
            {
                for (int z = 0; z < mapWidth; z++)
                {
                    voronoiField[x, z].distanceToClosestEdge = flowField[x, z].totalCostFlowField;

                    voronoiField[x, z].closestEdgeCells = flowField[x, z].closestStartNodes;

                    //Now we can calculate the euclidean distance to the closest obstacle, which is more accurate then the flow field distance
                    HashSet <IntVector2> closest = voronoiField[x, z].closestEdgeCells;

                    if (closest != null && closest.Count > 0)
                    {
                        foreach (IntVector2 c in closest)
                        {
                            float distance = (voronoiField[c.x, c.z].worldPos - voronoiField[x, z].worldPos).magnitude;

                            voronoiField[x, z].distanceToClosestEdge = distance;

                            //If we have multiple cells that are equally close, we only need to test one of them
                            break;
                        }
                    }
                }
            }
        }