Beispiel #1
0
        //Find the neighboring nodes to a node by checking all nodes around it
        private static HashSet <FlowFieldNode> FindNeighboringNodes(FlowFieldNode node, FlowFieldNode[,] nodeArray, int mapWidth, bool includeCorners)
        {
            HashSet <IntVector2> neighborCells = new HashSet <IntVector2>();

            //Get the directions we can move in, which are up, left, right, down
            IntVector2[] delta = HelpStuff.delta;

            if (includeCorners)
            {
                delta = HelpStuff.deltaWithCorners;
            }


            //Will track if at least one neighbor is an obstacle, which may be useful to know later
            bool isNeighborObstacle = false;

            for (int i = 0; i < delta.Length; i++)
            {
                IntVector2 cellPos = new IntVector2(node.cellPos.x + delta[i].x, node.cellPos.z + delta[i].z);

                //Is this cell position within the grid?
                if (IsCellPosWithinGrid(cellPos, mapWidth))
                {
                    //Is not a valid neighbor if its obstacle
                    if (!nodeArray[cellPos.x, cellPos.z].isWalkable)
                    {
                        isNeighborObstacle = true;
                    }
                    else
                    {
                        neighborCells.Add(cellPos);
                    }
                }
            }


            //If we are checking 8 neighbors we have to be careful to not jump diagonally if one cell next to the diagonal is obstacle
            //This is a costly operation (0.3 seconds if we do it for all cells) so only do it if at least one neighbor is obstacle
            if (includeCorners && isNeighborObstacle)
            {
                HashSet <IntVector2> corners = new HashSet <IntVector2>(HelpStuff.deltaJustCorners);

                //Loop through all 8 neighbors
                for (int i = 0; i < delta.Length; i++)
                {
                    //Is this neighbor a corner?
                    if (corners.Contains(delta[i]))
                    {
                        IntVector2 cellPos = new IntVector2(node.cellPos.x + delta[i].x, node.cellPos.z + delta[i].z);

                        //Have we added the corner to the list of neighbors
                        if (!neighborCells.Contains(cellPos))
                        {
                            continue;
                        }

                        //Check if neighbors to the corner are obstacles, if so we cant move to this corner
                        IntVector2 n1 = delta[HelpStuff.ClampListIndex(i + 1, delta.Length)];
                        IntVector2 n2 = delta[HelpStuff.ClampListIndex(i - 1, delta.Length)];

                        IntVector2 cellPos_n1 = new IntVector2(node.cellPos.x + n1.x, node.cellPos.z + n1.z);
                        IntVector2 cellPos_n2 = new IntVector2(node.cellPos.x + n2.x, node.cellPos.z + n2.z);

                        if (!nodeArray[cellPos_n1.x, cellPos_n1.z].isWalkable || !nodeArray[cellPos_n2.x, cellPos_n2.z].isWalkable)
                        {
                            //This is not a valid neighbor so remove it from neighbors
                            neighborCells.Remove(cellPos);
                        }
                    }
                }
            }



            //From cell to node
            HashSet <FlowFieldNode> neighborNodes = new HashSet <FlowFieldNode>();

            foreach (IntVector2 cell in neighborCells)
            {
                neighborNodes.Add(nodeArray[cell.x, cell.z]);
            }

            return(neighborNodes);
        }
Beispiel #2
0
        //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];
            }
        }