Pathfinding.NavGraph.GetNearestForce C# (CSharp) Method

		public virtual NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
			return GetNearest (position, constraint);
			//Debug.LogError ("This should not be called if not GetNearest has been overriden, and if GetNearest has been overriden, you should override this function too, always return a node which returns true when passed to constraint.Suitable (node)");
			//return new NNInfo ();
		}
		

        /** Will calculate a number of points around \a p which are on the graph and are separated by \a clearance from each other.
         * The maximum distance from \a p to any point will be \a radius.
         * Points will first be tried to be laid out as \a previousPoints and if that fails, random points will be selected.
         * This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position
         * this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using
         * some kind of local avoidance.
         *
         * \param g The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph.
         * Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best.
         * \param previousPoints The points to use for reference. Note that these should not be in world space. They are treated as relative to \a p.
         */
        public static void GetPointsAroundPoint(Vector3 p, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
        {
            if (g == null)
            {
                throw new System.ArgumentNullException("g");
            }

            NavGraph graph = g as NavGraph;

            if (graph == null)
            {
                throw new System.ArgumentException("g is not a NavGraph");
            }

            NNInfo nn = graph.GetNearestForce(p, NNConstraint.Default);

            p = nn.clampedPosition;

            if (nn.node == null)
            {
                // No valid point to start from
                return;
            }


            // Make sure the enclosing circle has a radius which can pack circles with packing density 0.5
            radius           = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt(previousPoints.Count));//Mathf.Sqrt(previousPoints.Count*clearanceRadius*2));
            clearanceRadius *= clearanceRadius;

            for (int i = 0; i < previousPoints.Count; i++)
            {
                Vector3 dir  = previousPoints[i];
                float   magn = dir.magnitude;

                if (magn > 0)
                {
                    dir /= magn;
                }

                float newMagn = radius;                //magn > radius ? radius : magn;
                dir *= newMagn;

                bool worked = false;

                GraphHitInfo hit;

                int tests = 0;
                do
                {
                    Vector3 pt = p + dir;

                    if (g.Linecast(p, pt, nn.node, out hit))
                    {
                        pt = hit.point;
                    }

                    for (float q = 0.1f; q <= 1.0f; q += 0.05f)
                    {
                        Vector3 qt = (pt - p) * q + p;
                        worked = true;
                        for (int j = 0; j < i; j++)
                        {
                            if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius)
                            {
                                worked = false;
                                break;
                            }
                        }

                        if (worked)
                        {
                            previousPoints[i] = qt;
                            break;
                        }
                    }

                    if (!worked)
                    {
                        // Abort after 8 tries
                        if (tests > 8)
                        {
                            worked = true;
                        }
                        else
                        {
                            clearanceRadius *= 0.9f;
                            // This will pick points in 2D closer to the edge of the circle with a higher probability
                            dir   = Random.onUnitSphere * Mathf.Lerp(newMagn, radius, tests / 5);
                            dir.y = 0;
                            tests++;
                        }
                    }
                } while (!worked);
            }
        }