//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);
}
//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;
}
}
}
}
}
