Pathfinding.GridGraph.GetNearestForce C# (CSharp) Method

        public override NNInfo GetNearestForce(Vector3 position, NNConstraint constraint)
        {
            if (nodes == null || depth*width != nodes.Length) {
                return new NNInfo ();
            }

            Vector3 globalPosition = position;

            position = inverseMatrix.MultiplyPoint3x4 (position);

            int x = Mathf.Clamp (Mathf.RoundToInt (position.x-0.5F)  , 0, width-1);
            int z = Mathf.Clamp (Mathf.RoundToInt (position.z-0.5F)  , 0, depth-1);

            Node node = nodes[x+z*width];

            Node minNode = null;
            float minDist = float.PositiveInfinity;
            int overlap = getNearestForceOverlap;

            if (constraint.Suitable (node)) {
                minNode = node;
                minDist = ((Vector3)minNode.position-globalPosition).sqrMagnitude;
            }

            if (minNode != null) {
                if (overlap == 0) return new NNInfo(minNode);
                else overlap--;
            }

            //int counter = 0;

            float maxDist = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistance : float.PositiveInfinity;
            float maxDistSqr = maxDist*maxDist;

            //for (int w = 1; w < getNearestForceLimit;w++) {
            for (int w = 1;;w++) {
                int nx = x;
                int nz = z+w;

                int nz2 = nz*width;

                //Check if the nodes are within distance limit
                if (nodeSize*w > maxDist) {
                    return new NNInfo(minNode);
                }

                bool anyInside = false;

                for (nx = x-w;nx <= x+w;nx++) {
                    if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
                    anyInside = true;
                    if (constraint.Suitable (nodes[nx+nz2])) {
                        float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
                        //Debug.DrawRay (nodes[nx+nz2].position,Vector3.up*dist,Color.cyan);counter++;
                        if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = nodes[nx+nz2]; }
                    }
                }

                nz = z-w;
                nz2 = nz*width;

                for (nx = x-w;nx <= x+w;nx++) {
                    if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
                    anyInside = true;
                    if (constraint.Suitable (nodes[nx+nz2])) {
                        float dist = ((Vector3)nodes[nx+nz2].position-globalPosition).sqrMagnitude;
                        //Debug.DrawRay (nodes[nx+nz2].position,Vector3.up*dist,Color.cyan);counter++;
                        if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = nodes[nx+nz2]; }
                    }
                }

                nx = x-w;
                nz = z-w+1;

                for (nz = z-w+1;nz <= z+w-1; nz++) {
                    if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
                    anyInside = true;
                    if (constraint.Suitable (nodes[nx+nz*width])) {
                        float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
                        //Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
                        if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = nodes[nx+nz*width]; }
                    }
                }

                nx = x+w;

                for (nz = z-w+1;nz <= z+w-1; nz++) {
                    if (nx < 0 || nz < 0 || nx >= width || nz >= depth) continue;
                    anyInside = true;
                    if (constraint.Suitable (nodes[nx+nz*width])) {
                        float dist = ((Vector3)nodes[nx+nz*width].position-globalPosition).sqrMagnitude;
                        //Debug.DrawRay (nodes[nx+nz*width].position,Vector3.up*dist,Color.cyan);counter++;
                        if (dist < minDist && dist < maxDistSqr) { minDist = dist; minNode = nodes[nx+nz*width]; }
                    }
                }

                if (minNode != null) {
                    if (overlap == 0) return new NNInfo(minNode);
                    else overlap--;
                }

                //No nodes were inside grid bounds
                if (!anyInside) {
                    return new NNInfo(minNode);
                }
            }
            //return new NNInfo ();
        }