public static VoronoiFieldCell[,] GenerateField(Vector3[,] cellPositions, bool[,] isObstacle)
{
int gridSize = isObstacle.GetLength(0);
//Init the data structure
VoronoiFieldCell[,] voronoiField = new VoronoiFieldCell[gridSize, gridSize];
for (int x = 0; x < gridSize; x++)
{
for (int z = 0; z < gridSize; z++)
{
voronoiField[x, z].worldPos = cellPositions[x, z];
voronoiField[x, z].isObstacle = isObstacle[x, z];
}
}
//Step 1. Find out which occupied cell belongs to one or more obstacle areas by using a flood-fill algorithm
//cell[5, 7] belongs to area 2, which is occupied by one or more obstacles connected.
FindObstacleRegions(voronoiField);
//Step 2. Run a flow-field algorithm to determine the closest distance to an obstacle
//and which cell with obstacle in it is the closest (may be multiple cells)
FindVoronoiRegions(voronoiField);
//Step 3. Find which cells are voronoi edges by searching through all cells, and if one of the surrounding cells belongs to
//another obstacle then the cell is on the edge, so the voronoi edge is not one-cell thick
FindVoronoiEdges(voronoiField);
//Step 4. Find the distance from each cell to the nearest voronoi edge, which can be done with a flow field from each edge
//Will also find which voronoi edge cell is the closest (may be multiple cells)
FindDistanceFromEdgeToCell(voronoiField);
//Step 5. Create the voronoi field which tells the traversal cost at each cell
for (int x = 0; x < gridSize; x++)
{
for (int z = 0; z < gridSize; z++)
{
//[0, 1] Is highest close to obstacles, and lowest close to voronoi edges
float rho = 0f;
if (voronoiField[x, z].isObstacle)
{
rho = 1f;
}
else if (voronoiField[x, z].isVoronoiEdge)
{
rho = 0f;
}
else
{
//The distance from the cell to the nearest obstacle
float d_o = voronoiField[x, z].distanceToClosestObstacle;
//The distance from the cell to the nearest oronoi edge
float d_v = voronoiField[x, z].distanceToClosestEdge;
//The falloff rate
float alpha = Parameters.voronoi_alpha;
//The maximum effective range of the field
//If d_0 >= d_o_max, the field is 0
float d_o_max = Parameters.d_o_max;
rho = (alpha / (alpha + d_o)) * (d_v / (d_o + d_v)) * (((d_o - d_o_max) * (d_o - d_o_max)) / (d_o_max * d_o_max));
}
voronoiField[x, z].voronoiFieldValue = rho;
}
}
//Find the max and min values for debug
//float min = float.MaxValue;
//float max = float.MinValue;
//for (int x = 0; x < gridSize; x++)
//{
// for (int z = 0; z < gridSize; z++)
// {
// float value = voronoiField[x, z].voronoiFieldValue;
// if (value < min)
// {
// min = value;
// }
// if (value > max)
// {
// max = value;
// }
// }
//}
//Debug.Log("Voronoi field max: " + max + " min: " + min);
return(voronoiField);
}
//Smooth a path with batch gradient descent (x = x - gamma * grad(x))
//https://www.youtube.com/watch?v=umAeJ7LMCfU
public static void GradientDescent(
List <Vector3> path,
List <bool> isNodeFixed,
List <Obstacle> obstacles,
Map map,
bool isCircular,
float alpha, float beta, float gamma, float delta,
bool isDebugOn)
{
//Using map.VoronoiField is slow in each iteration, so should cache
VoronoiFieldCell[,] voronoiField = null;
if (map != null && map.VoronoiField != null)
{
voronoiField = map.VoronoiField;
}
//The list with smooth coordinates
List <Vector3> smoothPath = new List <Vector3>();
//Add the old positions
for (int i = 0; i < path.Count; i++)
{
smoothPath.Add(path[i]);
}
//Stop smoothing when all points have moved a distance less than this distance
//If 0.00001 we need more than 1000 iterations
float tolerance = 0.001f;
//How far has all points together moved this iteration?
//We are comparing the distance sqr instead of magnitude which is faster
float totalChangeSqr = tolerance * tolerance;
//So we dont end up in an infinite loop, is generally < 100
int iterations = 0;
//If we find a nan value its super slow to print that we did each iteration, so we do it once in the end
bool hasFoundNan = false;
while (totalChangeSqr >= tolerance * tolerance)
{
if (iterations > 1000)
{
break;
}
iterations += 1;
totalChangeSqr = 0f;
//We are using surrounding values, so we need an array where we add values found during the iteration
List <Vector3> newValuesThisIteration = new List <Vector3>(smoothPath.Count);
for (int i = 0; i < path.Count; i++)
{
//Dont move nodes that are fixed
if (isNodeFixed[i])
{
newValuesThisIteration.Add(smoothPath[i]);
continue;
}
//Clamp when we reach end and beginning of list
//The first and last node should be set to fixed if the path is not set to circular
int i_plus_one = HelpStuff.ClampListIndex(i + 1, path.Count);
int i_minus_one = HelpStuff.ClampListIndex(i - 1, path.Count);
//Smooth!
//1. Minimize the distance between the smooth path and the original path
Vector3 newSmoothPos = smoothPath[i] + alpha * (path[i] - smoothPath[i]);
//2. Minimize the distance between this position and the surrounding positions
newSmoothPos += beta * (smoothPath[i_plus_one] + smoothPath[i_minus_one] - 2f * smoothPath[i]);
//3. Maximize the distance to the closest obstacle
//Is sometimes unstable because we use the closest obstacle, so is bouncing back and forth
//until the loop stops because of infinite check
//if (obstacles != null)
//{
// Vector3 closestObstaclePos = FindClosestObstaclePos(smoothPath[i], obstacles);
// Vector3 dirToObstacle = closestObstaclePos - smoothPath[i];
// //Ignore obstacles far away
// float maxDist = 10f;
// if (dirToObstacle.sqrMagnitude < maxDist * maxDist)
// {
// float distanceToObstacle = dirToObstacle.magnitude;
// //To make obstacles closer more important
// float scaler = 1f - (distanceToObstacle / maxDist);
// newSmoothPos -= gamma * dirToObstacle.normalized * scaler;
// }
//}
//We can also use the voronoi field to find the closest obstacle
if (voronoiField != null && gamma > 0f)
{
Vector3 nodePos = smoothPath[i];
//Get the data for this cell
IntVector2 cellPos = map.ConvertWorldToCell(nodePos);
//The data for this cell
VoronoiFieldCell v = voronoiField[cellPos.x, cellPos.z];
Vector3 closestObstaclePos = v.ClosestObstaclePos(nodePos, voronoiField);
if (closestObstaclePos.x != -1f)
{
Vector3 dirToObstacle = closestObstaclePos - nodePos;
//Ignore obstacles far away
float maxDist = 10f;
if (dirToObstacle.sqrMagnitude < maxDist * maxDist)
{
float distanceToObstacle = dirToObstacle.magnitude;
//To make obstacles closer more important
float scaler = 1f - (distanceToObstacle / maxDist);
newSmoothPos -= gamma * dirToObstacle.normalized * scaler;
}
}
}
//4. Use the Voronoi field to push vertices away from obstacles
//We need to find the derivative of the voronoi field function with respect to:
//- distance to obstacle edge - should be maximized
//- distance to voronoi edge - should be minimized
//...to optimize the distance to both edges
//This is kinda slow and useless since we are already pushing away the path from obstacles above?
if (voronoiField != null && delta > 0f)
{
Vector3 nodePos = smoothPath[i];
//Get the data for this cell
IntVector2 cellPos = map.ConvertWorldToCell(nodePos);
//The data for this cell
VoronoiFieldCell v = voronoiField[cellPos.x, cellPos.z];
//Same each iteration
float d_max = v.d_obs_max;
float a = v.alpha;
Vector3 closestObstaclePos = v.ClosestObstaclePos(nodePos, voronoiField);
Vector3 closestEdgePos = v.ClosestEdgePos(nodePos, voronoiField);
//If we are inside an obstacle when debugging, we wont have a closest
if (closestObstaclePos.x != -1f && closestEdgePos.x != -1f)
{
//The directions
Vector3 dirToObstacle = closestObstaclePos - nodePos;
Vector3 dirToEdge = closestEdgePos - nodePos;
//The distances
float d_obs = dirToObstacle.magnitude;
float d_edg = dirToEdge.magnitude;
//The distance to voronoi edge
float upper_edge = a * d_obs * (d_obs - d_max) * (d_obs - d_max);
float lower_edge = d_max * d_max * (d_obs + a) * (d_edg + d_obs) * (d_edg + d_obs);
newSmoothPos -= delta * (upper_edge / lower_edge) * (-dirToEdge / d_edg);
//The distance to voronoi obstacle
float upper_obs = a * d_edg * (d_obs - d_max) * ((d_edg + 2f * d_max + a) * d_obs + (d_max + 2f * a) + a * d_max);
float lower_obs = d_max * d_max * (d_obs + a) * (d_obs + a) * (d_obs + d_edg) * (d_obs + d_edg);
newSmoothPos += delta * (upper_obs / lower_obs) * (dirToObstacle / d_obs);
}
}
//Sometimes the algorithm is unstable and shoots the vertices far away
//Maybe better to check for Nan in each part, so if the pos is NaN after optimizing to obstacle, dont add it???
if (float.IsNaN(newSmoothPos.x) || float.IsNaN(newSmoothPos.x) || float.IsNaN(newSmoothPos.x))
{
newSmoothPos = smoothPath[i];
hasFoundNan = true;
}
//Check if the new pos is within the map and if it is a value
if (map != null && !map.IsPosWithinGrid(newSmoothPos))
{
newSmoothPos = smoothPath[i];
}
//How far did we move the position this update?
totalChangeSqr += (newSmoothPos - smoothPath[i]).sqrMagnitude;
newValuesThisIteration.Add(newSmoothPos);
}
//Add the new values we created this iteration
for (int i = 0; i < smoothPath.Count; i++)
{
smoothPath[i] = newValuesThisIteration[i];
}
}
if (isDebugOn)
{
string debugText = DisplayController.GetDisplayText("Smooth path iterations", iterations, null);
Debug.Log(debugText);
}
if (hasFoundNan)
{
Debug.Log("Found Nan value");
}
//Add the new smooth positions to the original waypoints
for (int i = 0; i < path.Count; i++)
{
path[i] = smoothPath[i];
}
}
