/** Will calculate a number of points around \a p which are on the graph and are separated by \a clearance from each other.
         * This is like GetPointsAroundPoint except that \a previousPoints are treated as being in world space.
         * The average of the points will be found and then that will be treated as the group center.
         *
         * \param p The point to generate points around
         * \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 are in world space.
         *      The new points will overwrite the existing points in the list. The result will be in world space.
         * \param radius The final points will be at most this distance from \a p.
         * \param clearanceRadius The points will if possible be at least this distance from each other.
         */
        public static void GetPointsAroundPointWorld (Vector3 p, IRaycastableGraph g, List<Vector3> previousPoints, float radius, float clearanceRadius) {
            if (previousPoints.Count == 0) return;

            Vector3 avg = Vector3.zero;
            for (int i = 0; i < previousPoints.Count; i++) avg += previousPoints[i];
            avg /= previousPoints.Count;

            for (int i = 0; i < previousPoints.Count; i++) previousPoints[i] -= avg;

            GetPointsAroundPoint(p, g, previousPoints, radius, clearanceRadius);
        }

        /** 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 p The point to generate points around
         * \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.
         *      The new points will overwrite the existing points in the list. The result will be in world space, not relative to \a p.
         * \param radius The final points will be at most this distance from \a p.
         * \param clearanceRadius The points will if possible be at least this distance from each other.
         *
         * \todo Write unit tests
         */
        public static void GetPointsAroundPoint (Vector3 p, IRaycastableGraph g, List<Vector3> previousPoints, float radius, float clearanceRadius) {
            if (g == null) throw new System.ArgumentNullException("g");

            var graph = g as NavGraph;

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

            NNInfoInternal 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;

                GraphHitInfo hit;

                int tests = 0;
                while (true) {
                    Vector3 pt = p + dir;

                    if (g.Linecast(p, pt, nn.node, out hit)) {
                        if (hit.point == Vector3.zero) {
                            // Oops, linecast actually failed completely
                            // try again unless we have tried lots of times
                            // then we just continue anyway
                            tests++;
                            if (tests > 8) {
                                previousPoints[i] = pt;
                                break;
                            }
                        } else {
                            pt = hit.point;
                        }
                    }

                    bool worked = false;

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

                        // Abort after 8 tests or when we have found a valid point
                        if (worked || tests > 8) {
                            worked = true;
                            previousPoints[i] = qt;
                            break;
                        }
                    }

                    // Break out of nested loop
                    if (worked) {
                        break;
                    }

                    // If we could not find a valid point, reduce the clearance radius slightly to improve
                    // the chances next time
                    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++;
                }
            }
        }