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]; } }